Skip to content
Snippets Groups Projects
Commit e94288f7 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added functions to get the maximum torque and the torque limit.

When the reset_motor() function is called, the max_torque is loaded instead of the maximum torque.
parent c5d61456
No related branches found
No related tags found
No related merge requests found
......@@ -30,6 +30,7 @@ CDynamixelMotor::CDynamixelMotor(std::string& cont_id,unsigned char bus_id,int b
this->dyn_enc_res=-1;
this->dyn_max_angle=-1;
this->dyn_max_speed=-1;
this->dyn_max_torque=-1;
this->torque_control=false;
try{
this->dyn_server->config_bus(bus_id,baudrate);
......@@ -44,6 +45,7 @@ CDynamixelMotor::CDynamixelMotor(std::string& cont_id,unsigned char bus_id,int b
this->alarms=this->get_turn_off_alarms();
this->info.firmware_ver=this->get_firmware_version();
this->get_position_range(min,max);
this->dyn_max_torque=this->get_max_torque();
config.acceleration.resize(1);
config.acceleration[0].min=0.0;
config.acceleration[0].max=0.0;
......@@ -91,6 +93,7 @@ CDynamixelMotor::CDynamixelMotor(std::string& cont_id,std::string &bus_id,int ba
this->dyn_enc_res=-1;
this->dyn_max_angle=-1;
this->dyn_max_speed=-1;
this->dyn_max_torque=-1;
this->torque_control=false;
try{
this->dyn_server->config_bus(bus_id,baudrate);
......@@ -105,6 +108,7 @@ CDynamixelMotor::CDynamixelMotor(std::string& cont_id,std::string &bus_id,int ba
this->alarms=this->get_turn_off_alarms();
this->info.firmware_ver=this->get_firmware_version();
this->get_position_range(min,max);
this->dyn_max_torque=this->get_max_torque();
config.acceleration.resize(1);
config.acceleration[0].min=0.0;
config.acceleration[0].max=0.0;
......@@ -561,7 +565,7 @@ void CDynamixelMotor::reset_motor(void)
/* do nothing - expected exception */
}
try{
this->dynamixel_dev->write_word_register(torque_limit,0x3FF);
this->dynamixel_dev->write_word_register(torque_limit,((unsigned short int)(fabs(this->dyn_max_torque)*1023.0/100.0))&0x03FF);
}catch(CDynamixelAlarmException &e){
/* do nothing - expected exception */
}
......@@ -1444,6 +1448,48 @@ float CDynamixelMotor::get_torque(void)
return torque;
}
float CDynamixelMotor::get_max_torque(void)
{
unsigned short int load;
float torque;
if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
}
else
{
this->dynamixel_dev->read_word_register(max_torque,&load);
torque=(load&0x3FF)*100.0/1023;
if(load>0x3FF)
torque=-1*torque;
}
return torque;
}
float CDynamixelMotor::get_limit_torque(void)
{
unsigned short int load;
float torque;
if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
}
else
{
this->dynamixel_dev->read_word_register(torque_limit,&load);
torque=(load&0x3FF)*100.0/1023;
if(load>0x3FF)
torque=-1*torque;
}
return torque;
}
CDynamixelMotor::~CDynamixelMotor()
{
this->close();
......
......@@ -130,6 +130,11 @@ class CDynamixelMotor : public CMotorControl
*
*/
int dyn_max_speed;
/**
* \brief
*
*/
int dyn_max_torque;
protected:
/**
* \brief
......@@ -414,6 +419,16 @@ class CDynamixelMotor : public CMotorControl
*
*/
float get_torque(void);
/**
* \brief
*
*/
float get_max_torque(void);
/**
* \brief
*
*/
float get_limit_torque(void);
/**
* \brief
*
......
......@@ -19,15 +19,15 @@ int main(int argc, char *argv[])
std::vector<float> pos2(1),max(1),min(1);
std::vector<float> position(1);
std::vector<bool> enable(1);
std::string serial="A9007DKA";
std::string serial="A400gaIt";
try{
if(dyn_server->get_num_buses()>0)
{
cont2=new CDynamixelMotor(cont2_name,serial,1000000,12);
cont2=new CDynamixelMotor(cont2_name,serial,1000000,1);
cont2->close();
delete cont2;
cont2=new CDynamixelMotor(cont2_name,serial,1000000,12);
cont2=new CDynamixelMotor(cont2_name,serial,1000000,1);
enable[0]=true;
cont2->enable(enable);
#ifdef _HAVE_XSD
......@@ -37,19 +37,19 @@ int main(int argc, char *argv[])
events2.push_back(cont2->config_position_feedback(fb_polling,100.0));
#endif
cont2->set_torque(100.0);
cont3=new CDynamixelMotor(cont3_name,serial,1000000,21);
enable[0]=true;
cont3->enable(enable);
#ifdef _HAVE_XSD
cont3->load_config(cont_config_file);
events3.push_back(cont3->get_position_feedback_event());
#else
events3.push_back(cont3->config_position_feedback(fb_polling,100.0));
#endif
// cont3=new CDynamixelMotor(cont3_name,serial,1000000,21);
// enable[0]=true;
// cont3->enable(enable);
//#ifdef _HAVE_XSD
// cont3->load_config(cont_config_file);
// events3.push_back(cont3->get_position_feedback_event());
//#else
// events3.push_back(cont3->config_position_feedback(fb_polling,100.0));
//#endif
position=cont2->get_position();
std::cout << "Current position of device 1: " << position[0] << std::endl;
position=cont3->get_position();
std::cout << "Current position of device 15: " << position[0] << std::endl;
// position=cont3->get_position();
// std::cout << "Current position of device 15: " << position[0] << std::endl;
vel1[0]=40.0;
pos0[0]=150.0;
acc1[0]=vel1[0]*vel1[0]/(0.05*pos0[0]);
......@@ -57,14 +57,14 @@ int main(int argc, char *argv[])
std::cout << "centering ..." << std::endl;
cont2->load(pos0,vel1,acc1);
cont2->move();
event_server->wait_all(events3);
std::cout << "centering ..." << std::endl;
cont3->load(pos0,vel1,acc1);
cont3->move();
// event_server->wait_all(events3);
// std::cout << "centering ..." << std::endl;
// cont3->load(pos0,vel1,acc1);
// cont3->move();
sleep(2);
pos1[0]=190.0;
pos2[0]=110.0;
pos1[0]=170.0;
pos2[0]=125.0;
for(;;)
{
event_server->wait_all(events2);
......@@ -80,19 +80,19 @@ int main(int argc, char *argv[])
cont2->load(pos0,vel1,acc1);
cont2->move();
sleep(1);
event_server->wait_all(events3);
std::cout << "servo 2 moving left ..." << std::endl;
cont3->load(pos1,vel1,acc1);
cont3->move();
event_server->wait_all(events3);
std::cout << "servo 2 moving right ..." << std::endl;
cont3->load(pos2,vel1,acc1);
cont3->move();
event_server->wait_all(events3);
std::cout << "servo 2 centering ..." << std::endl;
cont3->load(pos0,vel1,acc1);
cont3->move();
sleep(1);
// event_server->wait_all(events3);
// std::cout << "servo 2 moving left ..." << std::endl;
// cont3->load(pos1,vel1,acc1);
// cont3->move();
// event_server->wait_all(events3);
// std::cout << "servo 2 moving right ..." << std::endl;
// cont3->load(pos2,vel1,acc1);
// cont3->move();
// event_server->wait_all(events3);
// std::cout << "servo 2 centering ..." << std::endl;
// cont3->load(pos0,vel1,acc1);
// cont3->move();
// sleep(1);
}
}
}catch(CException &e){
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment