Changed the angle range and motion limits of the emmulated dynamixel servos to...

Changed the angle range and motion limits of the emmulated dynamixel servos to coincide with the real ones.

bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h

@@ -369,7 +369,7 @@ namespace bioloid_controller_cm510
       for (unsigned int i = 0; i < this->joints_.size(); ++i)
       {
         this->compliances_[i].slope=1<<(manager_servos[i+1].slope&0x0F);
-        target_angles[i] = ((((manager_servos[i+1].current_value+balance_offsets[i+1]-manager_servos[i+1].center_value)*300.0/1023.0)*3.14159)/180.0);
+        target_angles[i] = ((((manager_servos[i+1].current_value+balance_offsets[i+1]-manager_servos[i+1].center_value)*manager_servos[i+1].angle/1023.0)*3.14159)/180.0);
         const double command = this->compliance_control(this->compliances_[i],target_angles[i]-real_angles[i]);
         this->joints_[i].setCommand(command);
       }
@@ -542,6 +542,7 @@ namespace bioloid_controller_cm510
         else if(frame_name==this->exp_gpio18_frame)
           exp_gpio_values[18]=threshold;
       }
+
     }
 
   template <class HardwareInterface>

bioloid_controller_cm510/src/sim/dynamixel_master.c

@@ -179,11 +179,11 @@ uint8_t dyn_master_read_word(uint8_t id,uint16_t address,uint16_t *data)
     if(address==MODEL_NUM)
       *data=0x0C00;
     else if(address==CURRENT_POS)
-      *data=((((real_angles[id-1]*180.0)/3.143159)+150.0)*1023.0)/300.0;
+      *data=((((real_angles[id-1]*180.0)/3.143159)+90.0)*1023.0)/180.0;
     else if(address==CW_ANGLE_LIM)
-      *data=205;
+      *data=0;
     else if(address==CCW_ANGLE_LIM)
-      *data=818;
+      *data=1023;
     else
       *data=0x0000;
   }