Added the slope control to the action execution.

include/motion_manager.h

@@ -52,6 +52,8 @@ typedef struct{
   uint16_t center_angle;
   uint16_t max_speed;
   int16_t current_angle;
+  uint8_t cw_comp;
+  uint8_t ccw_comp;
   uint16_t current_value;
   TModules module;
   uint8_t enabled;
@@ -64,6 +66,7 @@ typedef struct{
 
 // public variables
 extern int64_t manager_current_angles[MANAGER_MAX_NUM_SERVOS];
+extern int8_t manager_current_slopes[MANAGER_MAX_NUM_SERVOS];
 
 // public functions
 void manager_init(uint16_t period_us);

src/action.c

@@ -124,10 +124,8 @@ void action_load_next_step(void)
     }
   }
   // compute trajectory
-//  action_pause_time=((action_current_page.steps[action_current_step_index].pause<<4)*action_current_page.header.speed);
   action_pause_time=(action_current_page.steps[action_current_step_index].pause*action_current_page.header.speed)>>5;
   action_pause_time=action_pause_time<<9;
-//  action_step_time=((action_current_page.steps[action_current_step_index].time<<4)*action_current_page.header.speed);
   action_step_time=(action_current_page.steps[action_current_step_index].time*action_current_page.header.speed)>>5;
   action_step_time=action_step_time<<9;
   if(action_step_time<action_period)
@@ -178,6 +176,7 @@ void action_load_next_step(void)
         if(tmp_angle>max_angle)
           max_angle=tmp_angle;
       }
+      manager_current_slopes[i]=action_current_page.header.slope[i];
     }
   }
   if(action_current_page.header.schedule==SPEED_BASE_SCHEDULE)

src/motion_manager.c

@@ -20,6 +20,7 @@ uint8_t manager_num_servos;
 TServoInfo manager_servos[MANAGER_MAX_NUM_SERVOS];
 // current angles used for all motion modules
 int64_t manager_current_angles[MANAGER_MAX_NUM_SERVOS];
+int8_t manager_current_slopes[MANAGER_MAX_NUM_SERVOS];
 // balance values and configuration
 int64_t manager_balance_offset[MANAGER_MAX_NUM_SERVOS];
 uint8_t manager_balance_enabled;
@@ -36,12 +37,14 @@ void manager_send_motion_command(void)
     if(manager_servos[i].enabled && manager_servos[i].model!=0x0000)
     {
       servo_ids[num]=manager_servos[i].id;
-      write_data[num].data_addr=(uint8_t *)&(manager_servos[i].current_value);
+      manager_servos[i].cw_comp=(1<<manager_current_slopes[i]);
+      manager_servos[i].ccw_comp=(1<<manager_current_slopes[i]);
+      write_data[num].data_addr=(uint8_t *)&(manager_servos[i].cw_comp);
       num++;
     }
   }
   if(num>0)
-    dyn_master_sync_write(&bioloid_dyn_master_servos,num,servo_ids,P_GOAL_POSITION_L,2,write_data);
+    dyn_master_sync_write(&bioloid_dyn_master_servos,num,servo_ids,P_GOAL_POSITION_L,4,write_data);
 }
 
 inline uint16_t manager_angle_to_value(uint8_t servo_id,int16_t angle)