diff --git a/src/dynamixel_motor.cpp b/src/dynamixel_motor.cpp
index e65e57f0db0bb65d0fa16c03ef2fbcf095c76da3..4a0966780f68a8cccfd81bd72589fa47c3c1e086 100644
--- a/src/dynamixel_motor.cpp
+++ b/src/dynamixel_motor.cpp
@@ -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();
diff --git a/src/dynamixel_motor.h b/src/dynamixel_motor.h
index c2b5a2b4df5a568c7b774c66a80e17b8064bda07..1245965a730db8226a2790ac9273ae95e2f10411 100644
--- a/src/dynamixel_motor.h
+++ b/src/dynamixel_motor.h
@@ -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
*
diff --git a/src/examples/test_dynamixel_motor.cpp b/src/examples/test_dynamixel_motor.cpp
index 0eeb01f6701e8cb884ea0ca8e3312aeea38347cd..20e81b32cf50724a585c0b22640a626fdaac9b41 100755
--- a/src/examples/test_dynamixel_motor.cpp
+++ b/src/examples/test_dynamixel_motor.cpp
@@ -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){