If servos are missing, the actions now execute properly, assigning each servos the correct angle.

When the page 0 is loaded, a default value of 32 is loaded to the compliance slope.

src/action.c

@@ -88,7 +88,12 @@ void action_load_next_step(void)
     }
     pages_copy_page(&action_next_page,&action_current_page);// make the next page active
     for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
-      action_slope[i]=pages_get_slope(&action_current_page,i);
+    {
+      if(action_next_index==0)
+        action_slope[i]=32;
+      else
+        action_slope[i]=pages_get_slope(&action_current_page,i);
+    }
     if(current_index!=action_next_index)
       num_repetitions=action_current_page.header.repeat;
     action_current_step_index=0;

src/motion_manager.c

@@ -23,13 +23,13 @@ void manager_send_motion_command(void)
   uint8_t data[256];
   uint8_t i,num=0;
   
-  for(i=0;i<manager_num_servos;i++)
+  for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
   {
     if(manager_servos[i].enabled)
     {
       servo_ids[num]=manager_servos[i].id;
-      data[num*4]=action_slope[i+1];
-      data[num*4+1]=action_slope[i+1];
+      data[num*4]=action_slope[i];
+      data[num*4+1]=action_slope[i];
       data[num*4+2]=manager_servos[i].current_value%256;
       data[num*4+3]=manager_servos[i].current_value/256;
       num++;
@@ -65,7 +65,7 @@ void manager_get_current_position(void)
 {
   uint8_t i;
 
-  for(i=0;i<manager_num_servos;i++)
+  for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
   {
     if(!manager_servos[i].enabled)// servo is enabled
     {
@@ -81,13 +81,13 @@ void manager_get_target_position(void)
 {
   uint8_t i;
 
-  for(i=0;i<manager_num_servos;i++)
+  for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
   {
     if(manager_servos[i].enabled)// servo is enabled
     {
       if(manager_servos[i].module==MM_ACTION)
       {
-        manager_servos[i].current_angle=((action_current_angles[i+1])>>9);
+        manager_servos[i].current_angle=((action_current_angles[i])>>9);
         //>>16 from the action codification, <<7 from the manager codification
         manager_servos[i].current_value=manager_angle_to_value(i,manager_servos[i].current_angle);
       }
@@ -129,11 +129,11 @@ void MOTION_TIMER_IRQHandler(void)
     // get the target angles from all modules
     manager_get_target_position();
     // balance the robot
-    manager_balance();
+//    manager_balance();
     // send the motion commands to the servos
     manager_send_motion_command();
     // get the disabled servos position
-    manager_get_current_position();
+//    manager_get_current_position();
   }
 }
 
@@ -156,12 +156,13 @@ void manager_init(uint16_t period_us)
   // detect the servos connected 
   dyn_master_scan(&num,servo_ids); 
   ram_data[BIOLOID_MM_NUM_SERVOS]=num;
+  manager_num_servos=0;
   for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
   {
-    if(i<num)
+    if(i==servo_ids[manager_num_servos])
     {
       // read the model of the i-th device
-      dyn_master_read_word(servo_ids[i],P_MODEL_NUMBER_L,&model);
+      dyn_master_read_word(servo_ids[manager_num_servos],P_MODEL_NUMBER_L,&model);
       switch(model)
       {
         case SERVO_AX12A: manager_servos[i].encoder_resolution=1023;
@@ -226,7 +227,7 @@ void manager_init(uint16_t period_us)
                           break;
         default: break; 
       }
-      manager_servos[i].id=servo_ids[i];
+      manager_servos[i].id=servo_ids[manager_num_servos];
       manager_servos[i].model=model;
       manager_servos[i].module=MM_NONE;
       manager_servos[i].enabled=0x00;
@@ -236,12 +237,12 @@ void manager_init(uint16_t period_us)
       ram_data[BIOLOID_MM_SERVO0_CUR_POS_L+i*2]=(manager_servos[i].current_angle&0xFF);
       ram_data[BIOLOID_MM_SERVO0_CUR_POS_H+i*2]=(manager_servos[i].current_angle>>8);
       // set the action current angles
-      action_current_angles[i+1]=manager_servos[i].current_angle<<9;
+      action_current_angles[i]=manager_servos[i].current_angle<<9;
       manager_num_servos++;
     }
     else
     {
-      manager_servos[i].id=0;
+      manager_servos[i].id=i;
       manager_servos[i].model=0x0000;
       manager_servos[i].module=MM_NONE;
       manager_servos[i].encoder_resolution=0;
@@ -254,7 +255,7 @@ void manager_init(uint16_t period_us)
       manager_servos[i].enabled=0x00;
     }
   }
-  manager_num_servos=num; 
+  dyn_master_disable_power();
 
   // initialize the timer interrupts
   NVIC_InitStructure.NVIC_IRQChannel = MOTION_TIMER_IRQn;
@@ -348,7 +349,7 @@ void manager_disable_servos(void)
   uint8_t data[256];
   uint8_t i,num=0;
 
-  for(i=0;i<manager_num_servos;i++)
+  for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
   {
     if(!manager_servos[i].enabled)
     {