Commit dc6fb5f0 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

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.
parent b59d3724
......@@ -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>
......
......@@ -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;
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment