diff --git a/include/eeprom_init.h b/include/eeprom_init.h
index e7ae5eb6f83eb5c9810fc8ea54067e0a04101119..cb4543cfe461d97c1d856b49320e3b7af92296b1 100644
--- a/include/eeprom_init.h
+++ b/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 
 
   
diff --git a/src/joint_motion.c b/src/joint_motion.c
index 8e8147aa5f2568e9fbb920ffdccddc71433295b6..395bfc0989f7d682875c94cf5972e15b4bcf4956 100644
--- a/src/joint_motion.c
+++ b/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;
           }