Set the stop signal to 0 when a new motion is started to avoid false cancelations.

Added a new variable to hold the angle required by the deacceleration phase with the initial command data, instead of computing it each cycle.
Changed the max force default value of the grippers.

include/eeprom_init.h

@@ -77,12 +77,12 @@ extern "C" {
 #define    DEFAULT_GRIPPER_LEFT_BOT_ID          0x0016 // ID 22 for the left gripper
 #define    DEFAULT_GRIPPER_LEFT_MAX_ANGLE       0x0F00 // 30 in fixed point format 9|7
 #define    DEFAULT_GRIPPER_LEFT_MIN_ANGLE       0xF100 // -30 in fixed point format 9|7
-#define    DEFAULT_GRIPPER_LEFT_MAX_FORCE       0x03FF // 1023 max force in binary format
+#define    DEFAULT_GRIPPER_LEFT_MAX_FORCE       0x0080 // 1023 max force in binary format
 #define    DEFAULT_GRIPPER_RIGHT_TOP_ID         0x0017 // ID 23 for the left gripper
 #define    DEFAULT_GRIPPER_RIGHT_BOT_ID         0x0018 // ID 24 for the left gripper
 #define    DEFAULT_GRIPPER_RIGHT_MAX_ANGLE      0x0F00 // 30 in fixed point format 9|7
 #define    DEFAULT_GRIPPER_RIGHT_MIN_ANGLE      0xF100 // -30 in fixed point format 9|7
-#define    DEFAULT_GRIPPER_RIGHT_MAX_FORCE      0x03FF // 1023 max force in binary format
+#define    DEFAULT_GRIPPER_RIGHT_MAX_FORCE      0x0080 // 1023 max force in binary format
 #define    DEFAULT_SMART_CHARGER_PERIOD         0x05DC //1500 ms 
 
   

src/joint_motion.c

@@ -6,6 +6,7 @@
 int64_t joint_target_angles[MANAGER_MAX_NUM_SERVOS];//48|16
 int64_t joint_target_speeds[MANAGER_MAX_NUM_SERVOS];//48|16
 int64_t joint_target_accels[MANAGER_MAX_NUM_SERVOS];//48|16
+int64_t joint_target_stop_angles[MANAGER_MAX_NUM_SERVOS];//48|16
 
 int64_t joint_current_angles[MANAGER_MAX_NUM_SERVOS];//48|16
 int64_t joint_current_speeds[MANAGER_MAX_NUM_SERVOS];//48|16
@@ -120,6 +121,8 @@ void joint_motion_start(uint8_t group_id)
             ram_data[DARWIN_JOINT_SERVO0_ACCEL+i]=joint_target_accels[i]>>16;
           }
         }
+        // stop angles
+        joint_target_stop_angles[i]=joint_target_speeds[i]*joint_target_speeds[i]/(2*joint_target_accels[i]);
         // the current angles, speeds and accelerations are in RAM
         if(joint_target_angles[i]>=joint_current_angles[i])
           joint_dir[i]=1;
@@ -129,6 +132,7 @@ void joint_motion_start(uint8_t group_id)
     }
   }
   joint_moving[group_id]=0x01;
+  joint_stop[group_id]=0x00;
   ram_data[DARWIN_JOINT_GRP0_CNTRL+group_id*5]|=JOINT_STATUS;
 }
 
@@ -236,7 +240,7 @@ void joint_motion_process(void)
               }  
               else
               {
-                if(abs(joint_target_angles[i]-joint_current_angles[i])>(joint_target_speeds[i]*joint_target_speeds[i]/(2*joint_target_accels[i])))// constant speed phase
+                if(abs(joint_target_angles[i]-joint_current_angles[i])>joint_target_stop_angles[i])// constant speed phase
                 {
                   joint_current_angles[i]+=((joint_current_speeds[i]*joint_motion_period)>>16);
                   if(joint_dir[i]==1 && joint_current_angles[i]>joint_target_angles[i])
@@ -256,6 +260,8 @@ void joint_motion_process(void)
                 }
               }
             }
+            else
+              joint_current_speeds[i]=0;
             manager_current_angles[i]=joint_current_angles[i];
             manager_current_slopes[i]=5;
           }