Added support to change the servos offsets in the firmware.

include/motion_manager.h

@@ -84,6 +84,7 @@ inline TModules manager_get_module(uint8_t servo_id);
 inline void manager_enable_servo(uint8_t servo_id);
 inline void manager_disable_servo(uint8_t servo_id);
 inline uint8_t manager_is_servo_enabled(uint8_t servo_id);
+void manager_set_offset(uint8_t servo_id,int8_t offset);
 inline int16_t manager_get_cw_angle_limit(uint8_t servo_id);
 inline int16_t manager_get_ccw_angle_limit(uint8_t servo_id);
 // motion modules handling

src/motion_manager.c

@@ -25,6 +25,8 @@ 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;
+// manager offsets
+int16_t manager_offset[MANAGER_MAX_NUM_SERVOS];
 
 // private functions
 void manager_send_motion_command(void)
@@ -99,7 +101,7 @@ void manager_get_target_position(void)
     {
       if(manager_servos[i].module==MM_ACTION)
       {
-        manager_servos[i].current_angle=((manager_current_angles[i]>>9)+manager_balance_offset[i]);
+        manager_servos[i].current_angle=((manager_current_angles[i]>>9)+manager_balance_offset[i]+manager_offset[i]);
         //>>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);
         ram_data[BIOLOID_MM_SERVO0_CUR_POS_L+i*2]=(manager_servos[i].current_angle&0xFF);
@@ -320,8 +322,12 @@ void manager_init(uint16_t period_us)
   manager_motion_period_us=period_us;
 
   /* initialize balance parameters */
+  /* initialize the manager offsets from EEPROM */
   for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
+  {
     manager_balance_offset[i]=0;
+    manager_set_offset(i,(signed char)ram_data[BIOLOID_MM_SERVO0_OFFSET+i]);
+  }
   manager_balance_enabled=0x00;
   gpio_set_led(TXD_LED);
 
@@ -454,6 +460,15 @@ inline uint8_t manager_is_servo_enabled(uint8_t servo_id)
     return 0x00;
 }
 
+void manager_set_offset(uint8_t servo_id, int8_t offset)
+{
+  if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS)
+  {
+    manager_offset[servo_id]=(int16_t)(offset*8);
+    ram_data[BIOLOID_MM_SERVO0_OFFSET+servo_id]=offset;
+  }
+}
+
 inline int16_t manager_get_cw_angle_limit(uint8_t servo_id)
 {
   return manager_servos[servo_id].cw_angle_limit;
@@ -468,8 +483,8 @@ inline int16_t manager_get_ccw_angle_limit(uint8_t servo_id)
 uint8_t manager_in_range(unsigned short int address, unsigned short int length)
 {
   if(ram_in_window(MANAGER_BASE_ADDRESS,MANAGER_MEM_LENGTH,address,length) ||
-     ram_in_window(BIOLOID_MM_PERIOD_L,BIOLOID_MM_BAL_HIP_ROLL_GAIN_H,address,length) ||
-     ram_in_window(BIOLOID_MM_SERVO0_OFFSET,BIOLOID_MM_SERVO31_OFFSET,address,length))
+     ram_in_window(BIOLOID_MM_PERIOD_L,10,address,length) ||
+     ram_in_window(BIOLOID_MM_SERVO0_OFFSET,MANAGER_MAX_NUM_SERVOS,address,length))
     return 0x01;
   else
     return 0x00;
@@ -485,6 +500,11 @@ void manager_process_write_cmd(unsigned short int address,unsigned short int len
     word_value=data[BIOLOID_MM_PERIOD_L-address]+(data[BIOLOID_MM_PERIOD_H-address]<<8);
     manager_set_period(word_value);
   }
+  for(i=BIOLOID_MM_SERVO0_OFFSET,j=0;i<=BIOLOID_MM_SERVO31_OFFSET;i++,j++)
+  {
+    if(ram_in_range(i,address,length))
+      manager_set_offset(j,(signed char)(data[i-address]));
+  }
   for(i=BIOLOID_MM_MODULE_EN0,j=0;i<=BIOLOID_MM_MODULE_EN15;i++,j+=2)
   {
     if(ram_in_range(i,address,length))