diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 5e813a86cca753328a61110daf4bdbdc7a9aa083..830e2d7a6966698a366ad6098a5b91247164359f 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,9 +1,9 @@
ADD_SUBDIRECTORY(xml)
# edit the following line to add all the source code files of the library
-SET(sources dynamixel_motor.cpp dynamixel_motor_group.cpp)
+SET(sources dynamixel_motor.cpp dynamixel_motor_group.cpp dynamixel_motor_exceptions.cpp)
# edit the following line to add all the header files of the library
-SET(headers dynamixel_motor.h dynamixel_motor_group.h)
+SET(headers dynamixel_motor.h dynamixel_motor_group.h dynamixel_motor_exceptions.h)
FIND_PACKAGE(comm REQUIRED)
diff --git a/src/dynamixel_motor.cpp b/src/dynamixel_motor.cpp
index 0441f25414fe6c1a70caaa34077f678fbb2e42c3..1db90d287811f207704fdb93023c85d79fa9fa7e 100644
--- a/src/dynamixel_motor.cpp
+++ b/src/dynamixel_motor.cpp
@@ -1,3 +1,4 @@
+#include "dynamixel_motor_exceptions.h"
#include "dynamixelexceptions.h"
#include "dynamixelserver.h"
#include "eventexceptions.h"
@@ -76,6 +77,7 @@ void CDynamixelMotor::angles_to_controller(std::vector<float>& angles,std::vecto
if(angles.size()!=1)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"Invalid input vector size (it must have only one element).");
}
else
{
@@ -89,6 +91,7 @@ void CDynamixelMotor::speeds_to_controller(std::vector<float>& speeds,std::vecto
if(speeds.size()!=1)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"Invalid input vector size (it must have only one element).");
}
else
{
@@ -105,6 +108,7 @@ void CDynamixelMotor::accels_to_controller(std::vector<float>& accels,std::vecto
if(accels.size()!=1)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"Invalid input vector size (it must have only one element).");
}
else
{
@@ -118,6 +122,7 @@ void CDynamixelMotor::controller_to_angles(std::vector<float>& counts,std::vecto
if(counts.size()!=1)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"Invalid input vector size (it must have only one element).");
}
else
{
@@ -131,6 +136,7 @@ void CDynamixelMotor::controller_to_speeds(std::vector<float>& counts,std::vecto
if(counts.size()!=1)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"Invalid input vector size (it must have only one element).");
}
else
{
@@ -144,6 +150,7 @@ void CDynamixelMotor::controller_to_accels(std::vector<float>& counts,std::vecto
if(counts.size()!=1)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"Invalid input vector size (it must have only one element).");
}
else
{
@@ -157,12 +164,14 @@ void CDynamixelMotor::cont_set_position_range(std::vector<float>& min, std::vect
if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
}
else
{
if(min[0]<0 || max[0]>(this->dyn_enc_res-1))
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The desired range is outside the valid range of the servo.");
}
else
{
@@ -188,6 +197,7 @@ void CDynamixelMotor::cont_get_position_range(std::vector<float>& min, std::vect
if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
}
else
{
@@ -220,6 +230,7 @@ std::vector<float> CDynamixelMotor::cont_get_position(void)
if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
}
else
{
@@ -246,6 +257,7 @@ std::vector<float> CDynamixelMotor::cont_get_velocity(void)
if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
}
else
{
@@ -270,6 +282,7 @@ void CDynamixelMotor::cont_enable(std::vector<bool> &enable)
if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
}
else
{
@@ -295,6 +308,7 @@ void CDynamixelMotor::cont_disable(std::vector<bool> &disable)
if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
}
else
{
@@ -320,6 +334,7 @@ void CDynamixelMotor::cont_load(std::vector<float>& position, std::vector<float>
if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
}
else
{
@@ -357,10 +372,12 @@ void CDynamixelMotor::cont_load(std::vector<float>& velocity, std::vector<float>
if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
}
else
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"Velocity control is not supported. Use torque control instead.");
// velocity control is not supported
}
}
@@ -377,6 +394,7 @@ void CDynamixelMotor::cont_stop(void)
if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
}
else
{
@@ -486,6 +504,7 @@ int CDynamixelMotor::get_baudrate(void)
if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
}
else
{
@@ -510,6 +529,7 @@ unsigned char CDynamixelMotor::get_node_address(void)
if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
}
else
{
@@ -531,6 +551,7 @@ void CDynamixelMotor::set_node_address(unsigned char dev_id)
if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
}
else
{
@@ -552,6 +573,7 @@ std::string CDynamixelMotor::get_model(void)
if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
}
else
{
@@ -599,6 +621,14 @@ std::string CDynamixelMotor::get_model(void)
// set resolution
this->dyn_enc_res=4096;
break;
+ case 0x001D: this->info.model="MX-28";
+ // set maximum angle
+ this->dyn_max_angle=360;
+ // set maximum speed
+ this->dyn_max_speed=54;
+ // set resolution
+ this->dyn_enc_res=4096;
+ break;
default: this->info.model="unknown";
break;
}
@@ -619,6 +649,7 @@ unsigned char CDynamixelMotor::get_firmware_version(void)
if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
}
else
{
@@ -642,6 +673,7 @@ int CDynamixelMotor::get_temperature_limit(void)
if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
}
else
{
@@ -664,12 +696,14 @@ void CDynamixelMotor::set_temperature_limit(int limit)
if(limit<0 && limit>255)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The desired temperatura limit is out of range.");
}
else
{
if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
}
else
{
@@ -692,6 +726,7 @@ int CDynamixelMotor::get_current_temperature(void)
if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
}
else
{
@@ -715,6 +750,7 @@ void CDynamixelMotor::get_voltage_limits(float *min, float *max)
if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
}
else
{
@@ -739,18 +775,21 @@ void CDynamixelMotor::set_voltage_limits(float min, float max)
if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
}
else
{
if(min>max)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The maximum voltage is lower than the minimum voltage.");
}
else
{
if(min<5.0 || min>25.0 || max<5.0 || max>25.0)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The desired voltage range is outside the valid range.");
}
else
{
@@ -777,6 +816,7 @@ float CDynamixelMotor::get_current_voltage(void)
if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
}
else
{
@@ -800,6 +840,7 @@ unsigned char CDynamixelMotor::get_led_alarms(void)
if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
}
else
{
@@ -822,12 +863,14 @@ void CDynamixelMotor::set_led_alarms(unsigned char alarms)
if(alarms&0x80)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"Invalid alarm mask");
}
else
{
if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
}
else
{
@@ -850,6 +893,7 @@ unsigned char CDynamixelMotor::get_turn_off_alarms(void)
if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
}
else
{
@@ -872,12 +916,14 @@ void CDynamixelMotor::set_turn_off_alarms(unsigned char alarms)
if(alarms&0x80)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"Invalid alarm mask.");
}
else
{
if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
}
else
{
@@ -899,6 +945,7 @@ void CDynamixelMotor::turn_led_on(void)
if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
}
else
{
@@ -919,6 +966,7 @@ void CDynamixelMotor::turn_led_off(void)
if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
}
else
{
@@ -938,23 +986,33 @@ void CDynamixelMotor::get_compliance_margin(unsigned char *cw_margin, unsigned c
if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
}
else
{
- if(cw_margin==NULL || ccw_margin==NULL)
+ if(this->info.model=="MX-28" || this->info.model=="EX-106")
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"This servo model does not support compliance margin.");
}
else
{
- try{
- this->dynamixel_dev->read_byte_register(cw_comp_margin,cw_margin);
- this->dynamixel_dev->read_byte_register(ccw_comp_margin,ccw_margin);
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms)
- this->reset_motor();
- throw;
+ if(cw_margin==NULL || ccw_margin==NULL)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"Invalid complinace margin.");
+ }
+ else
+ {
+ try{
+ this->dynamixel_dev->read_byte_register(cw_comp_margin,cw_margin);
+ this->dynamixel_dev->read_byte_register(ccw_comp_margin,ccw_margin);
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms)
+ this->reset_motor();
+ throw;
+ }
}
}
}
@@ -965,25 +1023,36 @@ void CDynamixelMotor::set_compliance_margin(unsigned char cw_margin, unsigned ch
if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
}
else
{
- if(cw_margin>254)
+ if(this->info.model=="MX-28" || this->info.model=="EX-106")
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"This servo model does not support compliance margin.");
}
- if(ccw_margin>254)
+ else
{
- /* handle exceptions */
- }
- try{
- this->dynamixel_dev->write_byte_register(cw_comp_margin,cw_margin);
- this->dynamixel_dev->write_byte_register(ccw_comp_margin,ccw_margin);
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms)
- this->reset_motor();
- throw;
+ if(cw_margin>254)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"Invalid clockwise compliance margin.");
+ }
+ if(ccw_margin>254)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"Invalid counterclockwise compliance margin.");
+ }
+ try{
+ this->dynamixel_dev->write_byte_register(cw_comp_margin,cw_margin);
+ this->dynamixel_dev->write_byte_register(ccw_comp_margin,ccw_margin);
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms)
+ this->reset_motor();
+ throw;
+ }
}
}
}
@@ -993,23 +1062,33 @@ void CDynamixelMotor::get_compliance_slope(unsigned char *cw_slope, unsigned cha
if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
}
else
{
- if(cw_slope==NULL || ccw_slope==NULL)
+ if(this->info.model=="MX-28" || this->info.model=="EX-106")
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"This servo model does not support compliance slope.");
}
else
{
- try{
- this->dynamixel_dev->read_byte_register(cw_comp_slope,cw_slope);
- this->dynamixel_dev->read_byte_register(ccw_comp_slope,ccw_slope);
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms)
- this->reset_motor();
- throw;
+ if(cw_slope==NULL || ccw_slope==NULL)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"Invalid compliance slope.");
+ }
+ else
+ {
+ try{
+ this->dynamixel_dev->read_byte_register(cw_comp_slope,cw_slope);
+ this->dynamixel_dev->read_byte_register(ccw_comp_slope,ccw_slope);
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms)
+ this->reset_motor();
+ throw;
+ }
}
}
}
@@ -1020,41 +1099,52 @@ void CDynamixelMotor::set_compliance_slope(unsigned char cw_slope, unsigned char
if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
}
else
{
- if(cw_slope>=1 && cw_slope<=5) cw_slope=4;
- else if(cw_slope>=6 && cw_slope<=11) cw_slope=8;
- else if(cw_slope>=12 && cw_slope<=23) cw_slope=16;
- else if(cw_slope>=24 && cw_slope<=47) cw_slope=32;
- else if(cw_slope>=48 && cw_slope<=95) cw_slope=64;
- else if(cw_slope>=96 && cw_slope<=191) cw_slope=128;
- else if(cw_slope>=192 && cw_slope<=254) cw_slope=254;
- else
+ if(this->info.model=="MX-28" || this->info.model=="EX-106")
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"This servo model does not support compliance slope.");
}
- if(ccw_slope>=1 && ccw_slope<=5) ccw_slope=4;
- else if(ccw_slope>=6 && ccw_slope<=11) ccw_slope=8;
- else if(ccw_slope>=12 && ccw_slope<=23) ccw_slope=16;
- else if(ccw_slope>=24 && ccw_slope<=47) ccw_slope=32;
- else if(ccw_slope>=48 && ccw_slope<=95) ccw_slope=64;
- else if(ccw_slope>=96 && ccw_slope<=191) ccw_slope=128;
- else if(ccw_slope>=192 && ccw_slope<=254) ccw_slope=254;
else
{
- /* handle exceptions */
- }
- try{
- this->dynamixel_dev->write_byte_register(cw_comp_slope,cw_slope);
- this->dynamixel_dev->write_byte_register(ccw_comp_slope,ccw_slope);
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms)
- this->reset_motor();
- throw;
+ if(cw_slope>=1 && cw_slope<=5) cw_slope=4;
+ else if(cw_slope>=6 && cw_slope<=11) cw_slope=8;
+ else if(cw_slope>=12 && cw_slope<=23) cw_slope=16;
+ else if(cw_slope>=24 && cw_slope<=47) cw_slope=32;
+ else if(cw_slope>=48 && cw_slope<=95) cw_slope=64;
+ else if(cw_slope>=96 && cw_slope<=191) cw_slope=128;
+ else if(cw_slope>=192 && cw_slope<=254) cw_slope=254;
+ else
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"Invalid clockwise compliance slope.");
+ }
+ if(ccw_slope>=1 && ccw_slope<=5) ccw_slope=4;
+ else if(ccw_slope>=6 && ccw_slope<=11) ccw_slope=8;
+ else if(ccw_slope>=12 && ccw_slope<=23) ccw_slope=16;
+ else if(ccw_slope>=24 && ccw_slope<=47) ccw_slope=32;
+ else if(ccw_slope>=48 && ccw_slope<=95) ccw_slope=64;
+ else if(ccw_slope>=96 && ccw_slope<=191) ccw_slope=128;
+ else if(ccw_slope>=192 && ccw_slope<=254) ccw_slope=254;
+ else
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"Invalid counterclockwise compliance slope.");
+ }
+ try{
+ this->dynamixel_dev->write_byte_register(cw_comp_slope,cw_slope);
+ this->dynamixel_dev->write_byte_register(ccw_comp_slope,ccw_slope);
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms)
+ this->reset_motor();
+ throw;
+ }
}
- }
+ }
}
short int CDynamixelMotor::get_punch(void)
@@ -1064,6 +1154,7 @@ short int CDynamixelMotor::get_punch(void)
if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
}
else
{
@@ -1085,12 +1176,14 @@ void CDynamixelMotor::set_punch(short int punch_value)
if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
}
else
{
if(punch<0 || punch>1023)
{
/* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"Invalid punch value.");
}
try{
this->dynamixel_dev->write_word_register(punch,punch_value);
@@ -1101,7 +1194,52 @@ void CDynamixelMotor::set_punch(short int punch_value)
throw;
}
}
+}
+
+void CDynamixelMotor::get_pid(unsigned char *p,unsigned char *i,unsigned char *d)
+{
+ if(this->dynamixel_dev==NULL)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+ }
+ else
+ {
+ if(this->info.model=="MX-28" || this->info.model=="EX-106")
+ {
+ this->dynamixel_dev->read_byte_register(pid_p,p);
+ this->dynamixel_dev->read_byte_register(pid_i,i);
+ this->dynamixel_dev->read_byte_register(pid_d,d);
+ }
+ else
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"This servo model does not support PID.");
+ }
+ }
+}
+void CDynamixelMotor::set_pid(unsigned char p,unsigned char i,unsigned char d)
+{
+ if(this->dynamixel_dev==NULL)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+ }
+ else
+ {
+ if(this->info.model=="MX-28" || this->info.model=="EX-106")
+ {
+ this->dynamixel_dev->write_byte_register(pid_p,p);
+ this->dynamixel_dev->write_byte_register(pid_i,i);
+ this->dynamixel_dev->write_byte_register(pid_d,d);
+ }
+ else
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"This servo model does not support PID.");
+ }
+ }
}
bool CDynamixelMotor::is_locked(void)
@@ -1118,23 +1256,39 @@ void CDynamixelMotor::enable_torque_control(void)
{
std::vector<float> min,max;
- min.push_back(0.0);
- max.push_back(0.0);
- this->set_position_range(min,max);
- this->disable_position_feedback();
- this->torque_control=true;
+ if(this->dynamixel_dev==NULL)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+ }
+ else
+ {
+ min.push_back(0.0);
+ max.push_back(0.0);
+ this->set_position_range(min,max);
+ this->disable_position_feedback();
+ this->torque_control=true;
+ }
}
void CDynamixelMotor::disable_torque_control(void)
{
std::vector<float> min,max;
- min.push_back(0.0);
- max.push_back(300.0);
- this->set_position_range(min,max);
- this->config_position_feedback(fb_polling,100.0);
- this->enable_position_feedback();
- this->torque_control=false;
+ if(this->dynamixel_dev==NULL)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+ }
+ else
+ {
+ min.push_back(0.0);
+ max.push_back(this->dyn_max_angle);
+ this->set_position_range(min,max);
+ this->config_position_feedback(fb_polling,100.0);
+ this->enable_position_feedback();
+ this->torque_control=false;
+ }
}
bool CDynamixelMotor::is_torque_control_enabled(void)
@@ -1146,34 +1300,44 @@ void CDynamixelMotor::set_torque(float torque_ratio)
{
unsigned short int torque=0x0000;
- if(this->torque_control)
+ if(this->dynamixel_dev==NULL)
{
- if(torque_ratio>100.0 || torque_ratio<-100.0)
- {
- /* handle exceptions */
- }
- else
- {
- if(torque_ratio<0.0)
- torque+=0x0400;
- torque+=((unsigned short int)(fabs(torque_ratio)*1023.0/100.0))&0x03FF;
- this->dynamixel_dev->write_word_register(goal_speed,torque);
- }
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
}
else
{
- if(torque_ratio>100.0 || torque_ratio<0.0)
+ if(this->torque_control)
{
- /* handle exceptions */
+ if(torque_ratio>100.0 || torque_ratio<-100.0)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"Invalid torque ratio.");
+ }
+ else
+ {
+ if(torque_ratio<0.0)
+ torque+=0x0400;
+ torque+=((unsigned short int)(fabs(torque_ratio)*1023.0/100.0))&0x03FF;
+ this->dynamixel_dev->write_word_register(goal_speed,torque);
+ }
}
else
{
- // set the maximum torque of the servo
- torque=((unsigned short int)(fabs(torque_ratio)*1023.0/100.0))&0x03FF;
- // save the value to EEPROM
- this->dynamixel_dev->write_word_register(max_torque,torque);
- // update the current value
- this->dynamixel_dev->write_word_register(torque_limit,torque);
+ if(torque_ratio>100.0 || torque_ratio<0.0)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"Invalid torque ratio.");
+ }
+ else
+ {
+ // set the maximum torque of the servo
+ torque=((unsigned short int)(fabs(torque_ratio)*1023.0/100.0))&0x03FF;
+ // save the value to EEPROM
+ this->dynamixel_dev->write_word_register(max_torque,torque);
+ // update the current value
+ this->dynamixel_dev->write_word_register(torque_limit,torque);
+ }
}
}
}
diff --git a/src/dynamixel_motor.h b/src/dynamixel_motor.h
index 320d63ede9f8ee9a0674a9abbd9907c7a842664c..ccf412aabfb9aaadf8957d6b59cb681ec10fd2f8 100644
--- a/src/dynamixel_motor.h
+++ b/src/dynamixel_motor.h
@@ -30,6 +30,9 @@ typedef enum {
up_cal=0x16,
torque_en=0x18,
led=0x19,
+ pid_p=0x1A,
+ pid_i=0x1B,
+ pid_d=0x1C,
cw_comp_margin=0x1A,
ccw_comp_margin=0x1B,
cw_comp_slope=0x1C,
@@ -356,6 +359,16 @@ class CDynamixelMotor : public CMotorControl
*
*/
void set_punch(short int punch_value);
+ /**
+ * \brief
+ *
+ */
+ void get_pid(unsigned char *p,unsigned char *i,unsigned char *d);
+ /**
+ * \brief
+ *
+ */
+ void set_pid(unsigned char p,unsigned char i,unsigned char d);
/**
* \brief
*
diff --git a/src/dynamixel_motor_group.cpp b/src/dynamixel_motor_group.cpp
index ca331b4e072ae39dccbf17c424058f107e7cf93b..4443ce90dd3a62891ef5d251f8ee590b2ab1e99b 100644
--- a/src/dynamixel_motor_group.cpp
+++ b/src/dynamixel_motor_group.cpp
@@ -1,19 +1,18 @@
-#include "dynamixelexceptions.h"
+#include "dynamixel_motor_exceptions.h"
#include "dynamixelserver.h"
#include "eventexceptions.h"
#include "dynamixel_motor_group.h"
+#include "dynamixel_motor.h"
#include "ftdiserver.h"
#include <math.h>
#include <iostream>
-
-
CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id):CMotorGroup(group_id)
{
if(group_id.size()==0)
{
/* handle exceptions */
- throw CDynamixelServerException(_HERE_,"Invalid group identifier - empty string");
+ throw CDynamixelMotorGroupException(_HERE_,"Invalid group identifier - empty string");
}
else
{
@@ -23,40 +22,31 @@ CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id):CMotorGroup(gr
this->servo_id[1]=0x04;
this->servo_id[2]=0x03;
this->servo_id[3]=0x05;
-
+
}
}
-
-
-
-
-
void CDynamixelMotorGroup::move(std::vector<float> &position,std::vector<float> &velocity,std::vector<float> &acceleration)
{
-
+ std::vector< std::vector<unsigned char> > data;
unsigned char star_addrs;
-
unsigned int i=0;
- int num_instruc=4;
- unsigned char const goal_pos=0x1E;
- std::vector< std::vector<unsigned char> > data;
-
-
+
+
if(position.size()!=this->servo_id.size())
{
/* handle errors */
- throw CDynamixelServerException(_HERE_,"The number of position commands does not coincide with the total number of motors in the group");
+ throw CDynamixelMotorGroupException(_HERE_,"The number of position commands does not coincide with the total number of motors in the group");
}
else if(velocity.size()!=this->servo_id.size())
{
/* handle exceptions */
- throw CDynamixelServerException(_HERE_,"The number of velocity commands does not coincide with the total number of motors in the group");
+ throw CDynamixelMotorGroupException(_HERE_,"The number of velocity commands does not coincide with the total number of motors in the group");
}
else if(acceleration.size()!=this->servo_id.size())
{
/* handle exceptions */
- throw CDynamixelServerException(_HERE_,"The number of acceleration commands does not coincide with the total number of motors in the group");
+ throw CDynamixelMotorGroupException(_HERE_,"The number of acceleration commands does not coincide with the total number of motors in the group");
}
else
{
@@ -65,34 +55,29 @@ void CDynamixelMotorGroup::move(std::vector<float> &position,std::vector<float>
data.resize(servo_id.size());
for(i=0;i<servo_id.size();i++)
{
- /* Number of instructions in bytes:
- - goal_pos: 2 bytes
- - goal_speed: 2 bytes */
- data[i].resize(num_instruc);
- }
- for(i=0;i<servo_id.size();i++)
- {
-
- data[i][0]=((int)position[i])%256;
- data[i][1]=position[i]/256;
- data[i][2]=0xFF;
- data[i][3]=0x03;
- }
-
- dyn_server->write_sync(servo_id,star_addrs,data);
-
-
- }catch(CException &e){
-
- std::cout << "Movement aborted" << std::endl;
- }
-
- }
-
+ /* Number of instructions in bytes:
+ - goal_pos: 2 bytes
+ - goal_speed: 2 bytes */
+ data[i].resize(4);
+ }
+ for(i=0;i<servo_id.size();i++)
+ {
+
+ data[i][0]=((int)position[i])%256;
+ data[i][1]=position[i]/256;
+ data[i][2]=0xFF;
+ data[i][3]=0x03;
+ }
+ dyn_server->write_sync(servo_id,star_addrs,data);
+ }catch(CException &e){
+ /* handle exceptions */
+ throw;
+ }
+ }
}
CDynamixelMotorGroup::~CDynamixelMotorGroup()
{
-
+
}
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index e37c14a68768fbaa9f55715ad4b5e022c4fdc2e5..0843898674d206ab2b5e622d22d67df39777d05f 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -21,3 +21,9 @@ ADD_EXECUTABLE(test_dynamixel_motor_group test_dynamixel_motor_group.cpp)
# edit the following line to add the necessary libraries
TARGET_LINK_LIBRARIES(test_dynamixel_motor_group dynamixel_motor_cont ${comm_LIBRARY} ${motor_control_LIBRARY})
+
+# edit the following line to add the source code for the example and the name of the executable
+ADD_EXECUTABLE(test_dynamixel_motor_open_loop test_dynamixel_motor_open_loop.cpp)
+
+# edit the following line to add the necessary libraries
+TARGET_LINK_LIBRARIES(test_dynamixel_motor_open_loop dynamixel_motor_cont ${comm_LIBRARY} ${motor_control_LIBRARY})
diff --git a/src/examples/test_dynamixel_motor.cpp b/src/examples/test_dynamixel_motor.cpp
index d010360a456c99b314d43a4865304d92ea084c56..39c5e5926b2910ebb74020a9beace5eddb8a8f2f 100755
--- a/src/examples/test_dynamixel_motor.cpp
+++ b/src/examples/test_dynamixel_motor.cpp
@@ -23,10 +23,10 @@ int main(int argc, char *argv[])
try{
if(dyn_server->get_num_buses()>0)
{
- cont2=new CDynamixelMotor(cont2_name,0,1000000,1);
+ cont2=new CDynamixelMotor(cont2_name,0,1000000,11);
cont2->close();
delete cont2;
- cont2=new CDynamixelMotor(cont2_name,0,1000000,1);
+ cont2=new CDynamixelMotor(cont2_name,0,1000000,11);
enable[0]=true;
cont2->enable(enable);
#ifdef _HAVE_XSD
@@ -35,8 +35,8 @@ int main(int argc, char *argv[])
#else
events2.push_back(cont2->config_position_feedback(fb_polling,100.0));
#endif
- cont2->set_torque(10.0);
- cont3=new CDynamixelMotor(cont3_name,0,1000000,2);
+ cont2->set_torque(100.0);
+ cont3=new CDynamixelMotor(cont3_name,0,1000000,21);
enable[0]=true;
cont3->enable(enable);
#ifdef _HAVE_XSD
diff --git a/src/examples/test_dynamixel_motor_group.cpp b/src/examples/test_dynamixel_motor_group.cpp
index 46989e59584aa5879355827622e7939bcf36c5d6..ef63132c2ec033eddc08e904d354e9c6a34655b4 100644
--- a/src/examples/test_dynamixel_motor_group.cpp
+++ b/src/examples/test_dynamixel_motor_group.cpp
@@ -19,7 +19,7 @@ int main(int argc, char *argv[])
std::vector<float> pos(4),vel(4),acc(4);
CDynamixelMotor *cont1,*cont2,*cont3,*cont4;
CDynamixelMotorGroup *group;
- int dir=1,i=0;
+ unsigned int dir=1,i=0;
try{
if(dyn_server->get_num_buses()>0)
diff --git a/src/examples/test_dynamixel_motor_open_loop.cpp b/src/examples/test_dynamixel_motor_open_loop.cpp
new file mode 100755
index 0000000000000000000000000000000000000000..6bf2c8b1e12e54059fc44c414bbf3956cfa851ad
--- /dev/null
+++ b/src/examples/test_dynamixel_motor_open_loop.cpp
@@ -0,0 +1,146 @@
+#include "dynamixelexceptions.h"
+#include "dynamixelserver.h"
+#include "eventserver.h"
+#include "exceptions.h"
+#include "dynamixel_motor.h"
+#include <iostream>
+
+std::string cont2_name="AX-12+_2";
+std::string cont3_name="AX-12+_3";
+std::string cont_config_file="../src/xml/base_dyn_config.xml";
+
+int main(int argc, char *argv[])
+{
+ CDynamixelServer *dyn_server=CDynamixelServer::instance();
+ CEventServer *event_server=CEventServer::instance();
+ std::list<std::string> events1,events2,events3;
+ CDynamixelMotor *cont2,*cont3;
+ std::vector<float> pos1(1),vel1(1),acc1(1),pos0(1);
+ std::vector<float> pos2(1),max(1),min(1);
+ std::vector<float> position(1);
+ std::vector<bool> enable(1);
+ int i=0;
+
+ try{
+ if(dyn_server->get_num_buses()>0)
+ {
+ cont2=new CDynamixelMotor(cont2_name,0,1000000,11);
+ cont2->close();
+ delete cont2;
+ cont2=new CDynamixelMotor(cont2_name,0,1000000,11);
+ enable[0]=true;
+ cont2->enable(enable);
+#ifdef _HAVE_XSD
+ cont2->load_config(cont_config_file);
+ events2.push_back(cont2->get_position_feedback_event());
+#else
+ events2.push_back(cont2->config_position_feedback(fb_polling,100.0));
+#endif
+ cont2->set_torque(100.0);
+ cont3=new CDynamixelMotor(cont3_name,0,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;
+ vel1[0]=40.0;
+ pos0[0]=150.0;
+ acc1[0]=vel1[0]*vel1[0]/(0.05*pos0[0]);
+ event_server->wait_all(events2);
+ 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();
+ sleep(2);
+ // set open loop mode
+
+ pos1[0]=190.0;
+ pos2[0]=110.0;
+ for(;;)
+ {
+ std::cout << "open loop control" << std::endl;
+ cont2->use_open_loop_control();
+ cont3->use_open_loop_control();
+ for(;;)
+ {
+// event_server->wait_all(events2);
+ std::cout << "servo 1 moving left ..." << std::endl;
+ cont2->load(pos1,vel1,acc1);
+ cont2->move();
+ sleep(1);
+// event_server->wait_all(events2);
+ std::cout << "servo 1 moving right ..." << std::endl;
+ cont2->load(pos2,vel1,acc1);
+ cont2->move();
+ sleep(2);
+// event_server->wait_all(events2);
+ std::cout << "servo 1 centering ..." << std::endl;
+ 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();
+ sleep(1);
+// event_server->wait_all(events3);
+ std::cout << "servo 2 moving right ..." << std::endl;
+ cont3->load(pos2,vel1,acc1);
+ cont3->move();
+ sleep(2);
+// event_server->wait_all(events3);
+ std::cout << "servo 2 centering ..." << std::endl;
+ cont3->load(pos0,vel1,acc1);
+ cont3->move();
+ sleep(1);
+ }
+
+ std::cout << "closed loop control" << std::endl;
+ cont2->use_closed_loop_control();
+ cont3->use_closed_loop_control();
+
+ for(i=0;i<1;i++)
+ {
+ event_server->wait_all(events2);
+ std::cout << "servo 1 moving left ..." << std::endl;
+ cont2->load(pos1,vel1,acc1);
+ cont2->move();
+ event_server->wait_all(events2);
+ std::cout << "servo 1 moving right ..." << std::endl;
+ cont2->load(pos2,vel1,acc1);
+ cont2->move();
+ event_server->wait_all(events2);
+ std::cout << "servo 1 centering ..." << std::endl;
+ cont2->load(pos0,vel1,acc1);
+ cont2->move();
+ 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){
+ std::cout << e.what() << std::endl;
+ }
+ return 0;
+}
diff --git a/src/xml/base_dyn_config.xml b/src/xml/base_dyn_config.xml
index 772978d491a7c8ef1725877aff464174b49bfdc8..7011ccd0c1c1f487b5066420cf6a152adb8ce294 100755
--- a/src/xml/base_dyn_config.xml
+++ b/src/xml/base_dyn_config.xml
@@ -22,16 +22,17 @@
<position_feedback>
<enabled>1</enabled>
<mode>polling</mode>
- <rate>1.0</rate>
+ <rate>100.0</rate>
</position_feedback>
<velocity_feedback>
<enabled>0</enabled>
<mode>polling</mode>
- <rate>1.0</rate>
+ <rate>100.0</rate>
</velocity_feedback>
<limits_feedback>
<enabled>0</enabled>
<mode>polling</mode>
<rate>1.0</rate>
</limits_feedback>
+ <open_loop>0</open_loop>
</motor_config>