Dynamixel manager

src/joint_motion.c

@@ -220,6 +220,7 @@ void joint_motion_process(void)
               moving=0x01;
               if(joint_current_speeds[i]!=joint_dir[i]*joint_target_speeds[i])// it is necessary to change the current speed
               {
+                joint_current_angles[i]+=((joint_current_speeds[i]*joint_motion_period)>>17);
                 if(joint_current_speeds[i]>joint_dir[i]*joint_target_speeds[i])
                 {
                   joint_current_speeds[i]-=((joint_target_accels[i]*joint_motion_period)>>16);// reduce speed
@@ -232,7 +233,7 @@ void joint_motion_process(void)
                   if(joint_current_speeds[i]>joint_dir[i]*joint_target_speeds[i])
                     joint_current_speeds[i]=joint_dir[i]*joint_target_speeds[i];
                 }
-                joint_current_angles[i]+=((joint_current_speeds[i]*joint_motion_period)>>16);
+                joint_current_angles[i]+=((joint_current_speeds[i]*joint_motion_period)>>17);
                 if(joint_dir[i]==1 && joint_current_angles[i]>joint_target_angles[i])
                   joint_current_angles[i]=joint_target_angles[i];
                 if(joint_dir[i]==-1 && joint_current_angles[i]<joint_target_angles[i])
@@ -250,9 +251,12 @@ void joint_motion_process(void)
                 }
                 else// deceleration phase
                 {
+                  joint_current_angles[i]=joint_current_angles[i]+((joint_current_speeds[i]*joint_motion_period)>>17);
                   joint_target_speeds[i]=joint_target_speeds[i]-((joint_target_accels[i]*joint_motion_period)>>16);
+                  if(joint_target_speeds[i]<0)
+                    joint_target_speeds[i]=0;
                   joint_current_speeds[i]=joint_dir[i]*joint_target_speeds[i];
-                  joint_current_angles[i]=joint_current_angles[i]+((joint_current_speeds[i]*joint_motion_period)>>16);
+                  joint_current_angles[i]=joint_current_angles[i]+((joint_current_speeds[i]*joint_motion_period)>>17);
                   if(joint_dir[i]==1 && joint_current_angles[i]>joint_target_angles[i])
                     joint_current_angles[i]=joint_target_angles[i];
                   if(joint_dir[i]==-1 && joint_current_angles[i]<joint_target_angles[i])
@@ -261,7 +265,9 @@ void joint_motion_process(void)
               }
             }
             else
+            {
               joint_current_speeds[i]=0;
+            }
             manager_current_angles[i]=joint_current_angles[i];
             manager_current_slopes[i]=5;
           }