diff --git a/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h b/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h index 27d1a06a44f545ac1e36d610d59376a9badbdd91..065a80f27d2383c50b8ebd186aa9bb4763792f17 100644 --- a/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h +++ b/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> diff --git a/bioloid_controller_cm510/src/sim/dynamixel_master.c b/bioloid_controller_cm510/src/sim/dynamixel_master.c index ab59668fdc8352cc195cd664733311b3555da41d..7ab8bd8b816eb61109ea1d44d66c5f8f64b6266f 100644 --- a/bioloid_controller_cm510/src/sim/dynamixel_master.c +++ b/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; }