Read the control parameters from the servo when initializing them.

Added a return value to the function to read servo registers.

dynamixel_manager/src/modules/motion_manager.c

@@ -4,7 +4,7 @@
 #include "ram.h"
 
 /* private functions */
-void mmanager_read_register(TMotionManager *mmanager,unsigned char id,TDynReg *reg,unsigned int *value)
+unsigned char mmanager_read_register(TMotionManager *mmanager,unsigned char id,TDynReg *reg,unsigned int *value)
 {
   unsigned char reg_value[4]={0};
 
@@ -17,7 +17,10 @@ void mmanager_read_register(TMotionManager *mmanager,unsigned char id,TDynReg *r
       (*value)=reg_value[0]+(reg_value[1]<<8);
     else if(reg->size==4)
       (*value)=reg_value[0]+(reg_value[1]<<8)+(reg_value[2]<<16)+(reg_value[3]<<24);
+    return 0x01;
   }
+  else
+    return 0x00;
 }
 
 void mmanager_write_register(TMotionManager *mmanager,unsigned char id,TDynReg *reg,unsigned int value)
@@ -97,7 +100,7 @@ void mmanager_compute_targets(TMotionManager *mmanager)
       angle=(mmanager->servo_values[i].target_angle>>9)+offsets[i]+(mmanager->servo_values[i].offset<<3);
       //>>16 from the action codification, <<7 from the manager codification
       mmanager->servo_values[i].target_value=mmanager_angle_to_value(mmanager,i,angle);
-      if(mmanager->servo_configs[i]->protocol_ver==1 || (mmanager->servo_configs[i]->model!=29 && mmanager->servo_configs[i]->model!=310 && mmanager->servo_configs[i]->model!=320 && mmanager->servo_configs[i]->model!=360))// get the target value as the feedback
+      if(mmanager->servo_configs[i]->protocol_ver==1 && mmanager->servo_configs[i]->model!=29 && mmanager->servo_configs[i]->model!=310 && mmanager->servo_configs[i]->model!=320 && mmanager->servo_configs[i]->model!=360)// get the target value as the feedback
       {
         mmanager->memory->data[mmanager->ram_base_address+MM_CURRENT_ANGLES_OFFSET+i*2]=angle&0x00FF;
         mmanager->memory->data[mmanager->ram_base_address+MM_CURRENT_ANGLES_OFFSET+i*2+1]=(angle>>8)&0x00FF;
@@ -139,14 +142,27 @@ void mmanager_add_device(TMotionManager *mmanager,unsigned char id,unsigned shor
         mmanager->servo_configs[id]=&servo_data[i];
 
     mmanager_read_register(mmanager,id,&mmanager->servo_configs[id]->registers[current_pos],&mmanager->servo_values[id].current_value);
-    mmanager_read_register(mmanager,id,&mmanager->servo_configs[id]->registers[min_angle_limit],&cw_value);
-    mmanager_read_register(mmanager,id,&mmanager->servo_configs[id]->registers[max_angle_limit],&ccw_value);
     mmanager->servo_values[id].target_value=mmanager->servo_values[id].current_value;
     mmanager->servo_values[id].current_angle=(mmanager_value_to_angle(mmanager,id,mmanager->servo_values[id].current_value)<<9);
     mmanager->servo_values[id].target_angle=mmanager->servo_values[id].current_angle;
     // read the servo limits
+    mmanager_read_register(mmanager,id,&mmanager->servo_configs[id]->registers[min_angle_limit],&cw_value);
     mmanager->servo_configs[id]->cw_angle_limit=(mmanager_value_to_angle(mmanager,id,cw_value)<<9);
+    mmanager_read_register(mmanager,id,&mmanager->servo_configs[id]->registers[max_angle_limit],&ccw_value);
     mmanager->servo_configs[id]->ccw_angle_limit=(mmanager_value_to_angle(mmanager,id,ccw_value)<<9);
+    // read the servo control values
+    if(mmanager->servo_configs[id]->pid)
+    {
+      mmanager_read_register(mmanager,id,&mmanager->servo_configs[id]->registers[pos_pid_p],&cw_value);
+      mmanager->servo_values[id].cw_compliance=cw_value;
+    }
+    else
+    {
+      mmanager_read_register(mmanager,id,&mmanager->servo_configs[id]->registers[cw_comp_slope],&cw_value);
+      mmanager->servo_values[id].cw_compliance=cw_value;
+      mmanager_read_register(mmanager,id,&mmanager->servo_configs[id]->registers[ccw_comp_slope],&ccw_value);
+      mmanager->servo_values[id].ccw_compliance=ccw_value;
+    }
     mmanager->present_servos|=(0x00000001<<id);
     mmanager->memory->data[mmanager->ram_base_address+MM_PRESENT_SERVOS_OFFSET]=mmanager->present_servos&0x000000FF;
     mmanager->memory->data[mmanager->ram_base_address+MM_PRESENT_SERVOS_OFFSET+1]=(mmanager->present_servos>>8)&0x000000FF;