diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 26b899230de257e66801bf79636009ce407de148..c626213960026842e233717dde865f1d5ce084ad 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -11,17 +11,15 @@ FIND_PACKAGE(comm REQUIRED)
FIND_PACKAGE(dynamixel REQUIRED)
-FIND_PACKAGE(motor_control REQUIRED)
-
# edit the following line to add the necessary include directories
-INCLUDE_DIRECTORIES(${iriutils_INCLUDE_DIR} ${comm_INCLUDE_DIR} ${dynamixel_INCLUDE_DIR} ${motor_control_INCLUDE_DIR} .)
+INCLUDE_DIRECTORIES(. ${iriutils_INCLUDE_DIR} ${comm_INCLUDE_DIR} ${dynamixel_INCLUDE_DIR})
SET_SOURCE_FILES_PROPERTIES(${XSD_SOURCES} PROPERTIES GENERATED 1)
ADD_LIBRARY(dynamixel_motor_cont SHARED ${sources} ${XSD_SOURCES})
#edit the following line to add the necessary system libraries (if any)
-TARGET_LINK_LIBRARIES(dynamixel_motor_cont ${iriutils_LIBRARY} ${comm_LIBRARY} ${dynamixel_LIBRARY} ${motor_control_LIBRARY} ${XSD_LIBRARY})
+TARGET_LINK_LIBRARIES(dynamixel_motor_cont ${iriutils_LIBRARY} ${comm_LIBRARY} ${dynamixel_LIBRARY} ${XSD_LIBRARY})
ADD_DEPENDENCIES(dynamixel_motor_cont xsd_files_gen)
diff --git a/src/dynamixel_motor.cpp b/src/dynamixel_motor.cpp
index 4a0966780f68a8cccfd81bd72589fa47c3c1e086..3648b1d35f56624bda3711511d69579707574b62 100644
--- a/src/dynamixel_motor.cpp
+++ b/src/dynamixel_motor.cpp
@@ -12,60 +12,52 @@
#include "xml/dynamixel_motor_cfg_file.hxx"
#endif
-CDynamixelMotor::CDynamixelMotor(std::string& cont_id,unsigned char bus_id,int baudrate,unsigned char dev_id):CMotorControl(cont_id,1)
+CDynamixelMotor::CDynamixelMotor(std::string& cont_id,unsigned char bus_id,int baudrate,unsigned char dev_id)
{
- std::vector<float> max,min,gear;
- std::vector<int> enc_res;
- TMotorConfig config;
-
this->info.model="";
- this->info.firmware_ver=0x00;
- this->control.cw_compliance_margin=0x00;
- this->control.ccw_compliance_margin=0x00;
- this->control.cw_compliance_slope=0x20;
- this->control.ccw_compliance_slope=0x20;
+ this->info.firmware_ver=(unsigned char)-1;
+ this->info.gear_ratio=(unsigned int)-1;
+ this->info.encoder_resolution=(unsigned int)-1;
+ this->info.pid_control=false;
+ this->info.max_angle=-1;
+ this->info.center_angle=-1;
+ this->info.max_speed=-1;
+ this->info.bus_id="";
+ this->info.baudrate=(unsigned int)-1;
+ this->info.id=(unsigned char)-1;
+ this->compliance.cw_compliance_margin=0x00;
+ this->compliance.ccw_compliance_margin=0x00;
+ this->compliance.cw_compliance_slope=0x20;
+ this->compliance.ccw_compliance_slope=0x20;
+ this->pid.p=0x00;
+ this->pid.i=0x00;
+ this->pid.d=0x00;
+ this->config.max_angle=0.0;
+ this->config.min_angle=0.0;
+ this->config.max_temperature=0.0;
+ this->config.max_voltage=0.0;
+ this->config.min_voltage=0.0;
+ this->config.max_torque=0.0;
this->dyn_server=CDynamixelServer::instance();
this->dynamixel_dev=NULL;
- this->alarms=0x24;
- 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);
+ this->info.baudrate=baudrate;
+ this->info.bus_id=this->dyn_server->get_bus_serial();
this->dynamixel_dev=this->dyn_server->get_device(dev_id);
- this->info.model=this->get_model();
- enc_res.push_back(this->dyn_enc_res);
- config.encoder_resolution=enc_res;
- this->set_encoder_resolution(enc_res);
- gear.push_back(1);
- config.gear_factor=gear;
- this->set_gear_factor(gear);
- 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;
- config.velocity.resize(1);
- config.velocity[0].min=0.0;
- config.velocity[0].max=0.0;
- config.position.resize(1);
- config.position[0].min=min[0];
- config.position[0].max=max[0];
- config.open_loop=false;
- this->config(&config);
- if(min[0]==0.0 && max[0]==0.0)
- this->enable_torque_control();
+ this->info.id=dev_id;
+ this->get_model();
+ this->get_position_range(&this->config.min_angle,&this->config.max_angle);
+ if(this->config.min_angle==-this->info.center_angle && this->config.max_angle==-this->info.center_angle)
+ this->mode=torque_ctrl;
else
- {
- this->config_position_feedback(fb_polling,100.0);
- this->enable_position_feedback();
- this->torque_control=false;
- }
- this->current_position=this->cont_get_position()[0];
+ this->mode=angle_ctrl;
+ this->config.max_temperature=this->get_temperature_limit();
+ this->get_voltage_limits(&this->config.min_voltage,&this->config.max_voltage);
+ this->config.max_torque=this->get_max_torque();
+ this->get_compliance_control(this->compliance);
+ this->get_pid_control(this->pid);
+ this->alarms=this->get_turn_off_alarms();
}catch(CException &e){
/* handle exceptions */
if(this->dynamixel_dev!=NULL)
@@ -75,60 +67,52 @@ CDynamixelMotor::CDynamixelMotor(std::string& cont_id,unsigned char bus_id,int b
}
}
-CDynamixelMotor::CDynamixelMotor(std::string& cont_id,std::string &bus_id,int baudrate,unsigned char dev_id):CMotorControl(cont_id,1)
+CDynamixelMotor::CDynamixelMotor(std::string& cont_id,std::string &bus_id,int baudrate,unsigned char dev_id)
{
- std::vector<float> max,min,gear;
- std::vector<int> enc_res;
- TMotorConfig config;
-
this->info.model="";
- this->info.firmware_ver=0x00;
- this->control.cw_compliance_margin=0x00;
- this->control.ccw_compliance_margin=0x00;
- this->control.cw_compliance_slope=0x20;
- this->control.ccw_compliance_slope=0x20;
+ this->info.firmware_ver=(unsigned char)-1;
+ this->info.gear_ratio=(unsigned int)-1;
+ this->info.encoder_resolution=(unsigned int)-1;
+ this->info.pid_control=false;
+ this->info.max_angle=-1;
+ this->info.center_angle=-1;
+ this->info.max_speed=-1;
+ this->info.bus_id="";
+ this->info.baudrate=(unsigned int)-1;
+ this->info.id=(unsigned char)-1;
+ this->compliance.cw_compliance_margin=0x00;
+ this->compliance.ccw_compliance_margin=0x00;
+ this->compliance.cw_compliance_slope=0x00;
+ this->compliance.ccw_compliance_slope=0x00;
+ this->pid.p=0x00;
+ this->pid.i=0x00;
+ this->pid.d=0x00;
+ this->config.max_angle=0.0;
+ this->config.min_angle=0.0;
+ this->config.max_temperature=0.0;
+ this->config.max_voltage=0.0;
+ this->config.min_voltage=0.0;
+ this->config.max_torque=0.0;
this->dyn_server=CDynamixelServer::instance();
this->dynamixel_dev=NULL;
- this->alarms=0x24;
- 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);
+ this->info.baudrate=baudrate;
+ this->info.bus_id=bus_id;
this->dynamixel_dev=this->dyn_server->get_device(dev_id);
- this->info.model=this->get_model();
- enc_res.push_back(this->dyn_enc_res);
- config.encoder_resolution=enc_res;
- this->set_encoder_resolution(enc_res);
- gear.push_back(1);
- config.gear_factor=gear;
- this->set_gear_factor(gear);
- 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;
- config.velocity.resize(1);
- config.velocity[0].min=0.0;
- config.velocity[0].max=0.0;
- config.position.resize(1);
- config.position[0].min=min[0];
- config.position[0].max=max[0];
- config.open_loop=false;
- this->config(&config);
- if(min[0]==0.0 && max[0]==0.0)
- this->enable_torque_control();
+ this->info.id=dev_id;
+ this->get_model();
+ this->get_position_range(&this->config.min_angle,&this->config.max_angle);
+ if(this->config.min_angle==-this->info.center_angle && this->config.max_angle==-this->info.center_angle)
+ this->mode=torque_ctrl;
else
- {
- this->config_position_feedback(fb_polling,100.0);
- this->enable_position_feedback();
- this->torque_control=false;
- }
- this->current_position=this->cont_get_position()[0];
+ this->mode=angle_ctrl;
+ this->config.max_temperature=this->get_temperature_limit();
+ this->get_voltage_limits(&this->config.min_voltage,&this->config.max_voltage);
+ this->config.max_torque=this->get_max_torque();
+ this->get_compliance_control(this->compliance);
+ this->get_pid_control(this->pid);
+ this->alarms=this->get_turn_off_alarms();
}catch(CException &e){
/* handle exceptions */
if(this->dynamixel_dev!=NULL)
@@ -138,328 +122,79 @@ CDynamixelMotor::CDynamixelMotor(std::string& cont_id,std::string &bus_id,int ba
}
}
-void CDynamixelMotor::angles_to_controller(std::vector<float>& angles,std::vector<float>& counts)
+unsigned int CDynamixelMotor::from_angles(double angle)
{
- if(angles.size()!=1)
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"Invalid input vector size (it must have only one element).");
- }
- else
- {
- counts.clear();
- counts.push_back(angles[0]*this->dyn_enc_res/this->dyn_max_angle);
- }
-}
+ unsigned int counts;
-void CDynamixelMotor::speeds_to_controller(std::vector<float>& speeds,std::vector<float>& counts)
-{
- if(speeds.size()!=1)
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"Invalid input vector size (it must have only one element).");
- }
- else
- {
- counts.clear();
- if(speeds[0]>0.0)
- counts.push_back(fabs(speeds[0])*1024/(this->dyn_max_speed*6.0));
- else
- counts.push_back(fabs(speeds[0])*1024/(this->dyn_max_speed*6.0));
- }
-}
+ counts=(angle+this->info.center_angle)*this->info.encoder_resolution/this->info.max_angle;
-void CDynamixelMotor::accels_to_controller(std::vector<float>& accels,std::vector<float>& counts)
-{
- if(accels.size()!=1)
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"Invalid input vector size (it must have only one element).");
- }
- else
- {
- counts.clear();
- counts.push_back((long int)accels[0]);
- }
+ return counts;
}
-void CDynamixelMotor::controller_to_angles(std::vector<float>& counts,std::vector<float>& angles)
+unsigned int CDynamixelMotor::from_speeds(double speed)
{
- if(counts.size()!=1)
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"Invalid input vector size (it must have only one element).");
- }
- else
- {
- angles.clear();
- angles.push_back(counts[0]*this->dyn_max_angle/this->dyn_enc_res);
- }
-}
+ unsigned int counts;
-void CDynamixelMotor::controller_to_speeds(std::vector<float>& counts,std::vector<float>& speeds)
-{
- if(counts.size()!=1)
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"Invalid input vector size (it must have only one element).");
- }
- else
- {
- speeds.clear();
- speeds.push_back(counts[0]*this->dyn_max_speed*6.0/1024);
- }
-}
+ if(speed>this->info.max_speed)
+ speed=this->info.max_speed;
+ counts=fabs(speed)/0.666;
-void CDynamixelMotor::controller_to_accels(std::vector<float>& counts,std::vector<float>& accels)
-{
- if(counts.size()!=1)
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"Invalid input vector size (it must have only one element).");
- }
- else
- {
- accels.clear();
- accels.push_back((float)counts[0]);
- }
+ return counts;
}
-void CDynamixelMotor::cont_set_position_range(std::vector<float>& min, std::vector<float>& max)
+double CDynamixelMotor::to_angles(unsigned short int counts)
{
- 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))
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"The desired range is outside the valid range of the servo.");
- }
- else
- {
- try{
- this->dynamixel_dev->write_word_register(ccw_angle_limit,(unsigned short int)max[0]);
- this->dynamixel_dev->write_word_register(cw_angle_limit,(unsigned short int)min[0]);
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms)
- this->reset_motor();
- throw;
- }
- }
- }
-}
+ double angle;
-void CDynamixelMotor::cont_get_position_range(std::vector<float>& min, std::vector<float>& max)
-{
- unsigned short int angle_limit;
+ angle=counts*this->info.max_angle/this->info.encoder_resolution-this->info.center_angle;
- min.clear();
- max.clear();
- if(this->dynamixel_dev==NULL)
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
- }
- else
- {
- try{
- this->dynamixel_dev->read_word_register(ccw_angle_limit,&angle_limit);
- max.push_back(angle_limit);
- this->dynamixel_dev->read_word_register(cw_angle_limit,&angle_limit);
- min.push_back(angle_limit);
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms)
- this->reset_motor();
- throw;
- }
- }
+ return angle;
}
-void CDynamixelMotor::cont_get_gear_factor(std::vector<float>& gear)
+double CDynamixelMotor::to_speeds(unsigned short int counts)
{
- gear.clear();
- gear.push_back(1);
-}
-
-std::vector<float> CDynamixelMotor::cont_get_position(void)
-{
- unsigned short int current_position;
- std::vector<float> position;
-
- position.clear();
- if(this->dynamixel_dev==NULL)
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
- }
- else
- {
- try{
- this->dynamixel_dev->read_word_register(current_pos,¤t_position);
- position.push_back(current_position&(this->dyn_enc_res));
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms)
- this->reset_motor();
- throw;
- }
- }
-
- return position;
-}
-
-std::vector<float> CDynamixelMotor::cont_get_velocity(void)
-{
- unsigned short int current_velocity;
- std::vector<float> velocity;
-
- velocity.clear();
- if(this->dynamixel_dev==NULL)
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
- }
- else
- {
- try{
- this->dynamixel_dev->read_word_register(current_speed,¤t_velocity);
- velocity.push_back(current_velocity&1023);
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms)
- this->reset_motor();
- throw;
- }
- }
-
- return velocity;
-}
+ double speed;
-void CDynamixelMotor::cont_enable(std::vector<bool> &enable)
-{
- unsigned char value;
+ speed = counts*0.666; // Taken from the datasheets (sfoix)
- if(this->dynamixel_dev==NULL)
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
- }
- else
- {
- try{
- if(enable[0]==true)
- value=0x01;
- else
- value=0x00;
- this->dynamixel_dev->write_byte_register(torque_en,value);
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms)
- this->reset_motor();
- throw;
- }
- }
+ return speed;
}
-void CDynamixelMotor::cont_disable(std::vector<bool> &disable)
+void CDynamixelMotor::reset_motor(void)
{
- unsigned char value;
+ unsigned short int current_position;
+ unsigned short int maximum_torque;
- if(this->dynamixel_dev==NULL)
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
- }
- else
- {
- try{
- if(disable[0]==true)
- value=0x00;
- else
- value=0x01;
- this->dynamixel_dev->write_byte_register(torque_en,value);
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms)
- this->reset_motor();
- throw;
- }
+ try{
+ this->dynamixel_dev->read_word_register(current_pos,¤t_position);
+ }catch(CDynamixelAlarmException &e){
+ /* do nothing - expected exception */
}
-}
-
-void CDynamixelMotor::cont_load(std::vector<float>& position, std::vector<float>& velocity, std::vector<float>& accel)
-{
- std::vector<bool> relative;
-
- if(this->dynamixel_dev==NULL)
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+ try{
+ this->dynamixel_dev->write_word_register(goal_pos,current_position);
+ }catch(CDynamixelAlarmException &e){
+ /* do nothing - expected exception */
}
- else
- {
- if(this->get_motor_group_id()=="")
- {
- try{
- if(!this->torque_control)
- {
- this->dynamixel_dev->write_word_register(goal_speed,(unsigned short int)velocity[0]);
- this->is_motion_relative(relative);
- if(relative[0])
- {
- this->dynamixel_dev->registered_word_write(goal_pos,(unsigned short int)(this->current_position+position[0]));
- this->current_position+=position[0];
- }
- else
- {
- this->dynamixel_dev->registered_word_write(goal_pos,(unsigned short int)position[0]);
- this->current_position=position[0];
- }
- }
- else
- {
- /* handle exceptions */
- }
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms)
- this->reset_motor();
- throw;
- }
- }
+ try{
+ this->dynamixel_dev->read_word_register(max_torque,&maximum_torque);
+ }catch(CDynamixelAlarmException &e){
+ /* do nothing - expected exception */
}
-}
-
-void CDynamixelMotor::cont_load(std::vector<float>& velocity, std::vector<float>& accel)
-{
- if(this->dynamixel_dev==NULL)
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+ try{
+ this->dynamixel_dev->write_word_register(torque_limit,maximum_torque);
+ }catch(CDynamixelAlarmException &e){
+ /* do nothing - expected exception */
}
- else
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"Velocity control is not supported. Use torque control instead.");
- // velocity control is not supported
+ try{
+ this->dynamixel_dev->write_byte_register(led,0x00);
+ }catch(CDynamixelAlarmException &e){
+ /* do nothing - expected exception */
}
}
-void CDynamixelMotor::cont_move(void)
+void CDynamixelMotor::get_model(void)
{
- if(this->get_motor_group_id()=="")
- this->dyn_server->action();
-}
-
-void CDynamixelMotor::cont_stop(void)
-{
- unsigned short int current_position;
+ unsigned short int model;
if(this->dynamixel_dev==NULL)
{
@@ -469,75 +204,181 @@ void CDynamixelMotor::cont_stop(void)
else
{
try{
- this->dynamixel_dev->read_word_register(current_pos,¤t_position);
- this->dynamixel_dev->write_word_register(goal_pos,current_position);
-
+ this->dynamixel_dev->read_byte_register(firmware_version,&this->info.firmware_ver);
+ this->dynamixel_dev->read_word_register(model_number,&model);
+ switch(model)
+ {
+ case 0x000C: this->info.model="AX-12A";
+ this->info.max_angle=300.0;
+ this->info.center_angle=150.0;
+ this->info.max_speed=354;
+ this->info.encoder_resolution=1023;
+ this->info.gear_ratio=254;
+ this->info.pid_control=false;
+ break;
+ case 0x012C: this->info.model="AX-12W";
+ this->info.max_angle=300.0;
+ this->info.center_angle=150.0;
+ this->info.max_speed=2830;
+ this->info.encoder_resolution=1023;
+ this->info.gear_ratio=32;
+ this->info.pid_control=false;
+ break;
+ case 0x0012: this->info.model="AX-18A";
+ this->info.max_angle=300.0;
+ this->info.center_angle=150.0;
+ this->info.max_speed=582;
+ this->info.encoder_resolution=1023;
+ this->info.gear_ratio=254;
+ this->info.pid_control=false;
+ break;
+ case 0x001D: this->info.model="MX-28";
+ this->info.max_angle=360.0;
+ this->info.center_angle=180.0;
+ this->info.max_speed=330;
+ this->info.encoder_resolution=4095;
+ this->info.gear_ratio=193;
+ this->info.pid_control=true;
+ break;
+ case 0x0018: this->info.model="RX-24F";
+ this->info.max_angle=300.0;
+ this->info.center_angle=150.0;
+ this->info.max_speed=756;
+ this->info.encoder_resolution=1023;
+ this->info.gear_ratio=193;
+ this->info.pid_control=false;
+ break;
+ case 0x001C: this->info.model="RX-28";
+ this->info.max_angle=300.0;
+ this->info.center_angle=150.0;
+ this->info.max_speed=402;
+ this->info.encoder_resolution=1023;
+ this->info.gear_ratio=193;
+ this->info.pid_control=false;
+ break;
+ case 0x0136: this->info.model="MX-64";
+ this->info.max_angle=360.0;
+ this->info.center_angle=180.0;
+ this->info.max_speed=378;
+ this->info.encoder_resolution=4095;
+ this->info.gear_ratio=200;
+ this->info.pid_control=true;
+ break;
+ case 0x0040: this->info.model="Rx-64";
+ this->info.max_angle=300.0;
+ this->info.center_angle=150.0;
+ this->info.max_speed=294;
+ this->info.encoder_resolution=1023;
+ this->info.gear_ratio=200;
+ this->info.pid_control=false;
+ break;
+ case 0x0140: this->info.model="MX-106";
+ this->info.max_angle=360.0;
+ this->info.center_angle=180.0;
+ this->info.max_speed=270;
+ this->info.encoder_resolution=4095;
+ this->info.gear_ratio=225;
+ this->info.pid_control=true;
+ break;
+ case 0x006B: this->info.model="Ex-106+";
+ this->info.max_angle=250.0;
+ this->info.center_angle=125;
+ this->info.max_speed=414;
+ this->info.encoder_resolution=4095;
+ this->info.gear_ratio=184;
+ this->info.pid_control=false;
+ break;
+ default: this->info.model="unknown";
+ break;
+ }
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
if(e.get_alarm()&this->alarms)
- this->reset_motor();
+ this->reset_motor();
throw;
}
}
}
-void CDynamixelMotor::cont_close(void)
+void CDynamixelMotor::load_config(TDynamixel_config &config)
{
- if(this->dynamixel_dev!=NULL)
- {
- this->dyn_server->free_device(this->dynamixel_dev->get_id());
- delete this->dynamixel_dev;
- this->dynamixel_dev=NULL;
- }
+ this->set_position_range(config.min_angle,config.max_angle);
+ this->set_temperature_limit(config.max_temperature);
+ this->set_voltage_limits(config.min_voltage,config.max_voltage);
+ this->set_max_torque(config.max_torque);
+ this->set_limit_torque(config.max_torque);
}
#ifdef _HAVE_XSD
-void CDynamixelMotor::cont_load_config(std::string &filename)
+void CDynamixelMotor::load_config(std::string &filename)
{
+ TDynamixel_compliance compliance;
+ TDynamixel_pid pid;
+
// try to open the specified file
try{
- std::auto_ptr<dynamixel_motor_config_t> cfg(dynamixel_motor_config(filename.c_str(),xml_schema::flags::dont_validate));
+ std::auto_ptr<dynamixel_motor_config_t> cfg(dynamixel_motor_config(filename.c_str(), xml_schema::flags::dont_validate));
// configure the parameters of the controller
+ this->set_turn_off_alarms(cfg->alarm_shtdwn());
+ this->set_position_range(cfg->min_angle(), cfg->max_angle());
this->set_temperature_limit(cfg->temp_limit());
this->set_voltage_limits(cfg->min_voltage(),cfg->max_voltage());
- if(this->info.model=="EX-106" || this->info.model=="MX-28")
- this->set_pid(cfg->cw_comp_margin(),cfg->ccw_comp_margin(),cfg->cw_comp_slope());
+ if(this->info.pid_control)
+ {
+ pid.p=cfg->kp();
+ pid.i=cfg->ki();
+ pid.d=cfg->kd();
+ this->set_pid_control(pid);
+ }
else
{
- this->set_compliance_margin(cfg->cw_comp_margin(),cfg->ccw_comp_margin());
- this->set_compliance_slope(cfg->cw_comp_slope(),cfg->ccw_comp_slope());
+ compliance.cw_compliance_margin=cfg->cw_comp_margin();
+ compliance.ccw_compliance_margin=cfg->ccw_comp_margin();
+ compliance.cw_compliance_slope=cfg->cw_comp_slope();
+ compliance.ccw_compliance_slope=cfg->ccw_comp_slope();
+ compliance.punch=cfg->punch();
+ this->set_compliance_control(compliance);
}
- this->set_punch(cfg->punch());
+ this->set_max_torque(cfg->max_torque());
+ this->set_limit_torque(cfg->max_torque());
}catch (const xml_schema::exception& e){
std::ostringstream os;
os << e;
/* handle exceptions */
- throw CMotorConfigException(_HERE_,os.str());
+ throw CDynamixelMotorException(_HERE_,os.str());
}
}
-void CDynamixelMotor::cont_save_config(std::string &filename)
+void CDynamixelMotor::save_config(std::string &filename)
{
xml_schema::namespace_infomap map;
- float max_voltage, min_voltage;
- unsigned char cw_margin=0x00, ccw_margin=0x00;
- unsigned char cw_slope=0x00, ccw_slope=0x00;
+ double max_voltage,min_voltage,max_angle,min_angle;
+ TDynamixel_compliance compliance;
+ TDynamixel_pid pid;
try{
std::ofstream output_file(filename.c_str(),std::ios_base::out);
+ this->get_position_range(&min_angle,&max_angle);
this->get_voltage_limits(&min_voltage,&max_voltage);
- if(this->info.model=="EX-106" || this->info.model=="MX-28")
- this->get_pid(&cw_margin,&ccw_margin,&cw_slope);
+ if(this->info.pid_control)
+ this->get_pid_control(pid);
else
- {
- this->get_compliance_margin(&cw_margin,&ccw_margin);
- this->get_compliance_slope(&cw_slope,&ccw_slope);
- }
- dynamixel_motor_config_t dyn_cfg(this->get_temperature_limit(),
- max_voltage,min_voltage,
- cw_margin,ccw_margin,
- cw_slope,ccw_slope,
- this->get_punch());
+ this->get_compliance_control(compliance);
+ dynamixel_motor_config_t dyn_cfg((int)this->get_turn_off_alarms(),
+ max_angle,
+ min_angle,
+ this->get_temperature_limit(),
+ max_voltage,
+ min_voltage,
+ this->get_max_torque(),
+ compliance.cw_compliance_margin,
+ compliance.ccw_compliance_margin,
+ compliance.cw_compliance_slope,
+ compliance.ccw_compliance_slope,
+ compliance.punch,
+ pid.p,
+ pid.i,
+ pid.d);
map[""].name="";
map[""].schema="dynamixel_motor_config.xsd";
dynamixel_motor_config(output_file,dyn_cfg,map);
@@ -545,41 +386,29 @@ void CDynamixelMotor::cont_save_config(std::string &filename)
std::ostringstream os;
os << e;
/* handle exceptions */
- CMotorConfigException(_HERE_,os.str());
+ throw CDynamixelMotorException(_HERE_,os.str());
}
}
#endif
-void CDynamixelMotor::reset_motor(void)
+void CDynamixelMotor::get_servo_info(TDynamixel_info &info)
{
- unsigned short int current_position;
-
- try{
- this->dynamixel_dev->read_word_register(current_pos,¤t_position);
- }catch(CDynamixelAlarmException &e){
- /* do nothing - expected exception */
- }
- try{
- this->dynamixel_dev->write_word_register(goal_pos,current_position);
- }catch(CDynamixelAlarmException &e){
- /* do nothing - expected exception */
- }
- try{
- 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 */
- }
- try{
- this->dynamixel_dev->write_byte_register(led,0x00);
- }catch(CDynamixelAlarmException &e){
- /* do nothing - expected exception */
- }
+ info.model=this->info.model;
+ info.firmware_ver=this->info.firmware_ver;
+ info.gear_ratio=this->info.gear_ratio;
+ info.encoder_resolution=this->info.encoder_resolution;
+ info.pid_control=this->info.pid_control;
+ info.max_angle=this->info.max_angle;
+ info.center_angle=this->info.center_angle;
+ info.max_speed=this->info.max_speed;
+ info.bus_id=this->info.bus_id;
+ info.baudrate=this->info.baudrate;
+ info.id=this->info.id;
}
-int CDynamixelMotor::get_baudrate(void)
+void CDynamixelMotor::set_position_range(double min, double max)
{
- unsigned char value=0;
- int baud_rate=0;
+ unsigned short int max_pos,min_pos;
if(this->dynamixel_dev==NULL)
{
@@ -588,148 +417,33 @@ int CDynamixelMotor::get_baudrate(void)
}
else
{
- try{
- this->dynamixel_dev->read_byte_register(baudrate,&value);
- baud_rate=(2000000/value)+1;
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms)
- this->reset_motor();
- throw;
- }
- }
-
- return baud_rate;
-}
-
-unsigned char CDynamixelMotor::get_node_address(void)
-{
- unsigned char address=0x00;
-
- if(this->dynamixel_dev==NULL)
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
- }
- else
- {
- try{
- this->dynamixel_dev->read_byte_register(id,&address);
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms)
- this->reset_motor();
- throw;
- }
- }
-
- return (unsigned char)address;
-}
-
-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
- {
- try{
- this->dynamixel_dev->set_id(dev_id);
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms)
- this->reset_motor();
- throw;
+ max_pos=this->from_angles(max);
+ min_pos=this->from_angles(min);
+ if(min_pos<0.0 || max_pos>(this->info.encoder_resolution))
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The desired range is outside the valid range of the servo.");
}
- }
-}
-
-std::string CDynamixelMotor::get_model(void)
-{
- unsigned short int model;
-
- if(this->dynamixel_dev==NULL)
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
- }
- else
- {
- try{
- this->dynamixel_dev->read_word_register(model_number,&model);
- switch(model)
- {
- case 0x000C: this->info.model="AX-12";
- // set maximum angle
- this->dyn_max_angle=300;
- // set maximum speed
- this->dyn_max_speed=114;
- // set resolution
- this->dyn_enc_res=1023;
- break;
- case 0x000A: this->info.model="RX-10";
- // set maximum angle
- this->dyn_max_angle=300;
- // set maximum speed
- this->dyn_max_speed=114;
- // set resolution
- this->dyn_enc_res=1023;
- break;
- case 0x001C: this->info.model="RX-28";
- // set maximum angle
- this->dyn_max_angle=300;
- // set maximum speed
- this->dyn_max_speed=114;
- // set resolution
- this->dyn_enc_res=1023;
- break;
- case 0x0040: this->info.model="RX-64";
- // set maximum angle
- this->dyn_max_angle=300;
- // set maximum speed
- this->dyn_max_speed=114;
- // set resolution
- this->dyn_enc_res=1023;
- break;
- case 0x006B: this->info.model="EX-106";
- // set maximum angle
- this->dyn_max_angle=251;
- // set maximum speed
- this->dyn_max_speed=114;
- // set resolution
- this->dyn_enc_res=4095;
- 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=4095;
- break;
- default: this->info.model="unknown";
- break;
+ else
+ {
+ try{
+ this->config.max_angle=max;
+ this->config.min_angle=min;
+ this->dynamixel_dev->write_word_register(ccw_angle_limit,max_pos);
+ this->dynamixel_dev->write_word_register(cw_angle_limit,min_pos);
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms)
+ this->reset_motor();
+ throw;
}
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms)
- this->reset_motor();
- throw;
}
}
-
- return this->info.model;
}
-int CDynamixelMotor::get_id(void)
-{
- return this->dynamixel_dev->get_id();
-}
-
-unsigned char CDynamixelMotor::get_firmware_version(void)
+void CDynamixelMotor::get_position_range(double *min, double *max)
{
+ unsigned short int angle_limit;
if(this->dynamixel_dev==NULL)
{
@@ -739,16 +453,17 @@ unsigned char CDynamixelMotor::get_firmware_version(void)
else
{
try{
- this->dynamixel_dev->read_byte_register(firmware_version,&this->info.firmware_ver);
+ this->dynamixel_dev->read_word_register(ccw_angle_limit,&angle_limit);
+ (*max)=this->to_angles(angle_limit);
+ this->dynamixel_dev->read_word_register(cw_angle_limit,&angle_limit);
+ (*min)=this->to_angles(angle_limit);
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
- if(e.get_alarm()&this->alarms)
- this->reset_motor();
+ if(e.get_alarm()&this->alarms)
+ this->reset_motor();
throw;
}
}
-
- return this->info.firmware_ver;
}
int CDynamixelMotor::get_temperature_limit(void)
@@ -767,7 +482,7 @@ int CDynamixelMotor::get_temperature_limit(void)
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
if(e.get_alarm()&this->alarms)
- this->reset_motor();
+ this->reset_motor();
throw;
}
}
@@ -777,7 +492,6 @@ int CDynamixelMotor::get_temperature_limit(void)
void CDynamixelMotor::set_temperature_limit(int limit)
{
-
if(limit<0 && limit>255)
{
/* handle exceptions */
@@ -793,42 +507,19 @@ void CDynamixelMotor::set_temperature_limit(int limit)
else
{
try{
- this->dynamixel_dev->write_byte_register(temp_limit,(unsigned char)limit);
+ this->config.max_temperature=limit;
+ this->dynamixel_dev->write_byte_register(temp_limit,(unsigned char)limit);
}catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms)
- this->reset_motor();
- throw;
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms)
+ this->reset_motor();
+ throw;
}
}
}
}
-int CDynamixelMotor::get_current_temperature(void)
-{
- unsigned char current;
-
- if(this->dynamixel_dev==NULL)
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
- }
- else
- {
- try{
- this->dynamixel_dev->read_byte_register(current_temp,¤t);
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms)
- this->reset_motor();
- throw;
- }
- }
-
- return current;
-}
-
-void CDynamixelMotor::get_voltage_limits(float *min, float *max)
+void CDynamixelMotor::get_voltage_limits(double *min, double *max)
{
unsigned char min_voltage,max_voltage;
@@ -842,18 +533,18 @@ void CDynamixelMotor::get_voltage_limits(float *min, float *max)
try{
this->dynamixel_dev->read_byte_register(low_voltage_limit,&min_voltage);
this->dynamixel_dev->read_byte_register(high_voltage_limit,&max_voltage);
- *min=((float)min_voltage/10.0);
- *max=((float)max_voltage/10.0);
+ *min=((double)min_voltage/10.0);
+ *max=((double)max_voltage/10.0);
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
if(e.get_alarm()&this->alarms)
- this->reset_motor();
+ this->reset_motor();
throw;
}
}
}
-void CDynamixelMotor::set_voltage_limits(float min, float max)
+void CDynamixelMotor::set_voltage_limits(double min, double max)
{
unsigned char min_voltage,max_voltage;
@@ -873,51 +564,29 @@ void CDynamixelMotor::set_voltage_limits(float min, float max)
{
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.");
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The desired voltage range is outside the valid range.");
}
else
{
- try{
- min_voltage=min*10;
- max_voltage=max*10;
- this->dynamixel_dev->write_byte_register(low_voltage_limit,min_voltage);
- this->dynamixel_dev->write_byte_register(high_voltage_limit,max_voltage);
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms)
- this->reset_motor();
- throw;
- }
+ try{
+ this->config.min_voltage=min;
+ this->config.max_voltage=max;
+ min_voltage=min*10;
+ max_voltage=max*10;
+ this->dynamixel_dev->write_byte_register(low_voltage_limit,min_voltage);
+ this->dynamixel_dev->write_byte_register(high_voltage_limit,max_voltage);
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms)
+ this->reset_motor();
+ throw;
+ }
}
}
}
}
-float CDynamixelMotor::get_current_voltage(void)
-{
- unsigned char voltage;
-
- if(this->dynamixel_dev==NULL)
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
- }
- else
- {
- try{
- this->dynamixel_dev->read_byte_register(current_voltage,&voltage);
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms)
- this->reset_motor();
- throw;
- }
- }
-
- return ((float)voltage/10.0);
-}
-
unsigned char CDynamixelMotor::get_led_alarms(void)
{
unsigned char led_alarms=0x00;
@@ -934,7 +603,7 @@ unsigned char CDynamixelMotor::get_led_alarms(void)
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
if(e.get_alarm()&this->alarms)
- this->reset_motor();
+ this->reset_motor();
throw;
}
}
@@ -944,7 +613,6 @@ unsigned char CDynamixelMotor::get_led_alarms(void)
void CDynamixelMotor::set_led_alarms(unsigned char alarms)
{
-
if(alarms&0x80)
{
/* handle exceptions */
@@ -960,12 +628,12 @@ void CDynamixelMotor::set_led_alarms(unsigned char alarms)
else
{
try{
- this->dynamixel_dev->write_byte_register(alarm_led,(unsigned char)alarms);
+ this->dynamixel_dev->write_byte_register(alarm_led,(unsigned char)alarms);
}catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms)
- this->reset_motor();
- throw;
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms)
+ this->reset_motor();
+ throw;
}
}
}
@@ -983,11 +651,11 @@ unsigned char CDynamixelMotor::get_turn_off_alarms(void)
else
{
try{
- this->dynamixel_dev->read_byte_register(alarm_shtdwn,&shutdown_alarms);
+ this->dynamixel_dev->read_byte_register(alarm_shtdwn, &shutdown_alarms);
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
if(e.get_alarm()&this->alarms)
- this->reset_motor();
+ this->reset_motor();
throw;
}
}
@@ -997,7 +665,6 @@ unsigned char CDynamixelMotor::get_turn_off_alarms(void)
void CDynamixelMotor::set_turn_off_alarms(unsigned char alarms)
{
-
if(alarms&0x80)
{
/* handle exceptions */
@@ -1013,19 +680,22 @@ void CDynamixelMotor::set_turn_off_alarms(unsigned char alarms)
else
{
try{
- this->dynamixel_dev->write_byte_register(alarm_shtdwn,(unsigned char)alarms);
+ this->alarms=alarms;
+ this->dynamixel_dev->write_byte_register(alarm_shtdwn,(unsigned char)alarms);
}catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms)
- this->reset_motor();
- throw;
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms)
+ this->reset_motor();
+ throw;
}
}
}
}
-void CDynamixelMotor::turn_led_on(void)
+double CDynamixelMotor::get_max_torque(void)
{
+ unsigned short int load;
+ double torque;
if(this->dynamixel_dev==NULL)
{
@@ -1035,18 +705,25 @@ void CDynamixelMotor::turn_led_on(void)
else
{
try{
- this->dynamixel_dev->write_byte_register(led,1);
+ this->dynamixel_dev->read_word_register(max_torque,&load);
+ torque=(load&0x3FF)*100.0/1023;
+ if(load>0x3FF)
+ torque=-1*torque;
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
if(e.get_alarm()&this->alarms)
- this->reset_motor();
+ this->reset_motor();
throw;
}
}
+
+ return torque;
}
-void CDynamixelMotor::turn_led_off(void)
+double CDynamixelMotor::get_limit_torque(void)
{
+ unsigned short int load;
+ double torque;
if(this->dynamixel_dev==NULL)
{
@@ -1056,18 +733,25 @@ void CDynamixelMotor::turn_led_off(void)
else
{
try{
- this->dynamixel_dev->write_byte_register(led,0);
+ this->dynamixel_dev->read_word_register(torque_limit,&load);
+ torque=(load&0x3FF)*100.0/1023;
+ if(load>0x3FF)
+ torque=-1*torque;
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
if(e.get_alarm()&this->alarms)
- this->reset_motor();
+ this->reset_motor();
throw;
}
}
+
+ return torque;
}
-void CDynamixelMotor::get_compliance_margin(unsigned char *cw_margin, unsigned char *ccw_margin)
+void CDynamixelMotor::set_max_torque(double torque_ratio)
{
+ unsigned short int load;
+
if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
@@ -1075,36 +759,30 @@ void CDynamixelMotor::get_compliance_margin(unsigned char *cw_margin, unsigned c
}
else
{
- if(this->info.model=="MX-28" || this->info.model=="EX-106")
+ if(torque_ratio<0.0 || torque_ratio>100.0)
{
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"This servo model does not support compliance margin.");
+ throw CDynamixelMotorException(_HERE_,"The desired torque ratio is outside the valid range.");
}
else
{
- 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;
- }
+ load=torque_ratio*1023/100.0;
+ try{
+ this->config.max_torque=torque_ratio;
+ this->dynamixel_dev->write_word_register(max_torque,load);
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms)
+ this->reset_motor();
+ throw;
}
}
}
}
-void CDynamixelMotor::set_compliance_margin(unsigned char cw_margin, unsigned char ccw_margin)
+void CDynamixelMotor::set_limit_torque(double torque_ratio)
{
+ unsigned short int load;
+
if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
@@ -1112,37 +790,26 @@ void CDynamixelMotor::set_compliance_margin(unsigned char cw_margin, unsigned ch
}
else
{
- if(this->info.model=="MX-28" || this->info.model=="EX-106")
+ if(torque_ratio<0.0 || torque_ratio>100.0)
{
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"This servo model does not support compliance margin.");
+ throw CDynamixelMotorException(_HERE_,"The desired torque ratio is outside the valid range.");
}
else
{
- 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.");
- }
+ load=torque_ratio*1023/100.0;
try{
- this->dynamixel_dev->write_byte_register(cw_comp_margin,cw_margin);
- this->dynamixel_dev->write_byte_register(ccw_comp_margin,ccw_margin);
+ this->dynamixel_dev->write_word_register(torque_limit,load);
}catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms)
- this->reset_motor();
- throw;
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms)
+ this->reset_motor();
+ throw;
}
}
}
}
-void CDynamixelMotor::get_compliance_slope(unsigned char *cw_slope, unsigned char *ccw_slope)
+void CDynamixelMotor::get_compliance_control(TDynamixel_compliance &config)
{
if(this->dynamixel_dev==NULL)
{
@@ -1151,35 +818,25 @@ void CDynamixelMotor::get_compliance_slope(unsigned char *cw_slope, unsigned cha
}
else
{
- if(this->info.model=="MX-28" || this->info.model=="EX-106")
+ if(!this->info.pid_control)
{
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"This servo model does not support compliance slope.");
- }
- else
- {
- 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;
- }
+ try{
+ this->dynamixel_dev->read_byte_register(cw_comp_margin,&config.cw_compliance_margin);
+ this->dynamixel_dev->read_byte_register(ccw_comp_margin,&config.ccw_compliance_margin);
+ this->dynamixel_dev->read_byte_register(cw_comp_slope,&config.cw_compliance_slope);
+ this->dynamixel_dev->read_byte_register(ccw_comp_slope,&config.ccw_compliance_slope);
+ this->dynamixel_dev->read_byte_register(punch,&config.punch);
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms)
+ this->reset_motor();
+ throw;
}
}
}
}
-void CDynamixelMotor::set_compliance_slope(unsigned char cw_slope, unsigned char ccw_slope)
+void CDynamixelMotor::set_compliance_control(TDynamixel_compliance &config)
{
if(this->dynamixel_dev==NULL)
{
@@ -1187,55 +844,116 @@ void CDynamixelMotor::set_compliance_slope(unsigned char cw_slope, unsigned char
throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
}
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.");
- }
- else
+ {
+ if(!this->info.pid_control)
{
- 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;
+ if(config.cw_compliance_margin>254)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"Invalid clockwise compliance margin.");
+ }
+ if(config.ccw_compliance_margin>254)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"Invalid counterclockwise compliance margin.");
+ }
+ if(config.cw_compliance_slope>=1 && config.cw_compliance_slope<=5) config.cw_compliance_slope=4;
+ else if(config.cw_compliance_slope>=6 && config.cw_compliance_slope<=11) config.cw_compliance_slope=8;
+ else if(config.cw_compliance_slope>=12 && config.cw_compliance_slope<=23) config.cw_compliance_slope=16;
+ else if(config.cw_compliance_slope>=24 && config.cw_compliance_slope<=47) config.cw_compliance_slope=32;
+ else if(config.cw_compliance_slope>=48 && config.cw_compliance_slope<=95) config.cw_compliance_slope=64;
+ else if(config.cw_compliance_slope>=96 && config.cw_compliance_slope<=191) config.cw_compliance_slope=128;
+ else if(config.cw_compliance_slope>=192 && config.cw_compliance_slope<=254) config.cw_compliance_slope=254;
else
{
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"Invalid clockwise compliance slope.");
+ /* 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;
+ if(config.ccw_compliance_slope>=1 && config.ccw_compliance_slope<=5) config.ccw_compliance_slope=4;
+ else if(config.ccw_compliance_slope>=6 && config.ccw_compliance_slope<=11) config.ccw_compliance_slope=8;
+ else if(config.ccw_compliance_slope>=12 && config.ccw_compliance_slope<=23) config.ccw_compliance_slope=16;
+ else if(config.ccw_compliance_slope>=24 && config.ccw_compliance_slope<=47) config.ccw_compliance_slope=32;
+ else if(config.ccw_compliance_slope>=48 && config.ccw_compliance_slope<=95) config.ccw_compliance_slope=64;
+ else if(config.ccw_compliance_slope>=96 && config.ccw_compliance_slope<=191) config.ccw_compliance_slope=128;
+ else if(config.ccw_compliance_slope>=192 && config.ccw_compliance_slope<=254) config.ccw_compliance_slope=254;
else
{
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"Invalid counterclockwise compliance slope.");
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"Invalid counterclockwise compliance slope.");
+ }
+ if(config.punch<0 || config.punch>1023)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"Invalid punch value.");
+ }
+ try{
+ this->compliance.cw_compliance_margin=config.cw_compliance_margin;
+ this->compliance.ccw_compliance_margin=config.ccw_compliance_margin;
+ this->compliance.cw_compliance_slope=config.cw_compliance_slope;
+ this->compliance.ccw_compliance_slope=config.ccw_compliance_slope;
+ this->dynamixel_dev->write_byte_register(cw_comp_margin,config.cw_compliance_margin);
+ this->dynamixel_dev->write_byte_register(ccw_comp_margin,config.ccw_compliance_margin);
+ this->dynamixel_dev->write_byte_register(cw_comp_slope,config.cw_compliance_slope);
+ this->dynamixel_dev->write_byte_register(ccw_comp_slope,config.ccw_compliance_slope);
+ this->dynamixel_dev->write_word_register(punch,config.punch);
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms)
+ this->reset_motor();
+ throw;
}
+ }
+ }
+}
+
+void CDynamixelMotor::get_pid_control(TDynamixel_pid &config)
+{
+ if(this->dynamixel_dev==NULL)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+ }
+ else
+ {
+ if(this->info.pid_control)
+ {
try{
- this->dynamixel_dev->write_byte_register(cw_comp_slope,cw_slope);
- this->dynamixel_dev->write_byte_register(ccw_comp_slope,ccw_slope);
+ this->dynamixel_dev->read_byte_register(pid_p,&config.p);
+ this->dynamixel_dev->read_byte_register(pid_i,&config.i);
+ this->dynamixel_dev->read_byte_register(pid_d,&config.d);
}catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms)
- this->reset_motor();
- throw;
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms)
+ this->reset_motor();
+ throw;
}
}
- }
+ }
}
-short int CDynamixelMotor::get_punch(void)
+void CDynamixelMotor::set_pid_control(TDynamixel_pid &config)
{
- unsigned short int punch_value;
+ if(this->dynamixel_dev==NULL)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+ }
+ else
+ {
+ if(this->info.pid_control)
+ {
+ this->pid.p=config.p;
+ this->pid.i=config.i;
+ this->pid.d=config.d;
+ this->dynamixel_dev->write_byte_register(pid_p,config.p);
+ this->dynamixel_dev->write_byte_register(pid_i,config.i);
+ this->dynamixel_dev->write_byte_register(pid_d,config.d);
+ }
+ }
+}
+void CDynamixelMotor::turn_led_on(void)
+{
if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
@@ -1244,19 +962,17 @@ short int CDynamixelMotor::get_punch(void)
else
{
try{
- this->dynamixel_dev->read_word_register(punch,&punch_value);
+ this->dynamixel_dev->write_byte_register(led,1);
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
if(e.get_alarm()&this->alarms)
- this->reset_motor();
+ this->reset_motor();
throw;
}
}
-
- return (short int)punch_value;
}
-void CDynamixelMotor::set_punch(short int punch_value)
+void CDynamixelMotor::turn_led_off(void)
{
if(this->dynamixel_dev==NULL)
{
@@ -1265,23 +981,28 @@ void CDynamixelMotor::set_punch(short int punch_value)
}
else
{
- if(punch<0 || punch>1023)
- {
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,"Invalid punch value.");
- }
try{
- this->dynamixel_dev->write_word_register(punch,punch_value);
+ this->dynamixel_dev->write_byte_register(led,0);
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
if(e.get_alarm()&this->alarms)
- this->reset_motor();
+ this->reset_motor();
throw;
}
}
}
-void CDynamixelMotor::get_pid(unsigned char *p,unsigned char *i,unsigned char *d)
+bool CDynamixelMotor::is_locked(void)
+{
+ return false;
+}
+
+void CDynamixelMotor::lock(void)
+{
+
+}
+
+void CDynamixelMotor::enable(void)
{
if(this->dynamixel_dev==NULL)
{
@@ -1290,21 +1011,18 @@ void CDynamixelMotor::get_pid(unsigned char *p,unsigned char *i,unsigned char *d
}
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.");
+ try{
+ this->dynamixel_dev->write_byte_register(torque_en,1);
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms)
+ this->reset_motor();
+ throw;
}
}
}
-void CDynamixelMotor::set_pid(unsigned char p,unsigned char i,unsigned char d)
+void CDynamixelMotor::disable(void)
{
if(this->dynamixel_dev==NULL)
{
@@ -1313,33 +1031,79 @@ void CDynamixelMotor::set_pid(unsigned char p,unsigned char i,unsigned char d)
}
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.");
+ try{
+ this->dynamixel_dev->write_byte_register(torque_en,0);
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms)
+ this->reset_motor();
+ throw;
}
}
}
-bool CDynamixelMotor::is_locked(void)
+void CDynamixelMotor::move_absolute_angle(double angle,double speed)
{
- return false;
+ unsigned short int cmd[2];
+
+ if(this->dynamixel_dev==NULL)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+ }
+ else
+ {
+ try{
+ if(this->mode==torque_ctrl)
+ {
+ this->set_position_range(-this->info.center_angle, this->info.max_angle - this->info.center_angle);
+ this->mode=angle_ctrl;
+ }
+ cmd[0]=this->from_angles(angle);
+ cmd[1]=this->from_speeds(speed);
+ this->dynamixel_dev->write_registers(goal_pos,(unsigned char *)cmd,4);
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms)
+ this->reset_motor();
+ throw;
+ }
+ }
}
-void CDynamixelMotor::lock(void)
+void CDynamixelMotor::move_relative_angle(double angle,double speed)
{
+ unsigned short int cmd[2],pos;
+ if(this->dynamixel_dev==NULL)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+ }
+ else
+ {
+ try{
+ if(this->mode==torque_ctrl)
+ {
+ this->set_position_range(-this->info.center_angle,this->info.max_angle-this->info.center_angle);
+ this->mode=angle_ctrl;
+ }
+ this->dynamixel_dev->read_word_register(current_pos,&pos);
+ cmd[0]=this->from_angles(angle+this->to_angles(pos));
+ cmd[1]=this->from_speeds(speed);
+ this->dynamixel_dev->write_registers(goal_pos,(unsigned char *)cmd,4);
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms)
+ this->reset_motor();
+ throw;
+ }
+ }
}
-void CDynamixelMotor::enable_torque_control(void)
+void CDynamixelMotor::move_torque(double torque_ratio)
{
- std::vector<float> min,max;
+ unsigned short int torque=0;
if(this->dynamixel_dev==NULL)
{
@@ -1348,17 +1112,28 @@ void CDynamixelMotor::enable_torque_control(void)
}
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;
+ try{
+ if(this->mode==angle_ctrl)
+ {
+ this->set_position_range(-this->info.center_angle,-this->info.center_angle);
+ this->mode=torque_ctrl;
+ }
+ 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);
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms)
+ this->reset_motor();
+ throw;
+ }
}
}
-
-void CDynamixelMotor::disable_torque_control(void)
+
+void CDynamixelMotor::stop(void)
{
- std::vector<float> min,max;
+ unsigned short int current_position;
if(this->dynamixel_dev==NULL)
{
@@ -1367,23 +1142,53 @@ void CDynamixelMotor::disable_torque_control(void)
}
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;
+ try{
+ if(this->mode==angle_ctrl)
+ {
+ this->dynamixel_dev->read_word_register(current_pos,¤t_position);
+ this->dynamixel_dev->write_word_register(goal_pos,current_position);
+ }
+ else
+ this->dynamixel_dev->write_word_register(goal_speed,0);
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms)
+ this->reset_motor();
+ throw;
+ }
}
}
-
-bool CDynamixelMotor::is_torque_control_enabled(void)
+
+double CDynamixelMotor::get_current_angle(void)
{
- return this->torque_control;
+ unsigned short int data;
+ double current_position;
+
+ if(this->dynamixel_dev==NULL)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+ }
+ else
+ {
+ try{
+ this->dynamixel_dev->read_word_register(current_pos,&data);
+ current_position = this->to_angles(data);
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms)
+ this->reset_motor();
+ throw;
+ }
+ }
+
+ return current_position;
}
-void CDynamixelMotor::set_torque(float torque_ratio)
+double CDynamixelMotor::get_current_speed(void)
{
- unsigned short int torque=0x0000;
+ unsigned short int data;
+ double c_speed;
if(this->dynamixel_dev==NULL)
{
@@ -1392,45 +1197,24 @@ void CDynamixelMotor::set_torque(float torque_ratio)
}
else
{
- if(this->torque_control)
- {
- 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
- {
- 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);
- }
+ try{
+ this->dynamixel_dev->read_word_register(current_speed,&data);
+ c_speed = this->to_speeds(data);
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms)
+ this->reset_motor();
+ throw;
}
}
+
+ return c_speed;
}
-float CDynamixelMotor::get_torque(void)
+double CDynamixelMotor::get_current_temperature(void)
{
- unsigned short int load;
- float torque;
+ unsigned char data;
+ double c_temp;
if(this->dynamixel_dev==NULL)
{
@@ -1439,19 +1223,24 @@ float CDynamixelMotor::get_torque(void)
}
else
{
- this->dynamixel_dev->read_word_register(current_load,&load);
- torque=(load&0x3FF)*100.0/1023;
- if(load>0x3FF)
- torque=-1*torque;
+ try{
+ this->dynamixel_dev->read_byte_register(current_temp,&data);
+ c_temp = (double)data;
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms)
+ this->reset_motor();
+ throw;
+ }
}
- return torque;
+ return c_temp;
}
-float CDynamixelMotor::get_max_torque(void)
+double CDynamixelMotor::get_current_voltage(void)
{
- unsigned short int load;
- float torque;
+ unsigned char data;
+ double c_voltage;
if(this->dynamixel_dev==NULL)
{
@@ -1460,19 +1249,24 @@ float CDynamixelMotor::get_max_torque(void)
}
else
{
- this->dynamixel_dev->read_word_register(max_torque,&load);
- torque=(load&0x3FF)*100.0/1023;
- if(load>0x3FF)
- torque=-1*torque;
+ try{
+ this->dynamixel_dev->read_byte_register(current_voltage,&data);
+ c_voltage = (double)data/10;
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms)
+ this->reset_motor();
+ throw;
+ }
}
- return torque;
+ return c_voltage;
}
-float CDynamixelMotor::get_limit_torque(void)
+double CDynamixelMotor::get_current_effort(void)
{
- unsigned short int load;
- float torque;
+ unsigned char data;
+ double c_effort;
if(this->dynamixel_dev==NULL)
{
@@ -1481,29 +1275,59 @@ float CDynamixelMotor::get_limit_torque(void)
}
else
{
- this->dynamixel_dev->read_word_register(torque_limit,&load);
- torque=(load&0x3FF)*100.0/1023;
- if(load>0x3FF)
- torque=-1*torque;
+ try{
+ this->dynamixel_dev->read_byte_register(current_load,&data);
+ c_effort = (double)((data&0x3FF)*100.0/1023);
+ if (this->get_current_speed() < 0)
+ {
+ c_effort *= -1.0;
+ }
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms)
+ this->reset_motor();
+ throw;
+ }
}
- return torque;
+ return c_effort;
}
CDynamixelMotor::~CDynamixelMotor()
{
- this->close();
- this->info.model="";
- this->info.firmware_ver=0x00;
- this->control.cw_compliance_margin=0x00;
- this->control.ccw_compliance_margin=0x00;
- this->control.cw_compliance_slope=0x20;
- this->control.ccw_compliance_slope=0x20;
+ /* stop the motor */
+ this->stop();
+ /* disable the motor */
+ this->disable();
if(this->dynamixel_dev!=NULL)
{
this->dyn_server->free_device(this->dynamixel_dev->get_id());
delete this->dynamixel_dev;
this->dynamixel_dev=NULL;
}
+ this->info.model="";
+ this->info.firmware_ver=(unsigned char)-1;
+ this->info.gear_ratio=(unsigned int)-1;
+ this->info.encoder_resolution=(unsigned int)-1;
+ this->info.pid_control=false;
+ this->info.max_angle=-1;
+ this->info.center_angle=-1;
+ this->info.max_speed=-1;
+ this->info.bus_id="";
+ this->info.baudrate=(unsigned int)-1;
+ this->info.id=(unsigned char)-1;
+ this->compliance.cw_compliance_margin=0x00;
+ this->compliance.ccw_compliance_margin=0x00;
+ this->compliance.cw_compliance_slope=0x20;
+ this->compliance.ccw_compliance_slope=0x20;
+ this->pid.p=0x00;
+ this->pid.i=0x00;
+ this->pid.d=0x00;
+ this->config.max_angle=0.0;
+ this->config.min_angle=0.0;
+ this->config.max_temperature=0.0;
+ this->config.max_voltage=0.0;
+ this->config.min_voltage=0.0;
+ this->config.max_torque=0.0;
}
diff --git a/src/dynamixel_motor.h b/src/dynamixel_motor.h
index 1245965a730db8226a2790ac9273ae95e2f10411..3d315201ce00e4af5b476f5c409d1824ba11d721 100644
--- a/src/dynamixel_motor.h
+++ b/src/dynamixel_motor.h
@@ -1,439 +1,395 @@
-#ifndef _DYNAMIXEL_MOTOR_H
-#define _DYNAMIXEL_MOTOR_H
-
-#include "dynamixelserver.h"
-#include "motor_control.h"
-#include "eventserver.h"
-#include "ftdimodule.h"
-#include "mutex.h"
-#include <stdio.h>
-#include <stdlib.h>
-#include <string>
-#include <vector>
-
-typedef enum {
- model_number=0x00,
- firmware_version=0x02,
- id=0x03,
- baudrate=0x04,
- return_delay_time=0x05,
- cw_angle_limit=0x06,
- ccw_angle_limit=0x08,
- temp_limit=0x0B,
- low_voltage_limit=0x0C,
- high_voltage_limit=0x0D,
- max_torque=0x0E,
- return_level=0x10,
- alarm_led=0x11,
- alarm_shtdwn=0x12,
- down_cal=0x14,
- 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,
- ccw_comp_slope=0x1D,
- goal_pos=0x1E,
- goal_speed=0x20,
- torque_limit=0x22,
- current_pos=0x24,
- current_speed=0x26,
- current_load=0x28,
- current_voltage=0x2A,
- current_temp=0x2B,
- reg_inst=0x2C,
- moving=0x2E,
- lock=0x2F,
- punch=0x30
-} reg_id;
-
-typedef enum {
- input_voltage_error=0x01,
- angle_limit_error=0x02,
- overheating_error=0x04,
- range_error=0x08,
- checksum_error=0x10,
- overload_error=0x20,
- instruction_error=0x40
-}dyn_alarm;
-
-typedef struct
-{
- std::string model;
- unsigned char firmware_ver;
-}TDynamixel_info;
-
-typedef struct
-{
- unsigned char cw_compliance_margin;
- unsigned char ccw_compliance_margin;
- unsigned char cw_compliance_slope;
- unsigned char ccw_compliance_slope;
- unsigned char punch;
-}TDynamixel_control;
-
-/**
- * \brief
- *
- */
-class CDynamixelMotor : public CMotorControl
-{
- private:
- CDynamixelServer *dyn_server;
- /**
- * \brief
- *
- */
- CDynamixel *dynamixel_dev;
- /**
- * \brief
- *
- */
- TDynamixel_control control;
- /**
- * \brief
- *
- */
- TDynamixel_info info;
- /**
- * \brief
- *
- */
- unsigned char alarms;
- /**
- * \brief
- *
- */
- bool torque_control;
- /**
- * \brief
- *
- */
- long int current_position;
- // model attributes
- /**
- * \brief
- *
- */
- int dyn_enc_res;
- /**
- * \brief
- *
- */
- int dyn_max_angle;
- /**
- * \brief
- *
- */
- int dyn_max_speed;
- /**
- * \brief
- *
- */
- int dyn_max_torque;
- protected:
- /**
- * \brief
- *
- */
- virtual void angles_to_controller(std::vector<float>& angles,std::vector<float>& counts);
- /**
- * \brief
- *
- */
- virtual void speeds_to_controller(std::vector<float>& angles,std::vector<float>& counts);
- /**
- * \brief
- *
- */
- virtual void accels_to_controller(std::vector<float>& angles,std::vector<float>& counts);
- /**
- * \brief
- *
- */
- virtual void controller_to_angles(std::vector<float>& counts,std::vector<float>& angles);
- /**
- * \brief
- *
- */
- virtual void controller_to_speeds(std::vector<float>& counts,std::vector<float>& angles);
- /**
- * \brief
- *
- */
- virtual void controller_to_accels(std::vector<float>& counts,std::vector<float>& angles);
- /**
- * \brief
- *
- */
- virtual void cont_set_position_range(std::vector<float>& min, std::vector<float>& max);
- /**
- * \brief
- *
- */
- virtual void cont_get_position_range(std::vector<float>& min, std::vector<float>& max);
- /**
- * \brief
- *
- */
- virtual void cont_get_gear_factor(std::vector<float>& gear);
- /**
- * \brief
- *
- */
- virtual std::vector<float> cont_get_position(void);
- /**
- * \brief
- *
- */
- virtual std::vector<float> cont_get_velocity(void);
- /**
- * \brief
- *
- */
- virtual void cont_enable(std::vector<bool> &enable);
- /**
- * \brief
- *
- */
- virtual void cont_disable(std::vector<bool> &disable);
- /**
- * \brief
- *
- */
- virtual void cont_load(std::vector<float>& position, std::vector<float>& velocity, std::vector<float>& accel);
- /**
- * \brief
- *
- */
- virtual void cont_load(std::vector<float>& velocity, std::vector<float>& accel);
- /**
- * \brief
- *
- */
- virtual void cont_move(void);
- /**
- * \brief
- *
- */
- virtual void cont_stop(void);
- /**
- * \brief
- *
- */
- virtual void cont_close(void);
-#ifdef _HAVE_XSD
- /**
- * \brief
- *
- */
- virtual void cont_load_config(std::string &filename);
- /**
- * \brief
- *
- */
- virtual void cont_save_config(std::string &filename);
-#endif
- /**
- * \brief
- *
- */
- void reset_motor(void);
- public:
- /**
- * \brief
- *
- */
- CDynamixelMotor(std::string& cont_id,unsigned char bus_id,int baudrate,unsigned char dev_id);
- /**
- * \brief
- *
- */
- CDynamixelMotor(std::string& cont_id,std::string &bus_id,int baudrate,unsigned char dev_id);
- /**
- * \brief
- *
- */
- int get_baudrate(void);
- /**
- * \brief
- *
- */
- unsigned char get_node_address(void);
- /**
- * \brief
- *
- */
- void set_node_address(unsigned char dev_id);
- /**
- * \brief
- *
- */
- std::string get_model(void);
- /**
- * \brief
- *
- */
- int get_id(void);
- /**
- * \brief
- *
- */
- unsigned char get_firmware_version(void);
- /**
- * \brief
- *
- */
- int get_temperature_limit(void);
- /**
- * \brief
- *
- */
- void set_temperature_limit(int limit);
- /**
- * \brief
- *
- */
- int get_current_temperature(void);
- /**
- * \brief
- *
- */
- void get_voltage_limits(float *min, float *max);
- /**
- * \brief
- *
- */
- void set_voltage_limits(float min, float max);
- /**
- * \brief
- *
- */
- float get_current_voltage(void);
- /**
- * \brief
- *
- */
- unsigned char get_led_alarms(void);
- /**
- * \brief
- *
- */
- void set_led_alarms(unsigned char alarms);
- /**
- * \brief
- *
- */
- unsigned char get_turn_off_alarms(void);
- /**
- * \brief
- *
- */
- void set_turn_off_alarms(unsigned char alarms);
- /**
- * \brief
- *
- */
- void turn_led_on(void);
- /**
- * \brief
- *
- */
- void turn_led_off(void);
- /**
- * \brief
- *
- */
- void get_compliance_margin(unsigned char *cw_margin, unsigned char *ccw_margin);
- /**
- * \brief
- *
- */
- void set_compliance_margin(unsigned char cw_margin, unsigned char ccw_margin);
- /**
- * \brief
- *
- */
- void get_compliance_slope(unsigned char *cw_slope, unsigned char *ccw_margin);
- /**
- * \brief
- *
- */
- void set_compliance_slope(unsigned char cw_slope, unsigned char ccw_slope);
- /**
- * \brief
- *
- */
- short int get_punch(void);
- /**
- * \brief
- *
- */
- 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
- *
- */
- bool is_locked(void);
- /**
- * \brief
- *
- */
- void lock(void);
- /**
- * \brief
- *
- */
- void enable_torque_control(void);
- /**
- * \brief
- *
- */
- void disable_torque_control(void);
- /**
- * \brief
- *
- */
- bool is_torque_control_enabled(void);
- /**
- * \brief
- *
- */
- void set_torque(float torque_ratio);
- /**
- * \brief
- *
- */
- float get_torque(void);
- /**
- * \brief
- *
- */
- float get_max_torque(void);
- /**
- * \brief
- *
- */
- float get_limit_torque(void);
- /**
- * \brief
- *
- */
- virtual ~CDynamixelMotor();
-};
-
-#endif
+#ifndef _DYNAMIXEL_MOTOR_H
+#define _DYNAMIXEL_MOTOR_H
+
+#include "dynamixelserver.h"
+#include "threadserver.h"
+#include "eventserver.h"
+#include "ftdimodule.h"
+#include "mutex.h"
+#include <stdio.h>
+#include <stdlib.h>
+#include <string>
+#include <vector>
+#include <memory>
+
+typedef enum {
+ // [Access] [Description]
+ // EEPROM Area
+ model_number=0x00, // (R) Model Number
+ firmware_version=0x02, // (R) Version of the Firmware
+ id=0x03, // (RW) ID of Dynamixel
+ baudrate=0x04, // (RW) Baud Rate of Dynamixel
+ return_delay_time=0x05, // (RW) Return Delay Time
+ cw_angle_limit=0x06, // (RW) Clockwise Angle Limit
+ ccw_angle_limit=0x08, // (RW) Counterclockwise Angle Limit
+ temp_limit=0x0B, // (RW) Internal Temperature Limit
+ low_voltage_limit=0x0C, // (RW) Lowest Voltage Limit
+ high_voltage_limit=0x0D, // (RW) Highest Voltage Limit
+ max_torque=0x0E, // (RW) Maximum Torque
+ return_level=0x10, // (RW) Status Return Level
+ alarm_led=0x11, // (RW) LED for Alarm
+ alarm_shtdwn=0x12, // (RW) Shutdown for Alarm
+ down_cal=0x14, // (RW)
+ up_cal=0x16, // (RW)
+ // RAM Area
+ torque_en=0x18, // (RW) Torque On/Off
+ led=0x19, // (RW) LED On/Off
+ pid_p=0x1A, // (RW)
+ pid_i=0x1B, // (RW)
+ pid_d=0x1C, // (RW)
+ cw_comp_margin=0x1A, // (RW) CW Compliance Margin
+ ccw_comp_margin=0x1B, // (RW) CCW Compliance Margin
+ cw_comp_slope=0x1C, // (RW) CW Compliance Slope
+ ccw_comp_slope=0x1D, // (RW) CCW Compliance Slope
+ goal_pos=0x1E, // (RW) Goal Position
+ goal_speed=0x20, // (RW) Moving Speed
+ torque_limit=0x22, // (RW) Torque Limit
+ current_pos=0x24, // (R) Current Position
+ current_speed=0x26, // (R) Current Speed
+ current_load=0x28, // (R) Current Load
+ current_voltage=0x2A, // (R) Current Voltage
+ current_temp=0x2B, // (RW) Current Temperature
+ reg_inst=0x2C, // (RW) Means if Instruction is registered
+ moving=0x2E, // (R) Means if there is any movement
+ lock=0x2F, // (RW) Locking EEPROM
+ punch=0x30 // (RW) Punch
+} reg_id;
+
+typedef enum
+{
+ input_voltage_error=0x01,
+ angle_limit_error=0x02,
+ overheating_error=0x04,
+ range_error=0x08,
+ checksum_error=0x10,
+ overload_error=0x20,
+ instruction_error=0x40
+}dyn_alarm;
+
+typedef struct
+{
+ std::string model;
+ unsigned char firmware_ver;
+ unsigned int gear_ratio;
+ unsigned int encoder_resolution;
+ bool pid_control;
+ double max_angle;
+ double center_angle;
+ double max_speed;
+ std::string bus_id;
+ unsigned int baudrate;
+ unsigned char id;
+}TDynamixel_info;
+
+typedef struct
+{
+ unsigned char cw_compliance_margin;
+ unsigned char ccw_compliance_margin;
+ unsigned char cw_compliance_slope;
+ unsigned char ccw_compliance_slope;
+ unsigned char punch;
+}TDynamixel_compliance;
+
+typedef struct
+{
+ unsigned char p;
+ unsigned char i;
+ unsigned char d;
+}TDynamixel_pid;
+
+typedef struct
+{
+ double max_angle;
+ double min_angle;
+ double max_temperature;
+ double max_voltage;
+ double min_voltage;
+ double max_torque;
+}TDynamixel_config;
+
+typedef enum{angle_ctrl=0,torque_ctrl=1} control_mode;
+
+/**
+ * \brief
+ *
+ */
+class CDynamixelMotor
+{
+ private:
+ CDynamixelServer *dyn_server;
+ /**
+ * \brief
+ *
+ */
+ CDynamixel *dynamixel_dev;
+ /**
+ * \brief
+ *
+ */
+ TDynamixel_compliance compliance;
+ /**
+ * \brief
+ *
+ */
+ TDynamixel_pid pid;
+ /**
+ * \brief
+ *
+ */
+ TDynamixel_config config;
+ /**
+ * \brief
+ *
+ */
+ TDynamixel_info info;
+ /**
+ * \brief
+ *
+ */
+ unsigned char alarms;
+ /**
+ * \brief
+ *
+ */
+ control_mode mode;
+
+ protected:
+ /**
+ * \brief
+ *
+ */
+ unsigned int from_angles(double angle);
+ /**
+ * \brief
+ *
+ */
+ unsigned int from_speeds(double speed);
+ /**
+ * \brief
+ *
+ */
+ double to_angles(unsigned short int counts);
+ /**
+ * \brief
+ *
+ */
+ double to_speeds(unsigned short int counts);
+ /**
+ * \brief
+ *
+ */
+ void reset_motor(void);
+ /**
+ * \brief
+ *
+ */
+ void get_model(void);
+
+ public:
+ /**
+ * \brief
+ *
+ */
+ CDynamixelMotor(std::string& cont_id,unsigned char bus_id,int baudrate,unsigned char dev_id);
+ /**
+ * \brief
+ *
+ */
+ CDynamixelMotor(std::string& cont_id,std::string &bus_id,int baudrate,unsigned char dev_id);
+ /**
+ * \brief
+ *
+ */
+ void get_servo_info(TDynamixel_info &info);
+ /* configuration functions */
+ /**
+ * \brief
+ *
+ */
+ void load_config(TDynamixel_config &config);
+#ifdef _HAVE_XSD
+ /**
+ * \brief
+ *
+ */
+ void load_config(std::string &filename);
+ /**
+ * \brief
+ *
+ */
+ void save_config(std::string &filename);
+#endif
+ /**
+ * \brief
+ *
+ */
+ void set_position_range(double min,double max);
+ /**
+ * \brief
+ *
+ */
+ void get_position_range(double *min, double *max);
+ /**
+ * \brief
+ *
+ */
+ int get_temperature_limit(void);
+ /**
+ * \brief
+ *
+ */
+ void set_temperature_limit(int limit);
+ /**
+ * \brief
+ *
+ */
+ void get_voltage_limits(double *min, double *max);
+ /**
+ * \brief
+ *
+ */
+ void set_voltage_limits(double min, double max);
+ /**
+ * \brief
+ *
+ */
+ unsigned char get_led_alarms(void);
+ /**
+ * \brief
+ *
+ */
+ void set_led_alarms(unsigned char alarms);
+ /**
+ * \brief
+ *
+ */
+ unsigned char get_turn_off_alarms(void);
+ /**
+ * \brief
+ *
+ */
+ void set_turn_off_alarms(unsigned char alarms);
+ /**
+ * \brief
+ *
+ */
+ double get_max_torque(void);
+ /**
+ * \brief
+ *
+ */
+ double get_limit_torque(void);
+ /**
+ * \brief
+ *
+ */
+ void set_max_torque(double torque_ratio);
+ /**
+ * \brief
+ *
+ */
+ void set_limit_torque(double torque_ratio);
+ /**
+ * \brief
+ *
+ */
+ void get_compliance_control(TDynamixel_compliance &config);
+ /**
+ * \brief
+ *
+ */
+ void set_compliance_control(TDynamixel_compliance &config);
+ /**
+ * \brief
+ *
+ */
+ void get_pid_control(TDynamixel_pid &config);
+ /**
+ * \brief
+ *
+ */
+ void set_pid_control(TDynamixel_pid &config);
+ /* control functions */
+ /**
+ * \brief
+ *
+ */
+ void turn_led_on(void);
+ /**
+ * \brief
+ *
+ */
+ void turn_led_off(void);
+ /**
+ * \brief
+ *
+ */
+ bool is_locked(void);
+ /**
+ * \brief
+ *
+ */
+ void lock(void);
+ /**
+ * \brief
+ *
+ */
+ void enable(void);
+ /**
+ * \brief
+ *
+ */
+ void disable(void);
+ /**
+ * \brief
+ *
+ */
+ void move_absolute_angle(double angle,double speed);
+ /**
+ * \brief
+ *
+ */
+ void move_relative_angle(double angle,double speed);
+ /**
+ * \brief
+ *
+ */
+ void move_torque(double torque_ratio);
+ /**
+ * \brief
+ *
+ */
+ void stop(void);
+ /**
+ * \brief
+ *
+ */
+ double get_current_angle(void);
+ /**
+ * \brief
+ *
+ */
+ double get_current_speed(void);
+ /**
+ * \brief
+ *
+ */
+ double get_current_effort(void);
+ /**
+ * \brief
+ *
+ */
+ double get_current_voltage(void);
+ /**
+ * \brief
+ *
+ */
+ double get_current_temperature(void);
+ /**
+ * \brief
+ *
+ */
+ virtual ~CDynamixelMotor();
+};
+
+#endif
diff --git a/src/dynamixel_motor_exceptions.cpp b/src/dynamixel_motor_exceptions.cpp
index 60fab00e51a23b8c7521a8221dabb2318eba2065..00402baa89c395af55945aa47a0f5712c6aa7aaa 100755
--- a/src/dynamixel_motor_exceptions.cpp
+++ b/src/dynamixel_motor_exceptions.cpp
@@ -5,6 +5,7 @@
const std::string dynamixel_motor_error_message="DynamixelMotor error - ";
const std::string dynamixel_motor_group_error_message="DynamixelMotorGroup error - ";
+const std::string dynamixel_motion_sequence_error_message="DynamixelMotionSeq error - ";
CDynamixelMotorException::CDynamixelMotorException(const std::string& where,const std::string &error_msg):CException(where,dynamixel_motor_error_message)
{
@@ -20,3 +21,9 @@ CDynamixelMotorGroupException::CDynamixelMotorGroupException(const std::string&
this->error_msg+=error_msg;
}
+CDynamixelMotionSequenceException::CDynamixelMotionSequenceException(const std::string& where,const std::string &error_msg):CException(where,dynamixel_motion_sequence_error_message)
+{
+ std::stringstream text;
+
+ this->error_msg+=error_msg;
+}
diff --git a/src/dynamixel_motor_exceptions.h b/src/dynamixel_motor_exceptions.h
index 0ee478e56e7097f0c5f7e74e5e1c044fc8577cfb..25487eedb5b253853b257fb4eafc4f371757ce67 100755
--- a/src/dynamixel_motor_exceptions.h
+++ b/src/dynamixel_motor_exceptions.h
@@ -33,4 +33,16 @@ class CDynamixelMotorGroupException : public CException
CDynamixelMotorGroupException(const std::string& where,const std::string &error_msg);
};
+/**
+ * \brief
+ */
+class CDynamixelMotionSequenceException : public CException
+{
+ public:
+ /**
+ * \brief
+ *
+ */
+ CDynamixelMotionSequenceException(const std::string& where,const std::string &error_msg);
+};
#endif
diff --git a/src/dynamixel_motor_group.cpp b/src/dynamixel_motor_group.cpp
index 98fb2efb0cdc7bb394efabc762a971ee76e31166..d383bd2cd826ca2679476bf1ae019feb442d36b8 100644
--- a/src/dynamixel_motor_group.cpp
+++ b/src/dynamixel_motor_group.cpp
@@ -1,4 +1,5 @@
#include "dynamixel_motor_exceptions.h"
+#include "dynamixelexceptions.h"
#include "dynamixelserver.h"
#include "eventexceptions.h"
#include "dynamixel_motor_group.h"
@@ -6,8 +7,14 @@
#include "ftdiserver.h"
#include <math.h>
#include <iostream>
+#include <fstream>
+#ifdef _HAVE_XSD
+#include "xml/dynamixel_motor_cfg_file.hxx"
+#include "xml/dynamixel_motor_group_cfg_file.hxx"
+#include "xml/dyn_motion_seq_file.hxx"
+#endif
-CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id):CMotorGroup(group_id)
+CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id)
{
if(group_id.size()==0)
{
@@ -16,122 +23,1799 @@ CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id):CMotorGroup(gr
}
else
{
+ this->group_id=group_id;
this->dyn_server=CDynamixelServer::instance();
+ this->clear();
+ this->mode=angle_ctrl;
+ /* initialize motion sequence attributes */
+ this->sequence_state=mtn_empty;
+ this->sequence_current_step=-1;
+ this->sequence_error_msg="Motion sequence ended successfully";
+ this->init_events();
+ this->init_threads();
}
}
-void CDynamixelMotorGroup::move(std::vector<float> &position,std::vector<float> &velocity,std::vector<float> &acceleration)
+CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id,unsigned bus_id,int baudrate,std::vector<unsigned char> &ids)
{
- std::vector<float> pos_count,vel_count,accel_count,tmp_pos;
- std::vector< std::vector<unsigned char> > data;
- std::vector<short int> count_value;
- std::vector<bool> absolute;
unsigned int i=0;
- if(position.size()!=this->servo_id.size())
+ if(group_id.size()==0)
{
- /* handle errors */
- throw CDynamixelMotorGroupException(_HERE_,"The number of position commands does not coincide with the total number of motors in the group");
+ /* handle exceptions */
+ throw CDynamixelMotorGroupException(_HERE_,"Invalid group identifier - empty string");
}
- else if(velocity.size()!=this->servo_id.size())
+ else
{
- /* handle exceptions */
- throw CDynamixelMotorGroupException(_HERE_,"The number of velocity commands does not coincide with the total number of motors in the group");
+ this->group_id=group_id;
+ this->dyn_server=CDynamixelServer::instance();
+ this->clear();
+ this->mode=angle_ctrl;
+ /* initialize motion sequence attributes */
+ this->sequence_state=mtn_empty;
+ this->sequence_current_step=-1;
+ this->sequence_error_msg="Motion sequence ended successfully";
+ if(ids.size()==0)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorGroupException(_HERE_,"Impossible to create an empty group");
+ }
+ try{
+ this->dyn_server->config_bus(bus_id,baudrate);
+ for(i=0;i<ids.size();i++)
+ this->init_motor(ids[i]);
+ if(this->config[0].min_angle==-this->info[0].center_angle && this->config[0].max_angle==-this->info[0].center_angle)
+ {
+ this->mode=torque_ctrl;
+ for(i=1;i<ids.size();i++)
+ this->set_position_range(i,-this->info[i].center_angle,-this->info[i].center_angle);
+ }
+ else
+ {
+ this->mode=angle_ctrl;
+ for(i=1;i<ids.size();i++)
+ this->set_position_range(i,-this->info[i].center_angle,this->info[i].max_angle-this->info[i].center_angle);
+ }
+ this->init_events();
+ this->init_threads();
+ }catch(CException &e){
+ /* handle exceptions */
+ this->clear();
+ throw;
+ }
}
- else if(acceleration.size()!=this->servo_id.size())
+}
+
+CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id,std::string &bus_id,int baudrate,std::vector<unsigned char> &ids)
+{
+ unsigned int i=0;
+
+ if(group_id.size()==0)
{
/* handle exceptions */
- throw CDynamixelMotorGroupException(_HERE_,"The number of acceleration commands does not coincide with the total number of motors in the group");
+ throw CDynamixelMotorGroupException(_HERE_,"Invalid group identifier - empty string");
}
- else
+ else
{
+ this->group_id=group_id;
+ this->dyn_server=CDynamixelServer::instance();
+ this->clear();
+ if(ids.size()==0)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorGroupException(_HERE_,"Impossible to create an empty group");
+ }
try{
- // call the base class function
- CMotorGroup::move(position,velocity,acceleration);
- 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(4);
- }
- absolute=this->is_motion_absolute();
- for(i=0;i<servo_id.size();i++)
- {
- if(!absolute[i])
- tmp_pos.push_back(position[i]+this->current_pos[i]);
- else
- tmp_pos.push_back(position[i]);
- }
- this->current_pos=tmp_pos;
- // convert the input data
- this->angles_to_controller(tmp_pos,pos_count);
- this->speeds_to_controller(velocity,vel_count);
- this->accels_to_controller(acceleration,accel_count);
- for(i=0;i<servo_id.size();i++)
- {
- data[i][0]=((int)(pos_count[i])%256);
- data[i][1]=(int)(pos_count[i]/256);
- data[i][2]=((int)(vel_count[i])%256);
- data[i][3]=(int)(vel_count[i]/256);
- }
- dyn_server->write_sync(servo_id,goal_pos,data);
+ this->dyn_server->config_bus(bus_id,baudrate);
+ for(i=0;i<ids.size();i++)
+ this->init_motor(ids[i]);
+ if(this->config[0].min_angle==-this->info[0].center_angle && this->config[0].max_angle==-this->info[0].center_angle)
+ {
+ this->mode=torque_ctrl;
+ for(i=1;i<ids.size();i++)
+ this->set_position_range(i,-this->info[i].center_angle,-this->info[i].center_angle);
+ }
+ else
+ {
+ this->mode=angle_ctrl;
+ for(i=1;i<ids.size();i++)
+ this->set_position_range(i,-this->info[i].center_angle,this->info[i].max_angle-this->info[i].center_angle);
+ }
+ this->init_events();
+ this->init_threads();
}catch(CException &e){
/* handle exceptions */
- throw;
- }
- }
+ this->clear();
+ throw;
+ }
+ }
+}
+
+unsigned int CDynamixelMotorGroup::from_angles(unsigned int index,double angle)
+{
+ unsigned int counts;
+
+ counts=(angle+this->info[index].center_angle)*this->info[index].encoder_resolution/this->info[index].max_angle;
+
+ return counts;
+}
+
+unsigned int CDynamixelMotorGroup::from_speeds(unsigned int index,double speed)
+{
+ unsigned int counts;
+
+ if(speed>this->info[index].max_speed)
+ speed=this->info[index].max_speed;
+ counts=fabs(speed)/0.666;
+
+ return counts;
}
-std::string CDynamixelMotorGroup::add_motor_control(CDynamixelMotor *controller)
+double CDynamixelMotorGroup::to_angles(unsigned int index,int unsigned counts)
{
- std::vector<float> current_position;
- std::string cont_id;
+ double angle;
+
+ angle=counts*this->info[index].max_angle/this->info[index].encoder_resolution-this->info[index].center_angle;
+
+ return angle;
+}
+
+double CDynamixelMotorGroup::to_speeds(unsigned int index,int unsigned counts)
+{
+ double speed;
+
+ speed=counts*0.666;
+
+ return speed;
+}
+
+void CDynamixelMotorGroup::reset_motor(unsigned int index)
+{
+ unsigned short int current_position;
+ unsigned short int maximum_torque;
try{
- cont_id=CMotorGroup::add_motor_control(controller);
- // if the controller is successfully added
- this->servo_id.push_back(((CDynamixelMotor *)controller)->get_id());
- current_position=controller->get_position();
- this->current_pos.push_back(current_position[0]);
- }catch(CException &e){
+ this->dynamixel_dev[index]->read_word_register(current_pos,¤t_position);
+ }catch(CDynamixelAlarmException &e){
+ /* do nothing - expected exception */
+ }
+ try{
+ this->dynamixel_dev[index]->write_word_register(goal_pos,current_position);
+ }catch(CDynamixelAlarmException &e){
+ /* do nothing - expected exception */
+ }
+ try{
+ this->dynamixel_dev[index]->read_word_register(max_torque,&maximum_torque);
+ }catch(CDynamixelAlarmException &e){
+ /* do nothing - expected exception */
+ }
+ try{
+ this->dynamixel_dev[index]->write_word_register(torque_limit,maximum_torque);
+ }catch(CDynamixelAlarmException &e){
+ /* do nothing - expected exception */
+ }
+ try{
+ this->dynamixel_dev[index]->write_byte_register(led,0x00);
+ }catch(CDynamixelAlarmException &e){
+ /* do nothing - expected exception */
+ }
+}
+
+void CDynamixelMotorGroup::get_model(unsigned int index)
+{
+ unsigned short int model;
+
+ if(this->dynamixel_dev[index]==NULL)
+ {
/* handle exceptions */
- throw;
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+ }
+ else
+ {
+ try{
+ this->dynamixel_dev[index]->read_byte_register(firmware_version,&this->info[index].firmware_ver);
+ this->dynamixel_dev[index]->read_word_register(model_number,&model);
+ switch(model)
+ {
+ case 0x000C: this->info[index].model="AX-12A";
+ this->info[index].max_angle=300.0;
+ this->info[index].center_angle=150.0;
+ this->info[index].max_speed=354.0;
+ this->info[index].encoder_resolution=1023;
+ this->info[index].gear_ratio=254;
+ this->info[index].pid_control=false;
+ break;
+ case 0x012C: this->info[index].model="AX-12W";
+ this->info[index].max_angle=300.0;
+ this->info[index].center_angle=150.0;
+ this->info[index].max_speed=2830;
+ this->info[index].encoder_resolution=1023;
+ this->info[index].gear_ratio=32;
+ this->info[index].pid_control=false;
+ break;
+ case 0x0012: this->info[index].model="AX-18A";
+ this->info[index].max_angle=300.0;
+ this->info[index].center_angle=150.0;
+ this->info[index].max_speed=582;
+ this->info[index].encoder_resolution=1023;
+ this->info[index].gear_ratio=254;
+ this->info[index].pid_control=false;
+ break;
+ case 0x001D: this->info[index].model="MX-28";
+ this->info[index].max_angle=360.0;
+ this->info[index].center_angle=180.0;
+ this->info[index].max_speed=330;
+ this->info[index].encoder_resolution=4095;
+ this->info[index].gear_ratio=193;
+ this->info[index].pid_control=true;
+ break;
+ case 0x0018: this->info[index].model="RX-24F";
+ this->info[index].max_angle=300.0;
+ this->info[index].center_angle=150.0;
+ this->info[index].max_speed=756;
+ this->info[index].encoder_resolution=1023;
+ this->info[index].gear_ratio=193;
+ this->info[index].pid_control=false;
+ break;
+ case 0x001C: this->info[index].model="RX-28";
+ this->info[index].max_angle=300.0;
+ this->info[index].center_angle=150.0;
+ this->info[index].max_speed=402;
+ this->info[index].encoder_resolution=1023;
+ this->info[index].gear_ratio=193;
+ this->info[index].pid_control=false;
+ break;
+ case 0x0136: this->info[index].model="MX-64";
+ this->info[index].max_angle=360.0;
+ this->info[index].center_angle=180.0;
+ this->info[index].max_speed=378;
+ this->info[index].encoder_resolution=4095;
+ this->info[index].gear_ratio=200;
+ this->info[index].pid_control=true;
+ break;
+ case 0x0040: this->info[index].model="Rx-64";
+ this->info[index].max_angle=300.0;
+ this->info[index].center_angle=150.0;
+ this->info[index].max_speed=294;
+ this->info[index].encoder_resolution=1023;
+ this->info[index].gear_ratio=200;
+ this->info[index].pid_control=false;
+ break;
+ case 0x0140: this->info[index].model="MX-106";
+ this->info[index].max_angle=360.0;
+ this->info[index].center_angle=180.0;
+ this->info[index].max_speed=270;
+ this->info[index].encoder_resolution=4095;
+ this->info[index].gear_ratio=225;
+ this->info[index].pid_control=true;
+ break;
+ case 0x006B: this->info[index].model="Ex-106+";
+ this->info[index].max_angle=251.0;
+ this->info[index].center_angle=125.5;
+ this->info[index].max_speed=414;
+ this->info[index].encoder_resolution=4095;
+ this->info[index].gear_ratio=184;
+ this->info[index].pid_control=false;
+ break;
+ default: this->info[index].model="unknown";
+ break;
+ }
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms[index])
+ this->reset_motor(index);
+ throw;
+ }
}
+}
+
+void CDynamixelMotorGroup::set_position_range(unsigned int index,double min, double max)
+{
+ unsigned short int max_pos,min_pos;
- return cont_id;
+ if(this->dynamixel_dev[index]==NULL)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+ }
+ else
+ {
+ max_pos=this->from_angles(index,max);
+ min_pos=this->from_angles(index,min);
+ if(min_pos<0.0 || max_pos>(this->info[index].encoder_resolution))
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The desired range is outside the valid range of the servo.");
+ }
+ else
+ {
+ try{
+ this->config[index].max_angle=max;
+ this->config[index].min_angle=min;
+ this->dynamixel_dev[index]->write_word_register(ccw_angle_limit,max_pos);
+ this->dynamixel_dev[index]->write_word_register(cw_angle_limit,min_pos);
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms[index])
+ this->reset_motor(index);
+ throw;
+ }
+ }
+ }
}
-void CDynamixelMotorGroup::remove_motor_control(std::string &cont_id)
+void CDynamixelMotorGroup::get_position_range(unsigned int index,double *min, double *max)
{
- std::vector<TMotorInfo>::iterator old_cont;
- std::vector<unsigned char> new_servo_id;
- std::vector<float> new_current_pos;
- unsigned int i,old_id;
+ unsigned short int angle_limit;
- try{
- if((old_cont=this->search_controller(cont_id))==(std::vector<TMotorInfo>::iterator)NULL)
+ if(this->dynamixel_dev[index]==NULL)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+ }
+ else
+ {
+ try{
+ this->dynamixel_dev[index]->read_word_register(ccw_angle_limit,&angle_limit);
+ (*max)=this->to_angles(index,angle_limit);
+ this->dynamixel_dev[index]->read_word_register(cw_angle_limit,&angle_limit);
+ (*min)=this->to_angles(index,angle_limit);
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms[index])
+ this->reset_motor(index);
+ throw;
+ }
+ }
+}
+
+void CDynamixelMotorGroup::set_temperature_limit(unsigned int index,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[index]==NULL)
{
/* handle exceptions */
- throw CMotorGroupException(_HERE_,"Impossible to remove the CMotorControl object, no object exist with the given identifier");
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
}
- old_id=((CDynamixelMotor *)old_cont->controller)->get_id();
- for(i=0;i<this->servo_id.size();i++)
- if(this->servo_id[i]!=old_id)
+ else
+ {
+ try{
+ this->config[index].max_temperature=limit;
+ this->dynamixel_dev[index]->write_byte_register(temp_limit,(unsigned char)limit);
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms[index])
+ this->reset_motor(index);
+ throw;
+ }
+ }
+ }
+}
+
+int CDynamixelMotorGroup::get_temperature_limit(unsigned int index)
+{
+ unsigned char limit;
+
+ if(this->dynamixel_dev[index]==NULL)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+ }
+ else
+ {
+ try{
+ this->dynamixel_dev[index]->read_byte_register(temp_limit,&limit);
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms[index])
+ this->reset_motor(index);
+ throw;
+ }
+ }
+
+ return limit;
+}
+
+void CDynamixelMotorGroup::set_voltage_limits(unsigned int index,double min, double max)
+{
+ unsigned char min_voltage,max_voltage;
+
+ if(this->dynamixel_dev[index]==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)
{
- new_servo_id.push_back(servo_id[i]);
- new_current_pos.push_back(this->current_pos[i]);
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The desired voltage range is outside the valid range.");
}
- this->servo_id=new_servo_id;
- CMotorGroup::remove_motor_control(cont_id);
- }catch(CException &e){
+ else
+ {
+ try{
+ this->config[index].min_voltage=min;
+ this->config[index].max_voltage=max;
+ min_voltage=min*10;
+ max_voltage=max*10;
+ this->dynamixel_dev[index]->write_byte_register(low_voltage_limit,min_voltage);
+ this->dynamixel_dev[index]->write_byte_register(high_voltage_limit,max_voltage);
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms[index])
+ this->reset_motor(index);
+ throw;
+ }
+ }
+ }
+ }
+}
+
+void CDynamixelMotorGroup::get_voltage_limits(unsigned int index,double *min, double *max)
+{
+ unsigned char min_voltage,max_voltage;
+
+ if(this->dynamixel_dev[index]==NULL)
+ {
/* handle exceptions */
- throw;
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+ }
+ else
+ {
+ try{
+ this->dynamixel_dev[index]->read_byte_register(low_voltage_limit,&min_voltage);
+ this->dynamixel_dev[index]->read_byte_register(high_voltage_limit,&max_voltage);
+ *min=((double)min_voltage/10.0);
+ *max=((double)max_voltage/10.0);
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms[index])
+ this->reset_motor(index);
+ throw;
+ }
}
}
-CDynamixelMotorGroup::~CDynamixelMotorGroup()
+unsigned char CDynamixelMotorGroup::get_turn_off_alarms(unsigned int index)
+{
+ unsigned char shutdown_alarms=0x00;
+
+ if(this->dynamixel_dev[index]==NULL)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+ }
+ else
+ {
+ try{
+ this->dynamixel_dev[index]->read_byte_register(alarm_shtdwn,&shutdown_alarms);
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms[index])
+ this->reset_motor(index);
+ throw;
+ }
+ }
+
+ return shutdown_alarms;
+}
+
+void CDynamixelMotorGroup::set_max_torque(unsigned int index,double torque_ratio)
+{
+ unsigned short int load;
+
+ if(this->dynamixel_dev[index]==NULL)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+ }
+ else
+ {
+ if(torque_ratio<0.0 || torque_ratio>100.0)
+ {
+ throw CDynamixelMotorException(_HERE_,"The desired torque ratio is outside the valid range.");
+ }
+ else
+ {
+ load=torque_ratio*1023/100.0;
+ try{
+ this->config[index].max_torque=torque_ratio;
+ this->dynamixel_dev[index]->write_word_register(max_torque,load);
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms[index])
+ this->reset_motor(index);
+ throw;
+ }
+ }
+ }
+}
+
+void CDynamixelMotorGroup::set_limit_torque(unsigned int index,double torque_ratio)
+{
+ unsigned short int load;
+
+ if(this->dynamixel_dev[index]==NULL)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+ }
+ else
+ {
+ if(torque_ratio<0.0 || torque_ratio>100.0)
+ {
+ throw CDynamixelMotorException(_HERE_,"The desired torque ratio is outside the valid range.");
+ }
+ else
+ {
+ load=torque_ratio*1023/100.0;
+ try{
+ this->dynamixel_dev[index]->write_word_register(torque_limit,load);
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms[index])
+ this->reset_motor(index);
+ throw;
+ }
+ }
+ }
+}
+
+double CDynamixelMotorGroup::get_max_torque(unsigned int index)
+{
+ unsigned short int load;
+ double torque;
+
+ if(this->dynamixel_dev[index]==NULL)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+ }
+ else
+ {
+ try{
+ this->dynamixel_dev[index]->read_word_register(max_torque,&load);
+ torque=(load&0x3FF)*100.0/1023;
+ if(load>0x3FF)
+ torque=-1*torque;
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms[index])
+ this->reset_motor(index);
+ throw;
+ }
+ }
+
+ return torque;
+}
+
+void CDynamixelMotorGroup::get_compliance_control(unsigned int index,TDynamixel_compliance &config)
+{
+ if(this->dynamixel_dev[index]==NULL)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+ }
+ else
+ {
+ if(!this->info[index].pid_control)
+ {
+ try{
+ this->dynamixel_dev[index]->read_byte_register(cw_comp_margin,&config.cw_compliance_margin);
+ this->dynamixel_dev[index]->read_byte_register(ccw_comp_margin,&config.ccw_compliance_margin);
+ this->dynamixel_dev[index]->read_byte_register(cw_comp_slope,&config.cw_compliance_slope);
+ this->dynamixel_dev[index]->read_byte_register(ccw_comp_slope,&config.ccw_compliance_slope);
+ this->dynamixel_dev[index]->read_byte_register(punch,&config.punch);
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms[index])
+ this->reset_motor(index);
+ throw;
+ }
+ }
+ }
+}
+
+void CDynamixelMotorGroup::get_pid_control(unsigned int index,TDynamixel_pid &config)
+{
+ if(this->dynamixel_dev[index]==NULL)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+ }
+ else
+ {
+ if(this->info[index].pid_control)
+ {
+ try{
+ this->dynamixel_dev[index]->read_byte_register(pid_p,&config.p);
+ this->dynamixel_dev[index]->read_byte_register(pid_i,&config.i);
+ this->dynamixel_dev[index]->read_byte_register(pid_d,&config.d);
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms[index])
+ this->reset_motor(index);
+ throw;
+ }
+ }
+ }
+}
+
+void *CDynamixelMotorGroup::sequence_thread(void *param)
{
+ CDynamixelMotorGroup *mtn_seq=(CDynamixelMotorGroup *)param;
+ std::list<std::string> events;
+ int event_id=0,to=-1;
+ bool end=false;
+ // initialize the motion events
+ events.push_back(mtn_seq->finish_thread_event_id);
+ events.push_back(mtn_seq->start_sequence_event_id);
+ events.push_back(mtn_seq->stop_sequence_event_id);
+ events.push_back(mtn_seq->resume_sequence_event_id);
+
+ while(!end)
+ {
+ try{
+ try{
+ event_id=mtn_seq->event_server->wait_first(events,to);
+ mtn_seq->motion_access.enter();
+ switch(event_id)
+ {
+ case -1: mtn_seq->sequence_current_step++;
+ to=mtn_seq->load_motion(mtn_seq->sequence_current_step);
+ break;
+ case 0: /* finish the thread */
+ end=true;
+ break;
+ case 1: /* start sequence */
+ to=mtn_seq->load_motion(mtn_seq->sequence_current_step);
+ break;
+ case 2: /* stop sequence */
+ mtn_seq->stop();
+ mtn_seq->sequence_error_msg="Motion sequence stopped by user";
+ mtn_seq->sequence_current_step=0;
+ break;
+ case 3: /* resume sequence */
+ mtn_seq->sequence_current_step++;
+ to=mtn_seq->load_motion(mtn_seq->sequence_current_step);
+ break;
+ }
+ mtn_seq->motion_access.exit();
+ }catch(CEventTimeoutException &e){
+ if(!mtn_seq->event_server->event_is_set(mtn_seq->pause_sequence_event_id))
+ {
+ /* load and execute the next step */
+ mtn_seq->motion_access.enter();
+ mtn_seq->sequence_current_step++;
+ to=mtn_seq->load_motion(mtn_seq->sequence_current_step);
+ mtn_seq->motion_access.exit();
+ }
+ else
+ {
+ mtn_seq->event_server->reset_event(mtn_seq->pause_sequence_event_id);
+ to=-1;
+ }
+ }
+ }catch(CException &e){
+ mtn_seq->motion_access.exit();
+ mtn_seq->stop();
+ mtn_seq->sequence_state=mtn_loaded;
+ mtn_seq->sequence_error_msg=e.what();
+ // signal the user
+ if(!mtn_seq->event_server->event_is_set(mtn_seq->sequence_error_event_id))
+ mtn_seq->event_server->set_event(mtn_seq->sequence_error_event_id);
+ if(!mtn_seq->event_server->event_is_set(mtn_seq->sequence_complete_event_id))
+ mtn_seq->event_server->set_event(mtn_seq->sequence_complete_event_id);
+ }
+ }
+
+ pthread_exit(NULL);
+}
+
+void CDynamixelMotorGroup::init_motor(unsigned int id)
+{
+ TDynamixel_info info;
+ TDynamixel_config config;
+ TDynamixel_pid pid;
+ TDynamixel_compliance compliance;
+ CDynamixel *dynamixel_dev=NULL;
+
+ info.baudrate=this->dyn_server->get_baudrate();
+ info.bus_id=this->dyn_server->get_bus_serial();
+ dynamixel_dev=this->dyn_server->get_device(id);
+ this->dynamixel_dev.push_back(dynamixel_dev);
+ info.id=id;
+ this->info.push_back(info);
+ this->get_model(this->servo_id.size());
+ this->get_position_range(this->servo_id.size(),&config.min_angle,&config.max_angle);
+ config.max_temperature=this->get_temperature_limit(this->servo_id.size());
+ this->get_voltage_limits(this->servo_id.size(),&config.min_voltage,&config.max_voltage);
+ config.max_torque=this->get_max_torque(this->servo_id.size());
+ this->config.push_back(config);
+ this->get_compliance_control(this->servo_id.size(),compliance);
+ this->compliance.push_back(compliance);
+ this->get_pid_control(this->servo_id.size(),pid);
+ this->pid.push_back(pid);
+ this->alarms.push_back(this->get_turn_off_alarms(this->servo_id.size()));
+ this->servo_id.push_back(id);
+}
+
+void CDynamixelMotorGroup::init_threads(void)
+{
+ this->thread_server=CThreadServer::instance();
+ this->sequence_thread_id=this->group_id + "sequence_thread";
+ this->thread_server->create_thread(this->sequence_thread_id);
+ this->thread_server->attach_thread(this->sequence_thread_id,this->sequence_thread,this);
+ this->thread_server->start_thread(this->sequence_thread_id);
+}
+
+void CDynamixelMotorGroup::init_events(void)
+{
+ this->event_server=CEventServer::instance();
+ this->finish_thread_event_id=this->group_id + "_finish_thread_event";
+ this->event_server->create_event(this->finish_thread_event_id);
+ this->sequence_complete_event_id=this->group_id + "_sequence_complete_event";
+ this->event_server->create_event(this->sequence_complete_event_id);
+ this->event_server->set_event(this->sequence_complete_event_id);
+ this->sequence_error_event_id=this->group_id + "_sequence_error_event";
+ this->event_server->create_event(this->sequence_error_event_id);
+ this->start_sequence_event_id=this->group_id + "_start_sequence_event";
+ this->event_server->create_event(this->start_sequence_event_id);
+ this->stop_sequence_event_id=this->group_id + "_stop_sequence_event";
+ this->event_server->create_event(this->stop_sequence_event_id);
+ this->pause_sequence_event_id=this->group_id + "_pause_sequence_event";
+ this->event_server->create_event(this->pause_sequence_event_id);
+ this->resume_sequence_event_id=this->group_id + "_resume_sequence_event";
+ this->event_server->create_event(this->resume_sequence_event_id);
+}
+
+void CDynamixelMotorGroup::clear(void)
+{
+ unsigned int i=0;
+
+ for(i=0;i<this->servo_id.size();i++)
+ {
+ if(this->dynamixel_dev[i]!=NULL)
+ {
+ this->dyn_server->free_device(this->dynamixel_dev[i]->get_id());
+ delete this->dynamixel_dev[i];
+ }
+ this->dynamixel_dev[i]=NULL;
+ }
+ this->dynamixel_dev.clear();
+ this->compliance.clear();
+ this->pid.clear();
+ this->config.clear();
+ this->info.clear();
+ this->alarms.clear();
+ this->servo_id.clear();
+ /* clear any motion attribute */
+ this->sequence_state=mtn_empty;
+ this->sequence_current_step=-1;
+ this->sequence_error_msg="Motion sequence ended successfully";
+ this->seq.clear();
+}
+
+int CDynamixelMotorGroup::load_motion(unsigned int step_id)
+{
+ static std::vector<int> time(this->get_num_motors());
+ std::vector<double> current_position;
+ unsigned int i=0;
+ double distance;
+ int max=0;
+
+ if(step_id<0)
+ {
+ /* handle errors */
+ throw CDynamixelMotionSequenceException(_HERE_,"Invalid step index");
+ }
+ else if(step_id>=this->seq.size())
+ {
+ this->sequence_state=mtn_loaded;
+ this->sequence_error_msg="Motion sequence completed successfully";
+ /* signal the completion of the sequence */
+ if(!this->event_server->event_is_set(this->sequence_complete_event_id))
+ this->event_server->set_event(this->sequence_complete_event_id);
+ return -1;
+ }
+ else
+ {
+ this->get_current_angle(current_position);
+ for(i=0;i<this->get_num_motors();i++)
+ {
+ if(this->seq[step_id].absolute_motion)// absolute motion
+ distance=fabs(current_position[i]-this->seq[step_id].position[i]);
+ else// relative motion
+ distance=fabs(this->seq[step_id].position[i]);
+ time[i]=distance/this->seq[step_id].velocity[i]*1000;// time in ms
+ }
+ if(this->seq[step_id].absolute_motion)
+ this->move_absolute_angle(this->seq[step_id].position,this->seq[step_id].velocity);
+ else
+ this->move_relative_angle(this->seq[step_id].position,this->seq[step_id].velocity);
+ max=time[0];
+ for(i=1;i<this->get_num_motors();i++)
+ if(time[i]>max)
+ max=time[i];
+ return max+this->seq[step_id].delay;
+ }
+}
+
+void CDynamixelMotorGroup::load_config(TDynamixelGroup_config &config)
+{
+ std::vector<TDynamixel_compliance> compliance;
+ std::vector<TDynamixel_pid> pid;
+ unsigned int i=0;
+
+ this->clear();
+ try{
+ this->dyn_server->config_bus(config.bus_id,config.baudrate);
+ pid.resize(config.id.size());
+ compliance.resize(config.id.size());
+ for(i=0;i<config.id.size();i++)
+ {
+ this->init_motor(config.id[i]);
+ // load the configuration file
+ std::auto_ptr<dynamixel_motor_config_t> motor(dynamixel_motor_config(config.config_files[i].c_str(),xml_schema::flags::dont_validate));
+ this->set_position_range(i,motor->min_angle(),motor->max_angle());
+ this->set_temperature_limit(i,motor->temp_limit());
+ this->set_voltage_limits(i,motor->min_voltage(),motor->max_voltage());
+ if(this->info[i].pid_control)
+ {
+ pid[i].p=motor->kp();
+ pid[i].i=motor->ki();
+ pid[i].d=motor->kd();
+ }
+ else
+ {
+ compliance[i].cw_compliance_margin=motor->cw_comp_margin();
+ compliance[i].ccw_compliance_margin=motor->ccw_comp_margin();
+ compliance[i].cw_compliance_slope=motor->cw_comp_slope();
+ compliance[i].ccw_compliance_slope=motor->ccw_comp_slope();
+ compliance[i].punch=motor->punch();
+ }
+ this->set_max_torque(i,motor->max_torque());
+ this->set_limit_torque(i,motor->max_torque());
+ }
+ if(this->config[0].min_angle==-this->info[0].center_angle && this->config[0].max_angle==-this->info[0].center_angle)
+ {
+ this->mode=torque_ctrl;
+ for(i=1;i<this->servo_id.size();i++)
+ this->set_position_range(i,-this->info[i].center_angle,-this->info[i].center_angle);
+ }
+ else
+ {
+ this->mode=angle_ctrl;
+ for(i=1;i<this->servo_id.size();i++)
+ this->set_position_range(i,-this->info[i].center_angle,this->info[i].max_angle-this->info[i].center_angle);
+ }
+ this->set_pid_control(pid);
+ this->set_compliance_control(compliance);
+ }catch (const xml_schema::exception& e){
+ std::ostringstream os;
+ os << e;
+ this->clear();
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,os.str());
+ }catch(CException &e){
+ this->clear();
+ throw e;
+ }
+}
+
+#ifdef _HAVE_XSD
+void CDynamixelMotorGroup::load_config(std::string &filename)
+{
+ dyn_motor_group_config_t::dyn_motor_config_iterator iterator;
+ std::vector<TDynamixel_compliance> compliance;
+ std::vector<TDynamixel_pid> pid;
+ unsigned int i=0;
+
+ this->clear();
+
+ // try to open the specified file
+ try{
+ std::auto_ptr<dyn_motor_group_config_t> cfg(dyn_motor_group_config(filename.c_str(), xml_schema::flags::dont_validate));
+ this->dyn_server->config_bus(cfg->bus_id(),cfg->baudrate());
+ pid.resize(cfg->dyn_motor_config().size());
+ compliance.resize(cfg->dyn_motor_config().size());
+ for(iterator=cfg->dyn_motor_config().begin(),i=0;iterator!=cfg->dyn_motor_config().end();iterator++,i++)
+ {
+ this->init_motor(iterator->id());
+ // load the configuration file
+ std::auto_ptr<dynamixel_motor_config_t> motor(dynamixel_motor_config(iterator->config_file().c_str(),xml_schema::flags::dont_validate));
+ this->set_position_range(i,motor->min_angle(),motor->max_angle());
+ this->set_temperature_limit(i,motor->temp_limit());
+ this->set_voltage_limits(i,motor->min_voltage(),motor->max_voltage());
+ if(this->info[i].pid_control)
+ {
+ pid[i].p=motor->kp();
+ pid[i].i=motor->ki();
+ pid[i].d=motor->kd();
+ }
+ else
+ {
+ compliance[i].cw_compliance_margin=motor->cw_comp_margin();
+ compliance[i].ccw_compliance_margin=motor->ccw_comp_margin();
+ compliance[i].cw_compliance_slope=motor->cw_comp_slope();
+ compliance[i].ccw_compliance_slope=motor->ccw_comp_slope();
+ compliance[i].punch=motor->punch();
+ }
+ this->set_max_torque(i,motor->max_torque());
+ this->set_limit_torque(i,motor->max_torque());
+ }
+ if(this->config[0].min_angle==-this->info[0].center_angle && this->config[0].max_angle==-this->info[0].center_angle)
+ {
+ this->mode=torque_ctrl;
+ for(i=1;i<this->servo_id.size();i++)
+ this->set_position_range(i,-this->info[i].center_angle,-this->info[i].center_angle);
+ }
+ else
+ {
+ this->mode=angle_ctrl;
+ for(i=1;i<this->servo_id.size();i++)
+ this->set_position_range(i,-this->info[i].center_angle,this->info[i].max_angle-this->info[i].center_angle);
+ }
+ this->set_pid_control(pid);
+ this->set_compliance_control(compliance);
+ }catch (const xml_schema::exception& e){
+ std::ostringstream os;
+ os << e;
+ this->clear();
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,os.str());
+ }catch(CException &e){
+ this->clear();
+ throw e;
+ }
+}
+
+void CDynamixelMotorGroup::save_config(std::string &filename)
+{
+
+}
+
+void CDynamixelMotorGroup::load_sequence(std::string &filename)
+{
+ dyn_sequence_t::step_iterator iterator;
+ TDynamixelMotionStep step;
+
+ // try to open the specified file
+ try{
+ // stop any current motion
+ this->motion_access.enter();
+ if(this->sequence_state==mtn_started || this->sequence_state==mtn_paused)
+ {
+ this->motion_access.exit();
+ this->stop_sequence();
+ this->motion_access.enter();
+ this->sequence_state=mtn_loaded;
+ }
+ // load the new sequence
+ std::auto_ptr<dyn_sequence_t> seq(dyn_motion_seq(filename.c_str(),xml_schema::flags::dont_validate));
+ if(seq->num_motors()!=this->get_num_motors())
+ {
+ this->motion_access.exit();
+ /* handle exceptions */
+ throw CDynamixelMotionSequenceException(_HERE_,"The number of motors in the loaded sequence does not coincide with the number of motors in the associated motor group");
+ }
+ else
+ {
+ // erase the previous sequence
+ this->seq.clear();
+ for(iterator=seq->step().begin();iterator!=seq->step().end();iterator++)
+ {
+ if(iterator->mode()=="absolute")
+ step.absolute_motion=true;
+ else
+ step.absolute_motion=false;
+ step.delay=iterator->delay();
+ step.position.clear();
+ step.position=iterator->position();
+ step.velocity.clear();
+ step.velocity=iterator->velocity();
+ this->seq.push_back(step);
+ }
+ this->sequence_state=mtn_loaded;
+ this->sequence_current_step=0;
+ }
+ this->motion_access.exit();
+ }catch (const xml_schema::exception& e){
+ this->sequence_state=mtn_empty;
+ this->motion_access.exit();
+ /* handle exceptions */
+ std::ostringstream os;
+ os << e;
+ throw CDynamixelMotionSequenceException(_HERE_,os.str());
+ }
+}
+
+void CDynamixelMotorGroup::save_sequence(std::string &filename)
+{
+ xml_schema::namespace_infomap map;
+ unsigned int i=0,j=0;
+ std::string mode;
+
+ try{
+ // save the base class parameters
+ std::ofstream output_file(filename.c_str(),std::ios_base::out);
+ dyn_sequence_t sequence_file(this->get_num_motors());
+ dyn_sequence_t::step_sequence& motion_sequence(sequence_file.step());
+
+ for(i=0;i<this->seq.size();i++)
+ {
+ if(this->seq[i].absolute_motion)
+ mode="absolute";
+ else
+ mode="relative";
+ step_t step(mode,this->seq[i].delay);
+ step_t::position_sequence& position(step.position());
+ step_t::velocity_sequence& velocity(step.velocity());
+ for(j=0;j<this->get_num_motors();j++)
+ {
+ position.push_back(this->seq[i].position[j]);
+ velocity.push_back(this->seq[i].velocity[j]);
+ }
+ motion_sequence.push_back(step);
+ }
+ map[""].name="";
+ map[""].schema="motion_sequence.xsd";
+ dyn_motion_seq(output_file,sequence_file,map);
+ }catch (const xml_schema::exception& e){
+ std::ostringstream os;
+ os << e;
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,os.str());
+ }
+}
+#endif
+
+void CDynamixelMotorGroup::set_compliance_control(std::vector<TDynamixel_compliance> &config)
+{
+ unsigned int i=0;
+
+ for(i=0;i<this->servo_id.size();i++)
+ {
+ if(this->dynamixel_dev[i]==NULL)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+ }
+ else
+ {
+ if(!this->info[i].pid_control)
+ {
+ if(config[i].cw_compliance_margin>254)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"Invalid clockwise compliance margin.");
+ }
+ if(config[i].ccw_compliance_margin>254)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"Invalid counterclockwise compliance margin.");
+ }
+ if(config[i].cw_compliance_slope>=1 && config[i].cw_compliance_slope<=5) config[i].cw_compliance_slope=4;
+ else if(config[i].cw_compliance_slope>=6 && config[i].cw_compliance_slope<=11) config[i].cw_compliance_slope=8;
+ else if(config[i].cw_compliance_slope>=12 && config[i].cw_compliance_slope<=23) config[i].cw_compliance_slope=16;
+ else if(config[i].cw_compliance_slope>=24 && config[i].cw_compliance_slope<=47) config[i].cw_compliance_slope=32;
+ else if(config[i].cw_compliance_slope>=48 && config[i].cw_compliance_slope<=95) config[i].cw_compliance_slope=64;
+ else if(config[i].cw_compliance_slope>=96 && config[i].cw_compliance_slope<=191) config[i].cw_compliance_slope=128;
+ else if(config[i].cw_compliance_slope>=192 && config[i].cw_compliance_slope<=254) config[i].cw_compliance_slope=254;
+ else
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"Invalid clockwise compliance slope.");
+ }
+ if(config[i].ccw_compliance_slope>=1 && config[i].ccw_compliance_slope<=5) config[i].ccw_compliance_slope=4;
+ else if(config[i].ccw_compliance_slope>=6 && config[i].ccw_compliance_slope<=11) config[i].ccw_compliance_slope=8;
+ else if(config[i].ccw_compliance_slope>=12 && config[i].ccw_compliance_slope<=23) config[i].ccw_compliance_slope=16;
+ else if(config[i].ccw_compliance_slope>=24 && config[i].ccw_compliance_slope<=47) config[i].ccw_compliance_slope=32;
+ else if(config[i].ccw_compliance_slope>=48 && config[i].ccw_compliance_slope<=95) config[i].ccw_compliance_slope=64;
+ else if(config[i].ccw_compliance_slope>=96 && config[i].ccw_compliance_slope<=191) config[i].ccw_compliance_slope=128;
+ else if(config[i].ccw_compliance_slope>=192 && config[i].ccw_compliance_slope<=254) config[i].ccw_compliance_slope=254;
+ else
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"Invalid counterclockwise compliance slope.");
+ }
+ if(config[i].punch<0 || config[i].punch>1023)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"Invalid punch value.");
+ }
+ try{
+ this->compliance[i].cw_compliance_margin=config[i].cw_compliance_margin;
+ this->compliance[i].ccw_compliance_margin=config[i].ccw_compliance_margin;
+ this->compliance[i].cw_compliance_slope=config[i].cw_compliance_slope;
+ this->compliance[i].ccw_compliance_slope=config[i].ccw_compliance_slope;
+ this->dynamixel_dev[i]->write_byte_register(cw_comp_margin,config[i].cw_compliance_margin);
+ this->dynamixel_dev[i]->write_byte_register(ccw_comp_margin,config[i].ccw_compliance_margin);
+ this->dynamixel_dev[i]->write_byte_register(cw_comp_slope,config[i].cw_compliance_slope);
+ this->dynamixel_dev[i]->write_byte_register(ccw_comp_slope,config[i].ccw_compliance_slope);
+ this->dynamixel_dev[i]->write_word_register(punch,config[i].punch);
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms[i])
+ this->reset_motor(i);
+ throw;
+ }
+ }
+ }
+ }
+}
+
+void CDynamixelMotorGroup::set_pid_control(std::vector<TDynamixel_pid> &config)
+{
+ unsigned int i=0;
+
+ for(i=0;i<this->servo_id.size();i++)
+ {
+ if(this->dynamixel_dev[i]==NULL)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+ }
+ else
+ {
+ if(this->info[i].pid_control)
+ {
+ this->pid[i].p=config[i].p;
+ this->pid[i].i=config[i].i;
+ this->pid[i].d=config[i].d;
+ this->dynamixel_dev[i]->write_byte_register(pid_p,config[i].p);
+ this->dynamixel_dev[i]->write_byte_register(pid_i,config[i].i);
+ this->dynamixel_dev[i]->write_byte_register(pid_d,config[i].d);
+ }
+ }
+ }
+}
+
+void CDynamixelMotorGroup::enable(void)
+{
+ unsigned int i=0;
+
+ for(i=0;i<this->servo_id.size();i++)
+ {
+ if(this->dynamixel_dev[i]==NULL)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+ }
+ else
+ {
+ try{
+ this->dynamixel_dev[i]->write_byte_register(torque_en,1);
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms[i])
+ this->reset_motor(i);
+ throw;
+ }
+ }
+ }
+}
+
+void CDynamixelMotorGroup::disable(void)
+{
+ unsigned int i=0;
+
+ for(i=0;i<this->servo_id.size();i++)
+ {
+ if(this->dynamixel_dev[i]==NULL)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+ }
+ else
+ {
+ try{
+ this->dynamixel_dev[i]->write_byte_register(torque_en,0);
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms[i])
+ this->reset_motor(i);
+ throw;
+ }
+ }
+ }
+}
+
+void CDynamixelMotorGroup::move_absolute_angle(std::vector<double> &angles,std::vector<double> &speeds)
+{
+ std::vector< std::vector<unsigned char> > data;
+ unsigned short int cmd[2];
+ unsigned int i=0;
+
+ try{
+ if(this->mode==torque_ctrl)
+ {
+ for(i=0;i<this->servo_id.size();i++)
+ this->set_position_range(i,-this->info[i].center_angle,this->info[i].max_angle-this->info[i].center_angle);
+ this->mode=angle_ctrl;
+ }
+ data.resize(this->servo_id.size());
+ for(i=0;i<this->servo_id.size();i++)
+ {
+ data[i].resize(4);
+ cmd[0]=this->from_angles(i,angles[i]);
+ cmd[1]=this->from_speeds(i,speeds[i]);
+ data[i][0]=((int)(cmd[0])%256);
+ data[i][1]=(int)(cmd[0]/256);
+ data[i][2]=((int)(cmd[1])%256);
+ data[i][3]=(int)(cmd[1]/256);
+ }
+ this->dyn_server->write_sync(this->servo_id, goal_pos, data);
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms[i])
+ this->reset_motor(i);
+ throw;
+ }
+}
+
+void CDynamixelMotorGroup::move_relative_angle(std::vector<double> &angles,std::vector<double> &speeds)
+{
+ std::vector< std::vector<unsigned char> > data;
+ unsigned short int cmd[2],pos;
+ unsigned int i=0;
+
+ try{
+ if(this->mode==torque_ctrl)
+ {
+ for(i=0;i<this->servo_id.size();i++)
+ this->set_position_range(i,-this->info[i].center_angle,this->info[i].max_angle-this->info[i].center_angle);
+ this->mode=angle_ctrl;
+ }
+ data.resize(this->servo_id.size());
+ for(i=0;i<this->servo_id.size();i++)
+ {
+ if(this->dynamixel_dev[i]==NULL)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+ }
+ else
+ {
+ this->dynamixel_dev[i]->read_word_register(current_pos,&pos);
+ data[i].resize(4);
+ cmd[0]=this->from_angles(i,angles[i]+this->to_angles(i,pos));
+ cmd[1]=this->from_speeds(i,speeds[i]);
+ data[i][0]=((int)(cmd[0])%256);
+ data[i][1]=(int)(cmd[0]/256);
+ data[i][2]=((int)(cmd[1])%256);
+ data[i][3]=(int)(cmd[1]/256);
+ }
+ }
+ this->dyn_server->write_sync(this->servo_id,goal_pos,data);
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms[i])
+ this->reset_motor(i);
+ throw;
+ }
+}
+
+void CDynamixelMotorGroup::move_torque(std::vector<double> &torque_ratios)
+{
+ std::vector< std::vector<unsigned char> > data;
+ unsigned short int torque=0;
+ unsigned int i=0;
+
+ try{
+ if(this->mode==angle_ctrl)
+ {
+ for(i=0;i<this->servo_id.size();i++)
+ this->set_position_range(i,-this->info[i].center_angle,-this->info[i].center_angle);
+ this->mode=torque_ctrl;
+ }
+ data.resize(this->servo_id.size());
+ for(i=0;i<this->servo_id.size();i++)
+ {
+ if(this->dynamixel_dev[i]==NULL)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+ }
+ else
+ {
+ torque=0;
+ data[i].resize(2);
+ if(torque_ratios[i]<0.0)
+ torque+=0x0400;
+ torque+=((unsigned short int)(fabs(torque_ratios[i])*1023.0/100.0))&0x03FF;
+ data[i][0]=((int)(torque)%256);
+ data[i][1]=(int)(torque/256);
+ }
+ }
+ this->dyn_server->write_sync(this->servo_id,goal_speed,data);
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms[i])
+ this->reset_motor(i);
+ throw;
+ }
+}
+
+void CDynamixelMotorGroup::stop(void)
+{
+ unsigned short int current_position;
+ unsigned int i=0;
+
+ try{
+ for(i=0;i<this->servo_id.size();i++)
+ {
+ if(this->dynamixel_dev[i]==NULL)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+ }
+ else
+ {
+ if(this->mode==angle_ctrl)
+ {
+ this->dynamixel_dev[i]->read_word_register(current_pos,¤t_position);
+ this->dynamixel_dev[i]->write_word_register(goal_pos,current_position);
+ }
+ else
+ this->dynamixel_dev[i]->write_word_register(goal_speed,0);
+ }
+ }
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms[i])
+ this->reset_motor(i);
+ throw;
+ }
+}
+
+void CDynamixelMotorGroup::get_current_angle(std::vector<double> &angles)
+{
+ unsigned short int data;
+
+ angles.resize(this->servo_id.size());
+
+ for(unsigned int i=0; i<this->servo_id.size(); i++)
+ {
+ if(this->dynamixel_dev[i]==NULL)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+ }
+ else
+ {
+ try{
+ this->dynamixel_dev[i]->read_word_register(current_pos,&data);
+ angles[i] = this->to_angles(i, data);
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms[i])
+ this->reset_motor(i);
+ throw;
+ }
+ }
+ }
+}
+
+void CDynamixelMotorGroup::get_current_speed(std::vector<double> &speeds)
+{
+ unsigned short int data;
+
+ speeds.resize(this->servo_id.size());
+
+ for(unsigned int i=0; i<this->servo_id.size(); i++)
+ {
+ if(this->dynamixel_dev[i]==NULL)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+ }
+ else
+ {
+ try{
+ this->dynamixel_dev[i]->read_word_register(current_speed,&data);
+ speeds[i] = this->to_speeds(i, data);
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms[i])
+ this->reset_motor(i);
+ throw;
+ }
+ }
+ }
+}
+
+void CDynamixelMotorGroup::get_current_effort(std::vector<double> &efforts)
+{
+ unsigned short int c_effort;
+ unsigned short int c_speed;
+
+ efforts.resize(this->servo_id.size());
+
+ for(unsigned int i=0; i<this->servo_id.size(); i++)
+ {
+ if(this->dynamixel_dev[i]==NULL)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+ }
+ else
+ {
+ try{
+ this->dynamixel_dev[i]->read_word_register(current_load,&c_effort);
+ this->dynamixel_dev[i]->read_word_register(current_speed,&c_speed);
+ efforts[i] = (double)((c_effort&0x3FF)*100.0/1023);
+ if (this->to_speeds(i, c_speed) < 0)
+ {
+ efforts[i] *= -1.0;
+ }
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms[i])
+ this->reset_motor(i);
+ throw;
+ }
+ }
+ }
+}
+
+void CDynamixelMotorGroup::get_current_voltage(std::vector<double> &voltages)
+{
+ unsigned char data;
+
+ voltages.resize(this->servo_id.size());
+
+ for(unsigned int i=0; i<this->servo_id.size(); i++)
+ {
+ if(this->dynamixel_dev[i]==NULL)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+ }
+ else
+ {
+ try{
+ this->dynamixel_dev[i]->read_byte_register(current_voltage,&data);
+ voltages[i] = (double)data/10;
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms[i])
+ this->reset_motor(i);
+ throw;
+ }
+ }
+ }
+}
+
+void CDynamixelMotorGroup::get_current_temperature(std::vector<double> &temperatures)
+{
+ unsigned char data;
+
+ temperatures.resize(this->servo_id.size());
+
+ for(unsigned int i=0; i<this->servo_id.size(); i++)
+ {
+ if(this->dynamixel_dev[i]==NULL)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+ }
+ else
+ {
+ try{
+ this->dynamixel_dev[i]->read_byte_register(current_temp,&data);
+ temperatures[i] = (double)data;
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms[i])
+ this->reset_motor(i);
+ throw;
+ }
+ }
+ }
+}
+
+unsigned int CDynamixelMotorGroup::get_num_motors(void)
+{
+ return this->servo_id.size();
+}
+
+TDynamixelMotionStates CDynamixelMotorGroup::get_sequence_current_state(void)
+{
+ return this->sequence_state;
+}
+
+void CDynamixelMotorGroup::add_sequence_step(std::vector<double> &pos,std::vector<double> &vel, double delay, bool abs_motion)
+{
+ TDynamixelMotionStep step;
+
+ if(pos.size()!=this->get_num_motors())
+ {
+ /* handle exceptions */
+ throw CDynamixelMotionSequenceException(_HERE_,"The position vector has a wrong length");
+ }
+ if(vel.size()!=this->get_num_motors())
+ {
+ /* handle exceptions */
+ throw CDynamixelMotionSequenceException(_HERE_,"The velocity vector has a wrong length");
+ }
+ if(delay<0.0)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotionSequenceException(_HERE_,"Negative delay time");
+ }
+ this->motion_access.enter();
+ step.position=pos;
+ step.velocity=vel;
+ step.delay=delay;
+ step.absolute_motion=abs_motion;
+ this->seq.push_back(step);
+ if(this->sequence_state!=mtn_loaded)
+ {
+ this->sequence_state=mtn_loaded;
+ this->sequence_current_step=0;
+ }
+ this->motion_access.exit();
+}
+
+void CDynamixelMotorGroup::insert_sequence_step(unsigned int index,std::vector<double> &pos,std::vector<double> &vel, double delay, bool abs_motion)
+{
+ std::vector<TDynamixelMotionStep> sequence;
+ TDynamixelMotionStep step;
+ unsigned int i;
+
+ if(index < 0 || index > this->seq.size())
+ {
+ /* handle exceptions */
+ throw CDynamixelMotionSequenceException(_HERE_,"The index is out of range");
+ }
+ if(pos.size()!=this->get_num_motors())
+ {
+ /* handle exceptions */
+ throw CDynamixelMotionSequenceException(_HERE_,"The position vector has a wrong length");
+ }
+ if(vel.size()!=this->get_num_motors())
+ {
+ /* handle exceptions */
+ throw CDynamixelMotionSequenceException(_HERE_,"The velocity vector has a wrong length");
+ }
+ if(delay<0.0)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotionSequenceException(_HERE_,"Negative delay time");
+ }
+ this->motion_access.enter();
+ step.position=pos;
+ step.velocity=vel;
+ step.delay=delay;
+ step.absolute_motion=abs_motion;
+ sequence=this->seq;
+ this->seq.clear();
+ for(i=0;i<sequence.size();i++)
+ {
+ if(i==index)
+ this->seq.push_back(step);
+ this->seq.push_back(sequence[i]);
+ }
+ if(this->sequence_state!=mtn_loaded)
+ {
+ this->sequence_state=mtn_loaded;
+ this->sequence_current_step=0;
+ }
+ this->motion_access.exit();
+}
+
+void CDynamixelMotorGroup::delete_sequence_step(unsigned int index)
+{
+ std::vector<TDynamixelMotionStep> sequence;
+ unsigned int i;
+
+ if(index < 0 || index > this->seq.size())
+ {
+ /* handle exceptions */
+ throw CDynamixelMotionSequenceException(_HERE_,"The index is out of range");
+ }
+ this->motion_access.enter();
+ sequence=this->seq;
+ this->seq.clear();
+ for(i=0;i<sequence.size();i++)
+ {
+ if(i!=index)
+ this->seq.push_back(sequence[i]);
+ }
+ if(this->seq.size()==0)
+ this->sequence_state=mtn_empty;
+ this->motion_access.exit();
+}
+
+void CDynamixelMotorGroup::clear_sequence(void)
+{
+ this->stop_sequence();
+ this->motion_access.enter();
+ this->seq.clear();
+ this->sequence_state=mtn_empty;
+ this->motion_access.exit();
+}
+
+void CDynamixelMotorGroup::start_sequence(void)
+{
+ this->motion_access.enter();
+ switch(this->sequence_state)
+ {
+ case mtn_empty: /* handle exceptions */
+ //this->motion_access.exit();
+ //throw CMotionSequenceException(_HERE_,"There is no motion sequence loaded");
+ break;
+ case mtn_loaded: // enable the motors
+ this->enable();
+ this->sequence_current_step=0;
+ if(this->event_server->event_is_set(this->sequence_complete_event_id))
+ this->event_server->reset_event(this->sequence_complete_event_id);
+ if(this->event_server->event_is_set(this->sequence_error_event_id))
+ this->event_server->reset_event(this->sequence_error_event_id);
+ if(!this->event_server->event_is_set(this->start_sequence_event_id))
+ this->event_server->set_event(this->start_sequence_event_id);
+ this->sequence_state=mtn_started;
+ break;
+ case mtn_started: /* do nothing */
+ break;
+ case mtn_paused: /* handle exceptions */
+ //this->motion_access.exit();
+ //throw CMotionSequenceException(_HERE_,"A sequence is currently pause, stop it before starting a new one.");
+ break;
+ }
+ this->motion_access.exit();
+}
+
+void CDynamixelMotorGroup::pause_sequence(void)
+{
+ this->motion_access.enter();
+ switch(this->sequence_state)
+ {
+ case mtn_empty: /* handle exceptions */
+ //this->motion_access.exit();
+ //throw CMotionSequenceException(_HERE_,"There is no motion sequence loaded");
+ break;
+ case mtn_loaded: /* handle exceptions */
+ //this->motion_access.exit();
+ //throw CMotionSequenceException(_HERE_,"There is no motion sequence currently being executed");
+ break;
+ case mtn_started: if(!this->event_server->event_is_set(this->pause_sequence_event_id))
+ this->event_server->set_event(this->pause_sequence_event_id);
+ this->sequence_state=mtn_paused;
+ break;
+ case mtn_paused: /* do nothing */
+ break;
+ }
+ this->motion_access.exit();
+}
+
+void CDynamixelMotorGroup::resume_sequence(void)
+{
+ this->motion_access.enter();
+ switch(this->sequence_state)
+ {
+ case mtn_empty: /* handle exceptions */
+ //this->motion_access.exit();
+ //throw CMotionSequenceException(_HERE_,"There is no motion sequence loaded");
+ break;
+ case mtn_loaded: /* handle exceptions */
+ //this->motion_access.exit();
+ //throw CMotionSequenceException(_HERE_,"There is no motion sequence currently being executed");
+ break;
+ case mtn_started: /* do nothing */
+ break;
+ case mtn_paused: if(!this->event_server->event_is_set(this->resume_sequence_event_id))
+ this->event_server->set_event(this->resume_sequence_event_id);
+ this->sequence_state=mtn_started;
+ break;
+ }
+ this->motion_access.exit();
+}
+
+void CDynamixelMotorGroup::stop_sequence(void)
+{
+ this->motion_access.enter();
+ switch(this->sequence_state)
+ {
+ case mtn_empty: /* handle exceptions */
+ //this->motion_access.exit();
+ //throw CMotionSequenceException(_HERE_,"There is no motion sequence loaded");
+ break;
+ case mtn_loaded: /* handle exceptions */
+ //this->motion_access.exit();
+ //throw CMotionSequenceException(_HERE_,"There is no motion sequence currently being executed");
+ break;
+ case mtn_started: if(!this->event_server->event_is_set(this->stop_sequence_event_id))
+ this->event_server->set_event(this->stop_sequence_event_id);
+ this->sequence_state=mtn_loaded;
+ break;
+ case mtn_paused: if(!this->event_server->event_is_set(this->stop_sequence_event_id))
+ this->event_server->set_event(this->stop_sequence_event_id);
+ this->sequence_state=mtn_loaded;
+ break;
+ }
+ this->motion_access.exit();
+}
+
+std::string CDynamixelMotorGroup::get_sequence_complete_event_id(void)
+{
+ return this->sequence_complete_event_id;
+}
+
+std::string CDynamixelMotorGroup::get_sequence_error_event_id(void)
+{
+ return this->sequence_error_event_id;
+}
+
+std::string CDynamixelMotorGroup::get_sequence_error_message(void)
+{
+ return this->sequence_error_msg;
+}
+
+int CDynamixelMotorGroup::get_sequence_current_step(void)
+{
+ return this->sequence_current_step;
+}
+
+double CDynamixelMotorGroup::get_sequence_completed_percentage(void)
+{
+ double ratio;
+
+ if(this->sequence_state!=mtn_empty)
+ ratio=((double)this->sequence_current_step*100.0)/(this->seq.size());
+ else
+ {
+ /* handle errors */
+ throw CDynamixelMotionSequenceException(_HERE_,"There is no motion sequence loaded");
+ }
+
+ return ratio;
+}
+
+CDynamixelMotorGroup::~CDynamixelMotorGroup()
+{
+ /* stop the motor */
+ this->stop();
+ /* stop any active sequence */
+ if(!this->event_server->event_is_set(this->sequence_complete_event_id))
+ this->stop_sequence();
+ /* stop the internal trhead */
+ this->event_server->set_event(this->finish_thread_event_id);
+ this->thread_server->end_thread(this->sequence_thread_id);
+ this->thread_server->detach_thread(this->sequence_thread_id);
+ this->thread_server->delete_thread(this->sequence_thread_id);
+ this->sequence_thread_id="";
+ this->event_server->set_event(this->finish_thread_event_id);
+ this->event_server->delete_event(this->finish_thread_event_id);
+ this->finish_thread_event_id="";
+ this->event_server->delete_event(this->start_sequence_event_id);
+ this->start_sequence_event_id="";
+ this->event_server->delete_event(this->stop_sequence_event_id);
+ this->stop_sequence_event_id="";
+ this->event_server->delete_event(this->pause_sequence_event_id);
+ this->pause_sequence_event_id="";
+ this->event_server->delete_event(this->resume_sequence_event_id);
+ this->resume_sequence_event_id="";
+ this->event_server->delete_event(this->sequence_complete_event_id);
+ this->sequence_complete_event_id="";
+ this->event_server->delete_event(this->sequence_error_event_id);
+ this->sequence_error_event_id="";
+ this->disable();
+ this->clear();
}
diff --git a/src/dynamixel_motor_group.h b/src/dynamixel_motor_group.h
index b4c66067e0fcf293f34d65b95085defa0d192456..f9329a18cfbbdb7fa05e4d25edf77f3da5f0bc9e 100644
--- a/src/dynamixel_motor_group.h
+++ b/src/dynamixel_motor_group.h
@@ -2,26 +2,473 @@
#define _DYNAMIXEL_MOTOR_GROUP_H
#include "dynamixel_motor.h"
-#include "threadserver.h"
-#include "eventserver.h"
-#include "motor_group.h"
#include "mutex.h"
#include <vector>
-class CDynamixelMotorGroup : public CMotorGroup
+typedef enum {mtn_empty,mtn_loaded,mtn_started,mtn_paused} TDynamixelMotionStates;
+
+typedef struct
+{
+ std::string bus_id;
+ unsigned int baudrate;
+ std::vector<unsigned char> id;
+ std::vector<std::string> config_files;
+}TDynamixelGroup_config;
+
+typedef struct
+{
+ std::vector<double> position;
+ std::vector<double> velocity;
+ bool absolute_motion;
+ double delay;
+}TDynamixelMotionStep;
+
+class CDynamixelMotorGroup
{
private:
- std::vector<unsigned char> servo_id;
- std::vector<float> current_pos;
+ /**
+ * \brief
+ *
+ */
+ std::string group_id;
+ /**
+ * \brief
+ *
+ */
CDynamixelServer *dyn_server;
+ /**
+ * \brief
+ *
+ */
+ std::vector<CDynamixel *> dynamixel_dev;
+ /**
+ * \brief
+ *
+ */
+ std::vector<TDynamixel_compliance> compliance;
+ /**
+ * \brief
+ *
+ */
+ std::vector<TDynamixel_pid> pid;
+ /**
+ * \brief
+ *
+ */
+ std::vector<TDynamixel_config> config;
+ /**
+ * \brief
+ *
+ */
+ std::vector<TDynamixel_info> info;
+ /**
+ * \brief
+ *
+ */
+ std::vector<unsigned char> alarms;
+ /**
+ * \brief
+ *
+ */
+ control_mode mode;
+ /* thread attributes */
+ /**
+ * \brief
+ *
+ */
+ CThreadServer *thread_server;
+ /**
+ * \brief
+ *
+ */
+ std::string sequence_thread_id;
+ /* event attributes */
+ /**
+ * \brief
+ *
+ */
+ CEventServer *event_server;
+ /**
+ * \brief
+ *
+ */
+ std::string finish_thread_event_id;
+ /**
+ * \brief
+ *
+ */
+ std::string sequence_complete_event_id;
+ /**
+ * \brief
+ *
+ */
+ std::string sequence_error_event_id;
+ /**
+ * \brief
+ *
+ */
+ std::string start_sequence_event_id;
+ /**
+ * \brief
+ *
+ */
+ std::string stop_sequence_event_id;
+ /**
+ * \brief
+ *
+ */
+ std::string pause_sequence_event_id;
+ /**
+ * \brief
+ *
+ */
+ std::string resume_sequence_event_id;
+ /**
+ * \brief
+ *
+ */
+ std::vector<unsigned char> servo_id;
+ /* Motion sequence attributes */
+ /**
+ * \brief
+ *
+ */
+ TDynamixelMotionStates sequence_state;
+ /**
+ * \brief
+ *
+ */
+ std::vector<TDynamixelMotionStep> seq;
+ /**
+ * \brief
+ *
+ */
+ int sequence_current_step;
+ /**
+ * \brief
+ *
+ */
+ std::string sequence_error_msg;
+ /**
+ * \brief
+ *
+ */
+ CMutex motion_access;
+ protected:
+ /**
+ * \brief
+ *
+ */
+ unsigned int from_angles(unsigned int index,double angle);
+ /**
+ * \brief
+ *
+ */
+ unsigned int from_speeds(unsigned int index,double speed);
+ /**
+ * \brief
+ *
+ */
+ double to_angles(unsigned int index,int unsigned counts);
+ /**
+ * \brief
+ *
+ */
+ double to_speeds(unsigned int index,int unsigned counts);
+ /**
+ * \brief
+ *
+ */
+ void reset_motor(unsigned int index);
+ /**
+ * \brief
+ *
+ */
+ void get_model(unsigned int index);
+ /**
+ * \brief
+ *
+ */
+ void set_position_range(unsigned int index,double min, double max);
+ /**
+ * \brief
+ *
+ */
+ void get_position_range(unsigned int index,double *min, double *max);
+ /**
+ * \brief
+ *
+ */
+ void set_temperature_limit(unsigned int index,int limit);
+ /**
+ * \brief
+ *
+ */
+ int get_temperature_limit(unsigned int index);
+ /**
+ * \brief
+ *
+ */
+ void set_voltage_limits(unsigned int index,double min, double max);
+ /**
+ * \brief
+ *
+ */
+ void get_voltage_limits(unsigned int index,double *min, double *max);
+ /**
+ * \brief
+ *
+ */
+ unsigned char get_turn_off_alarms(unsigned int index);
+ /**
+ * \brief
+ *
+ */
+ void set_max_torque(unsigned int index,double torque_ratio);
+ /**
+ * \brief
+ *
+ */
+ void set_limit_torque(unsigned int index,double torque_ratio);
+ /**
+ * \brief
+ *
+ */
+ double get_max_torque(unsigned int index);
+ /**
+ * \brief
+ *
+ */
+ void get_compliance_control(unsigned int index,TDynamixel_compliance &config);
+ /**
+ * \brief
+ *
+ */
+ void get_pid_control(unsigned int index,TDynamixel_pid &config);
+ /**
+ * \brief
+ *
+ */
+ void init_motor(unsigned int id);
+ /**
+ * \brief
+ *
+ */
+ void init_threads(void);
+ /**
+ * \brief
+ *
+ */
+ void init_events(void);
+ /**
+ * \brief
+ *
+ */
+ void clear(void);
+ /* motion sequence protected functions */
+ /**
+ * \brief
+ *
+ */
+ static void *sequence_thread(void *param);
+ /**
+ * \brief
+ *
+ */
+ int load_motion(unsigned int step_id);
public:
+ /**
+ * \brief
+ *
+ */
CDynamixelMotorGroup(std::string &group_id);
- void move(std::vector<float> &position,std::vector<float> &velocity,std::vector<float> &acceleration);
- std::string add_motor_control(CDynamixelMotor *controller);
- void remove_motor_control(std::string &cont_id);
+ /**
+ * \brief
+ *
+ */
+ CDynamixelMotorGroup(std::string &group_id,unsigned bus_id,int baudrate,std::vector<unsigned char> &ids);
+ /**
+ * \brief
+ *
+ */
+ CDynamixelMotorGroup(std::string &group_id,std::string &bus_id,int baudrate,std::vector<unsigned char> &ids);
+ /* configuration functions */
+ /**
+ * \brief
+ *
+ */
+ void load_config(TDynamixelGroup_config &config);
+#ifdef _HAVE_XSD
+ /**
+ * \brief
+ *
+ */
+ void load_config(std::string &filename);
+ /**
+ * \brief
+ *
+ */
+ void save_config(std::string &filename);
+#endif
+ /**
+ * \brief
+ *
+ */
+ void set_compliance_control(std::vector<TDynamixel_compliance> &config);
+ /**
+ * \brief
+ *
+ */
+ void set_pid_control(std::vector<TDynamixel_pid> &config);
+ /**
+ * \brief
+ *
+ */
+ void enable(void);
+ /**
+ * \brief
+ *
+ */
+ void disable(void);
+ /**
+ * \brief
+ *
+ */
+ void move_absolute_angle(std::vector<double> &angles, std::vector<double> &speeds);
+ /**
+ * \brief
+ *
+ */
+ void move_relative_angle(std::vector<double> &angles, std::vector<double> &speeds);
+ /**
+ * \brief
+ *
+ */
+ void move_torque(std::vector<double> &torque_ratios);
+ /**
+ * \brief
+ *
+ */
+ void stop(void);
+ /**
+ * \brief
+ *
+ */
+ void get_current_angle(std::vector<double> &angles);
+ /**
+ * \brief
+ *
+ */
+ void get_current_speed(std::vector<double> &speeds);
+ /**
+ * \brief
+ *
+ */
+ void get_current_effort(std::vector<double> &efforts);
+ /**
+ * \brief
+ *
+ */
+ void get_current_voltage(std::vector<double> &voltages);
+ /**
+ * \brief
+ *
+ */
+ void get_current_temperature(std::vector<double> &temperatures);
+ /**
+ * \brief
+ *
+ */
+ unsigned int get_num_motors(void);
+ /* motion sequence public functions */
+ /**
+ * \brief
+ *
+ */
+ TDynamixelMotionStates get_sequence_current_state(void);
+#ifdef _HAVE_XSD
+ /**
+ * \brief
+ *
+ */
+ void load_sequence(std::string &filename);
+ /**
+ * \brief
+ *
+ */
+ void save_sequence(std::string &filename);
+#endif
+ /**
+ * \brief
+ *
+ */
+ void add_sequence_step(std::vector<double> &pos,std::vector<double> &vel,double delay=0.0,bool abs_motion=true);
+ /**
+ * \brief
+ *
+ */
+ void insert_sequence_step(unsigned int index,std::vector<double> &pos,std::vector<double> &vel,double delay=0.0,bool abs_motion=true);
+ /**
+ * \brief
+ *
+ */
+ void delete_sequence_step(unsigned int index);
+ /**
+ * \brief
+ *
+ */
+ void clear_sequence(void);
+ /**
+ * \brief
+ *
+ */
+ void start_sequence(void);
+ /**
+ * \brief
+ *
+ */
+ void pause_sequence(void);
+ /**
+ * \brief
+ *
+ */
+ void resume_sequence(void);
+ /**
+ * \brief
+ *
+ */
+ void stop_sequence(void);
+ /**
+ * \brief
+ *
+ */
+ std::string get_sequence_complete_event_id(void);
+ /**
+ * \brief
+ *
+ */
+ std::string get_sequence_error_event_id(void);
+ /**
+ * \brief
+ *
+ */
+ std::string get_sequence_error_message(void);
+ /**
+ * \brief
+ *
+ */
+ int get_sequence_current_step(void);
+ /**
+ * \brief
+ *
+ */
+ double get_sequence_completed_percentage(void);
+ /**
+ * \brief
+ *
+ */
virtual ~CDynamixelMotorGroup();
};
-
#endif
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index f9d55000831510d397cdd77bca238071c1bdc62a..d9cd6210087bde801f80bb1799652b5129eb4830 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -2,40 +2,16 @@
ADD_EXECUTABLE(test_dynamixel_motor test_dynamixel_motor.cpp)
# edit the following line to add the necessary libraries
-TARGET_LINK_LIBRARIES(test_dynamixel_motor 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_scan test_dynamixel_motor_scan.cpp)
-
-# edit the following line to add the necessary libraries
-TARGET_LINK_LIBRARIES(test_dynamixel_motor_scan 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_sequence test_sequence.cpp)
-
-# edit the following line to add the necessary libraries
-TARGET_LINK_LIBRARIES(test_sequence dynamixel_motor_cont ${comm_LIBRARY} ${motor_control_LIBRARY})
+TARGET_LINK_LIBRARIES(test_dynamixel_motor dynamixel_motor_cont ${comm_LIBRARY})
# edit the following line to add the source code for the example and the name of the executable
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})
-
-# edit the following line to add the source code for the example and the name of the executable
-ADD_EXECUTABLE(test_dynamixel_motor_group_open_loop test_dynamixel_motor_group_open_loop.cpp)
-
-# edit the following line to add the necessary libraries
-TARGET_LINK_LIBRARIES(test_dynamixel_motor_group_open_loop dynamixel_motor_cont ${comm_LIBRARY} ${motor_control_LIBRARY})
+TARGET_LINK_LIBRARIES(test_dynamixel_motor_group dynamixel_motor_cont ${comm_LIBRARY})
# edit the following line to add the source code for the example and the name of the executable
-ADD_EXECUTABLE(test_sequence_open_loop test_sequence_open_loop.cpp)
+ADD_EXECUTABLE(test_dynamixel_motion_seq test_dynamixel_motion_seq.cpp)
# edit the following line to add the necessary libraries
-TARGET_LINK_LIBRARIES(test_sequence_open_loop dynamixel_motor_cont ${comm_LIBRARY} ${motor_control_LIBRARY})
+TARGET_LINK_LIBRARIES(test_dynamixel_motion_seq dynamixel_motor_cont ${comm_LIBRARY})
diff --git a/src/examples/test_dynamixel_motion_seq.cpp b/src/examples/test_dynamixel_motion_seq.cpp
new file mode 100755
index 0000000000000000000000000000000000000000..c7266485269dfe2f1401f1d3c498c9c31dffccb1
--- /dev/null
+++ b/src/examples/test_dynamixel_motion_seq.cpp
@@ -0,0 +1,40 @@
+#include "dynamixelexceptions.h"
+#include "dynamixelserver.h"
+#include "eventserver.h"
+#include "exceptions.h"
+#include "dynamixel_motor_group.h"
+#include "dynamixel_motor.h"
+#include <iostream>
+#include <math.h>
+
+std::string sequence_filename="../src/xml/sequence1.xml";
+
+std::string group_name="GROUP1";
+
+std::string config_file="../src/xml/dyn_group_config.xml";
+
+int main(int argc, char *argv[])
+{
+ CDynamixelServer *dyn_server=CDynamixelServer::instance();
+ CEventServer *event_server=CEventServer::instance();
+ std::vector<double> angles,speeds;
+ CDynamixelMotorGroup group(group_name);
+
+ try{
+ if(dyn_server->get_num_buses()>0)
+ {
+ group.load_config(config_file);
+ group.load_sequence(sequence_filename);
+ group.start_sequence();
+ while(!event_server->event_is_set(group.get_sequence_complete_event_id()))
+ {
+ std::cout << group.get_sequence_completed_percentage() << "% of sequence completed" << std::endl;
+ usleep(200000);
+ }
+ }
+ }catch(CException &e){
+ std::cout << e.what() << std::endl;
+ }
+}
+
+
diff --git a/src/examples/test_dynamixel_motor.cpp b/src/examples/test_dynamixel_motor.cpp
index 20e81b32cf50724a585c0b22640a626fdaac9b41..df7663f3349fbc1524449d977a3e4a82d71ee699 100755
--- a/src/examples/test_dynamixel_motor.cpp
+++ b/src/examples/test_dynamixel_motor.cpp
@@ -1,102 +1,108 @@
#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";
+std::string cont_name="RX-28";
+std::string config_file="../src/xml/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);
- std::string serial="A400gaIt";
+ CDynamixelMotor *cont;
+ std::string serial="A4012B3G";
+ TDynamixel_info info;
+ double max_angle,min_angle;
+ unsigned char led_alarms,sd_alarms;
+ TDynamixel_compliance compliance;
+ TDynamixel_pid pid;
- try{
+ try
+ {
if(dyn_server->get_num_buses()>0)
{
- cont2=new CDynamixelMotor(cont2_name,serial,1000000,1);
- cont2->close();
- delete cont2;
- cont2=new CDynamixelMotor(cont2_name,serial,1000000,1);
- 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,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;
- 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);
+ cont = new CDynamixelMotor(cont_name, serial, 1000000, 23);
- pos1[0]=170.0;
- pos2[0]=125.0;
- for(;;)
+ cont->get_servo_info(info);
+ std::cout << "[INFO]: Servo model: " << info.model << std::endl;
+ std::cout << "[INFO]: Firmware version: " << (int)info.firmware_ver << std::endl;
+ std::cout << "[INFO]: Gear ratio: " << info.gear_ratio << std::endl;
+ std::cout << "[INFO]: Encoder resolution: " << info.encoder_resolution << " counts" << std::endl;
+ if(info.pid_control)
{
- 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();
- 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);
+ std::cout << "[INFO]: PID control capable" << std::endl;
+ pid.p=10;
+ pid.i=20;
+ pid.d=30;
+ cont->set_pid_control(pid);
+ cont->get_pid_control(pid);
+ std::cout << "[INFO]: PID control: (" << (int)pid.p << "," << (int)pid.i << "," << (int)pid.d << ")" << std::endl;
+ }
+ else
+ {
+ std::cout << "[INFO]: Compliance control capable" << std::endl;
+ cont->get_compliance_control(compliance);
+ std::cout << "[INFO]: Compliance margin: (" << std::dec << (int)compliance.cw_compliance_margin << "," << (int)compliance.ccw_compliance_margin << ")" << std::endl;
+ std::cout << "[INFO]: Compliance slope: (" << std::dec << (int)compliance.cw_compliance_slope << "," << (int)compliance.ccw_compliance_slope << ")" << std::endl;
+ }
+ std::cout << "[INFO]: Maximum angle: " << info.max_angle << " degrees" << std::endl;
+ std::cout << "[INFO]: Center angle: " << info.center_angle << " degrees" << std::endl;
+ std::cout << "[INFO]: Maximum speed: " << info.max_speed << " degress/s" << std::endl;
+ std::cout << "[INFO]: Bus identifier: " << info.bus_id << std::endl;
+ std::cout << "[INFO]: Bus baudrate: " << info.baudrate << " bps" << std::endl;
+ std::cout << "[INFO]: Servo identifier: " << (int)info.id << std::endl;
+ cont->get_position_range(&min_angle,&max_angle);
+ std::cout << "[INFO]: Angle range: (" << min_angle << "," << max_angle << ")" << std::endl;
+ sd_alarms = cont->get_turn_off_alarms();
+ std::cout << "[INFO]: Shutdown alarm flags: " << std::hex << (int)sd_alarms << std::endl;
+ std::cout << "[INFO]: - Input Voltage Error:\t" << bool(sd_alarms&0x01) << std::endl;
+ std::cout << "[INFO]: - Angle Limit Error:\t" << bool(sd_alarms&0x02) << std::endl;
+ std::cout << "[INFO]: - OverHeating Error:\t" << bool(sd_alarms&0x04) << std::endl;
+ std::cout << "[INFO]: - Range Error:\t\t" << bool(sd_alarms&0x08) << std::endl;
+ std::cout << "[INFO]: - CheckSum Error:\t" << bool(sd_alarms&0x10) << std::endl;
+ std::cout << "[INFO]: - Overload Error:\t" << bool(sd_alarms&0x20) << std::endl;
+ std::cout << "[INFO]: - Instruction Error:\t" << bool(sd_alarms&0x40) << std::endl;
+ led_alarms=cont->get_led_alarms();
+ std::cout << "[INFO]: LED alarm flags: " << std::hex << (int)led_alarms << std::endl;
+ std::cout << "[INFO]: Punch: " << std::dec << (int)compliance.punch << std::endl;
+ cont->turn_led_on();
+ sleep(1);
+ cont->turn_led_off();
+ // cont->move_absolute_angle(-90,300);
+ // sleep(2);
+ // cont->move_absolute_angle(90,100);
+ // sleep(2);
+ // cont->move_relative_angle(-5,100);
+ // sleep(1);
+ // cont->move_relative_angle(-5,100);
+ // sleep(1);
+ // cont->move_relative_angle(-5,100);
+ // sleep(1);
+ // cont->move_relative_angle(-5,100);
+ // sleep(1);
+ // cont->move_relative_angle(-5,100);
+ // sleep(1);
+ // cont->move_relative_angle(-5,100);
+ // sleep(1);
+ // cont->move_torque(50);
+ // sleep(5);
+ // cont->move_torque(-50);
+ // sleep(5);
+ for (size_t aa = 0; aa < 100; ++aa)
+ {
+ try
+ {
+ cont->move_absolute_angle(125.0, 250);
+ sleep(2);
+ cont->move_absolute_angle(-125.0, 250);
+ sleep(2);
+ }catch(CDynamixelAlarmException &e){
+ std::cout << "[Alarm exception]: " << e.what() << std::endl;
+ }
}
}
}catch(CException &e){
- std::cout << e.what() << std::endl;
- }
+ std::cout << "[Genereal exception]: " << e.what() << std::endl; }
return 0;
}
diff --git a/src/examples/test_dynamixel_motor_group.cpp b/src/examples/test_dynamixel_motor_group.cpp
index 10bcd592f9a69272a81cd492e3a547264a1220bf..0f026e88f4bb78291f2d217f0e4db2d5db26f41a 100644
--- a/src/examples/test_dynamixel_motor_group.cpp
+++ b/src/examples/test_dynamixel_motor_group.cpp
@@ -7,71 +7,49 @@
#include <iostream>
#include <math.h>
-std::string cont1_name="AX-12+_1";
-std::string cont2_name="AX-12+_2";
-std::string cont3_name="AX-12+_3";
-std::string cont4_name="AX-12+_4";
std::string group_name="GROUP1";
-std::string config_file="../src/xml/base_dyn_config.xml";
+std::string config_file="../src/xml/dyn_group_config.xml";
int main(int argc, char *argv[])
{
CDynamixelServer *dyn_server=CDynamixelServer::instance();
- CEventServer *event_server=CEventServer::instance();
- std::vector<float> pos(2),vel(2),acc(2);
- std::vector<bool> enable(2,true);
- std::list<std::string> events;
- CDynamixelMotor *cont1,*cont2;
- CDynamixelMotorGroup *group;
+ std::vector<double> angles,speeds,torques;
+ CDynamixelMotorGroup group(group_name);
unsigned int i=0;
- std::string name;
try{
if(dyn_server->get_num_buses()>0)
{
- cont1=new CDynamixelMotor(cont1_name,0,1000000,21);
- cont2=new CDynamixelMotor(cont2_name,0,1000000,11);
- cont1->set_compliance_slope(254,254);
- cont1->set_punch(4);
- cont1->set_compliance_margin(1,1);
- cont2->set_compliance_slope(254,254);
- cont2->set_punch(16);
- cont2->set_compliance_margin(1,1);
-
- group=new CDynamixelMotorGroup(group_name);
- name=group->add_motor_control(cont1);
- #ifdef _HAVE_XSD
- group->config_motor_control(name,config_file);
- #endif
- name=group->add_motor_control(cont2);
- #ifdef _HAVE_XSD
- group->config_motor_control(name,config_file);
- #endif
-
- group->get_position(pos);
- group->enable(enable);
- group->use_closed_loop_control();
- events.push_back(group->get_position_reached_event_id());
- pos[0]=150.0;
- pos[1]=150.0;
- vel[0]=100.0;
- vel[1]=100.0;
- acc[0]=vel[0]*vel[0]/(0.2*50);
- acc[1]=vel[1]*vel[1]/(0.2*50);
- event_server->wait_all(events);
- group->move(pos,vel,acc);
+ group.load_config(config_file);
+ angles.resize(2);
+ angles[0]=90;
+ angles[1]=-90;
+ speeds.resize(2);
+ speeds[0]=100;
+ speeds[1]=50;
+ group.move_absolute_angle(angles,speeds);
+ sleep(5);
+ angles.resize(2);
+ angles[0]=-90;
+ angles[1]=90;
+ group.move_absolute_angle(angles,speeds);
+ sleep(5);
+ // relative motion
+ angles[0]=10;
+ angles[1]=-10;
for(i=0;i<10;i++)
{
- pos[0]=100;
- pos[1]=100;
- event_server->wait_all(events);
- group->move(pos,vel,acc);
- pos[0]=200;
- pos[1]=200;
- event_server->wait_all(events);
- group->move(pos,vel,acc);
- }
+ group.move_relative_angle(angles,speeds);
+ sleep(1);
+ }
+ // torque motion
+ torques.resize(2);
+ torques[0]=100.0;
+ torques[1]=-50;
+ group.move_torque(torques);
+ sleep(4);
+ group.stop();
}
}catch(CException &e){
std::cout << e.what() << std::endl;
diff --git a/src/examples/test_dynamixel_motor_group_open_loop.cpp b/src/examples/test_dynamixel_motor_group_open_loop.cpp
deleted file mode 100755
index 59d4266495d78b130fa1505239ab0c5c83e51705..0000000000000000000000000000000000000000
--- a/src/examples/test_dynamixel_motor_group_open_loop.cpp
+++ /dev/null
@@ -1,88 +0,0 @@
-#include "dynamixelexceptions.h"
-#include "dynamixelserver.h"
-#include "eventserver.h"
-#include "exceptions.h"
-#include "dynamixel_motor_group.h"
-#include "dynamixel_motor.h"
-#include <iostream>
-#include <math.h>
-
-std::string cont1_name="AX-12+_1";
-std::string cont2_name="AX-12+_2";
-std::string group_name="GROUP1";
-
-int main(int argc, char *argv[])
-{
- CDynamixelServer *dyn_server=CDynamixelServer::instance();
- CEventServer *event_server=CEventServer::instance();
- std::vector<float> pos(2),vel(2),acc(2);
- std::vector<bool> enable(2,true);
- std::list<std::string> events;
- CDynamixelMotor *cont1,*cont2;
- CDynamixelMotorGroup *group;
- unsigned int i=0,j=0;
- int dir=1;
-
- try{
- if(dyn_server->get_num_buses()>0)
- {
- cont1=new CDynamixelMotor(cont1_name,0,1000000,21);
- cont2=new CDynamixelMotor(cont2_name,0,1000000,11);
- cont1->set_compliance_slope(254,254);
- cont1->set_punch(4);
- cont1->set_compliance_margin(1,1);
- cont2->set_compliance_slope(254,254);
- cont2->set_punch(16);
- cont2->set_compliance_margin(1,1);
-
- group=new CDynamixelMotorGroup(group_name);
- group->add_motor_control(cont1);
- group->add_motor_control(cont2);
- group->enable(enable);
- group->use_open_loop_control();
- pos[0]=150.0;
- pos[1]=150.0;
- group->move(pos,vel,acc);
- sleep(2);
- for(j=0;j<1000;j++)
- {
- if(pos[0]>=290)
- dir=-1;
- else if(pos[0]<=10)
- dir=1;
- for(i=0;i<pos.size();i++)
- {
- pos[i]+=1.5*dir;
- }
- group->move(pos,vel,acc);
- usleep(20000);
- }
- group->use_closed_loop_control();
- events.push_back(group->get_position_reached_event_id());
- pos[0]=150.0;
- pos[1]=150.0;
- vel[0]=100.0;
- vel[1]=100.0;
- acc[0]=vel[0]*vel[0]/(0.2*50);
- acc[1]=vel[1]*vel[1]/(0.2*50);
- event_server->wait_all(events);
- group->move(pos,vel,acc);
- for(i=0;i<10;i++)
- {
- pos[0]=100;
- pos[1]=100;
- event_server->wait_all(events);
- group->move(pos,vel,acc);
- pos[0]=200;
- pos[1]=200;
- event_server->wait_all(events);
- group->move(pos,vel,acc);
- }
-
- }
- }catch(CException &e){
- std::cout << e.what() << std::endl;
- }
-}
-
-
diff --git a/src/examples/test_dynamixel_motor_open_loop.cpp b/src/examples/test_dynamixel_motor_open_loop.cpp
deleted file mode 100755
index 6bf2c8b1e12e54059fc44c414bbf3956cfa851ad..0000000000000000000000000000000000000000
--- a/src/examples/test_dynamixel_motor_open_loop.cpp
+++ /dev/null
@@ -1,146 +0,0 @@
-#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/examples/test_dynamixel_motor_scan.cpp b/src/examples/test_dynamixel_motor_scan.cpp
deleted file mode 100755
index 04d54e29ff82691340fdd0f6430af974017a8aba..0000000000000000000000000000000000000000
--- a/src/examples/test_dynamixel_motor_scan.cpp
+++ /dev/null
@@ -1,52 +0,0 @@
-#include "dynamixelexceptions.h"
-#include "dynamixelserver.h"
-#include "eventserver.h"
-#include "exceptions.h"
-#include "dynamixel_motor.h"
-#include <iostream>
-
-std::string cont1_name="AX-12+_1";
-std::string cont2_name="AX-12+_2";
-
-int main(int argc, char *argv[])
-{
- CDynamixelServer *dyn_server=CDynamixelServer::instance();
- CEventServer *event_server=CEventServer::instance();
- std::vector<std::string> motion_done_1;
- std::vector<int> ids;
- std::list<std::string> events;
- CDynamixelMotor *cont1;
- 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)
- {
- events.push_back(dyn_server->get_scan_done_event_id());
- dyn_server->config_bus(0,1000000);
- dyn_server->start_scan();
- event_server->wait_first(events);
- std::cout << "Found " << dyn_server->get_num_devices() << " devices" << std::endl;
- ids=dyn_server->get_device_ids();
- for(i=0;i<dyn_server->get_num_devices();i++)
- std::cout << "Device " << i << " -> id: " << ids[i] << std::endl;
- cont1=new CDynamixelMotor(cont1_name,0,dyn_server->get_baudrate(),ids[0]);
- cont1->enable(enable);
- cont1->enable_torque_control();
- position=cont1->get_position();
- std::cout << "Current position of device " << ids[0] << ": " << position[0] << std::endl;
- cont1->set_torque(100.0);
- sleep(2);
- cont1->set_torque(-100.0);
- sleep(2);
- cont1->set_torque(0.0);
- }
- }catch(CException &e){
- std::cout << e.what() << std::endl;
- }
-
- return 0;
-}
diff --git a/src/examples/test_sequence.cpp b/src/examples/test_sequence.cpp
deleted file mode 100755
index 6b446da0e653170d5478edb51d03aa16f044299e..0000000000000000000000000000000000000000
--- a/src/examples/test_sequence.cpp
+++ /dev/null
@@ -1,92 +0,0 @@
-#include "motion_sequence.h"
-#include "eventserver.h"
-#include "exceptions.h"
-#include "dynamixel_motor.h"
-#include "motor_group.h"
-#include <math.h>
-
-std::string cont_name1="AX-12+_1";
-std::string cont_name2="AX-12+_2";
-std::string cont_name3="AX-12+_3";
-std::string seq_name="test_sequence";
-std::string seq_file="../src/xml/test_motion.xml";
-std::string seq_file2="../src/xml/test_motion2.xml";
-std::string seq_file3="../src/xml/test_motion3.xml";
-std::string group_name="test";
-std::string cont_config_file="../src/xml/base_dyn_config.xml";
-
-int main(int argc, char * argv[])
-{
- try{
-#ifdef _HAVE_XSD
- CEventServer *event_server=CEventServer::instance();
-#endif
- std::list<std::string> events;
- CMotorGroup group(group_name);
- CMotionSequence sequence(seq_name);
- CDynamixelMotor cont1(cont_name1,0,1000000,1);
- CDynamixelMotor cont2(cont_name2,0,1000000,12);
- CDynamixelMotor cont3(cont_name3,0,1000000,15);
- std::vector<float> position(3);
- std::vector<float> velocity(3);
-
- events.push_back(sequence.get_sequence_complete_event_id());
-#ifdef _HAVE_XSD
- cont1.load_config(cont_config_file);
- cont2.load_config(cont_config_file);
- cont3.load_config(cont_config_file);
-#endif
- group.add_motor_control(&cont1);
- group.add_motor_control(&cont2);
- group.add_motor_control(&cont3);
- sequence.set_motor_group(&group);
-#ifdef _HAVE_XSD
- sequence.load_sequence(seq_file);
- sequence.set_timeout_factor(1.5);
- sequence.start_sequence();
- while(!event_server->event_is_set(sequence.get_sequence_complete_event_id()))
- {
- std::cout << sequence.get_completed_percentage() << "% of sequence completed" << std::endl;
- usleep(200000);
- }
- sequence.start_sequence();
- while(!event_server->event_is_set(sequence.get_sequence_complete_event_id()))
- {
- std::cout << sequence.get_completed_percentage() << "% of sequence completed" << std::endl;
- usleep(200000);
- }
- sequence.load_sequence(seq_file2);
- position[0]=100;
- position[1]=200;
- position[2]=100;
- velocity[0]=50;
- velocity[1]=50;
- velocity[2]=50;
- sequence.add_step(position,velocity,0);
- position[0]=200;
- position[1]=100;
- position[2]=200;
- sequence.add_step(position,velocity,2000);
- position[0]=100;
- position[1]=200;
- position[2]=100;
- sequence.add_step(position,velocity,0);
- position[0]=0;
- position[1]=0;
- position[2]=0;
- sequence.insert_step(0,position,velocity,0);
- sequence.start_sequence();
- while(!event_server->event_is_set(sequence.get_sequence_complete_event_id()))
- {
- std::cout << sequence.get_completed_percentage() << "% of sequence completed" << std::endl;
- usleep(200000);
- }
- sequence.save_sequence(seq_file3);
-#else
- std::cout << "Impossible to execute sequence because the XML library is not available" << std::endl;
-#endif
-
- }catch(CException &e){
- std::cout << e.what() << std::endl;
- }
-}
diff --git a/src/examples/test_sequence_open_loop.cpp b/src/examples/test_sequence_open_loop.cpp
deleted file mode 100755
index 820797c5092c18e47d2790be8934489ad7769ebf..0000000000000000000000000000000000000000
--- a/src/examples/test_sequence_open_loop.cpp
+++ /dev/null
@@ -1,89 +0,0 @@
-#include "motion_sequence.h"
-#include "eventserver.h"
-#include "exceptions.h"
-#include "dynamixel_motor.h"
-#include "dynamixel_motor_group.h"
-#include "motor_group.h"
-#include <math.h>
-
-std::string cont_name1="AX-12+_1";
-std::string cont_name2="AX-12+_2";
-std::string seq_name="test_sequence";
-std::string seq_file="../src/xml/test_motion.xml";
-std::string seq_file2="../src/xml/test_motion2.xml";
-std::string seq_file3="../src/xml/test_motion3.xml";
-std::string group_name="test";
-std::string cont_config_file="../src/xml/base_dyn_config.xml";
-
-int main(int argc, char * argv[])
-{
- try{
-#ifdef _HAVE_XSD
- CEventServer *event_server=CEventServer::instance();
-#endif
- std::list<std::string> events;
- CDynamixelMotorGroup group(group_name);
- CMotionSequence sequence(seq_name);
- CDynamixelMotor cont1(cont_name1,0,1000000,11);
- CDynamixelMotor cont2(cont_name2,0,1000000,21);
- std::vector<float> position(2);
- std::vector<float> velocity(2);
-
- events.push_back(sequence.get_sequence_complete_event_id());
-#ifdef _HAVE_XSD
- cont1.load_config(cont_config_file);
- cont2.load_config(cont_config_file);
-#endif
- group.add_motor_control(&cont1);
- group.add_motor_control(&cont2);
- sequence.set_motor_group(&group);
-#ifdef _HAVE_XSD
- sequence.load_sequence(seq_file);
- sequence.set_timeout_factor(1.5);
- sequence.start_sequence();
- while(!event_server->event_is_set(sequence.get_sequence_complete_event_id()))
- {
- std::cout << sequence.get_completed_percentage() << "% of sequence completed" << std::endl;
- usleep(200000);
- }
- sequence.start_sequence();
- while(!event_server->event_is_set(sequence.get_sequence_complete_event_id()))
- {
- std::cout << sequence.get_completed_percentage() << "% of sequence completed" << std::endl;
- usleep(200000);
- }
- sequence.load_sequence(seq_file2);
- position[0]=100;
- position[1]=200;
- position[2]=100;
- velocity[0]=50;
- velocity[1]=50;
- velocity[2]=50;
- sequence.add_step(position,velocity,0);
- position[0]=200;
- position[1]=100;
- position[2]=200;
- sequence.add_step(position,velocity,2000);
- position[0]=100;
- position[1]=200;
- position[2]=100;
- sequence.add_step(position,velocity,0);
- position[0]=0;
- position[1]=0;
- position[2]=0;
- sequence.insert_step(0,position,velocity,0);
- sequence.start_sequence();
- while(!event_server->event_is_set(sequence.get_sequence_complete_event_id()))
- {
- std::cout << sequence.get_completed_percentage() << "% of sequence completed" << std::endl;
- usleep(200000);
- }
- sequence.save_sequence(seq_file3);
-#else
- std::cout << "Impossible to execute sequence because the XML library is not available" << std::endl;
-#endif
-
- }catch(CException &e){
- std::cout << e.what() << std::endl;
- }
-}
diff --git a/src/xml/CMakeLists.txt b/src/xml/CMakeLists.txt
index 5112a622d9a4079523b6d3ffd162dbd8a57381bf..72bb23ab1f278fb9b59f512758b393baaa5733ba 100755
--- a/src/xml/CMakeLists.txt
+++ b/src/xml/CMakeLists.txt
@@ -13,7 +13,7 @@ ENDIF(EXISTS "/usr/include/xsd/cxx")
IF(XSD_FOUND)
SET(XSD_LIBRARY ${XSD_LIBRARY} PARENT_SCOPE)
- SET(XSD_FILES dynamixel_motor_cfg_file.xsd)
+ SET(XSD_FILES dynamixel_motor_cfg_file.xsd dynamixel_motor_group_cfg_file.xsd dyn_motion_seq_file.xsd)
IF(XSD_FILES)
SET(XSD_PATH ${CMAKE_CURRENT_SOURCE_DIR})
@@ -34,7 +34,7 @@ IF(XSD_FOUND)
ADD_CUSTOM_TARGET(xsd_files_gen DEPENDS ${XSD_SOURCES_INT})
ADD_CUSTOM_COMMAND(
OUTPUT ${XSD_SOURCES_INT}
- COMMAND xsd cxx-tree --generate-serialization ${XSD_FILES}
+ COMMAND xsdcxx cxx-tree --generate-serialization ${XSD_FILES}
WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
DEPENDS ${XSD_PATH_FILES}
COMMENT "Parsing the xml template file ${XSD_FILES}")
diff --git a/src/xml/base_dyn_config.xml b/src/xml/base_dyn_config.xml
deleted file mode 100755
index 401aa3e9255fca4993c338778c4272919c496442..0000000000000000000000000000000000000000
--- a/src/xml/base_dyn_config.xml
+++ /dev/null
@@ -1,38 +0,0 @@
-<?xml version="1.0"?>
-
-<motor_config>
- <num_axis>1</num_axis>
- <axis_config>
- <accel_range>
- <max>0</max>
- <min>0</min>
- </accel_range>
- <velocity_range>
- <max>0</max>
- <min>0</min>
- </velocity_range>
- <position_range>
- <max>300</max>
- <min>0</min>
- </position_range>
- <encoder_res>1</encoder_res>
- <gear_factor>1.0</gear_factor>
- </axis_config>
- <config_file>dyn_config.xml</config_file>
- <position_feedback>
- <enabled>1</enabled>
- <mode>polling</mode>
- <rate>100.0</rate>
- </position_feedback>
- <velocity_feedback>
- <enabled>0</enabled>
- <mode>polling</mode>
- <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>
diff --git a/src/xml/dyn_config.xml b/src/xml/dyn_config.xml
index d8eb131954c553e430490392d030efa9ea4c455e..6f8a57d8d99081e3b8ce8f53334d4cef36a499bf 100755
--- a/src/xml/dyn_config.xml
+++ b/src/xml/dyn_config.xml
@@ -3,12 +3,19 @@
<dynamixel_motor_config xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
xsi:noNamespaceSchemaLocation="dynamixel_motor_cfg_file.xsd">
+ <alarm_shtdwn>4</alarm_shtdwn>
+ <min_angle>-125</min_angle>
+ <max_angle>125</max_angle>
<temp_limit>85</temp_limit>
<max_voltage>19</max_voltage>
<min_voltage>6</min_voltage>
- <cw_comp_margin>2</cw_comp_margin>
- <ccw_comp_margin>2</ccw_comp_margin>
- <cw_comp_slope>16</cw_comp_slope>
- <ccw_comp_slope>16</ccw_comp_slope>
- <punch>0</punch>
+ <max_torque>100</max_torque>
+ <cw_comp_margin>0</cw_comp_margin>
+ <ccw_comp_margin>0</ccw_comp_margin>
+ <cw_comp_slope>32</cw_comp_slope>
+ <ccw_comp_slope>32</ccw_comp_slope>
+ <punch>32</punch>
+ <kp>0</kp>
+ <ki>0</ki>
+ <kd>0</kd>
</dynamixel_motor_config>
diff --git a/src/xml/dyn_group_config.xml b/src/xml/dyn_group_config.xml
new file mode 100755
index 0000000000000000000000000000000000000000..48864feed344cf19b166a902c2977495ac6b025b
--- /dev/null
+++ b/src/xml/dyn_group_config.xml
@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+
+<dyn_motor_group_config xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
+ xsi:noNamespaceSchemaLocation="dyn_motor_group_cfg_file.xsd">
+
+ <bus_id>A4012B3G</bus_id>
+ <baudrate>1000000</baudrate>
+ <dyn_motor_config>
+ <id>1</id>
+ <config_file>../src/xml/dyn_config.xml</config_file>
+ </dyn_motor_config>
+ <dyn_motor_config>
+ <id>3</id>
+ <config_file>../src/xml/dyn_config.xml</config_file>
+ </dyn_motor_config>
+</dyn_motor_group_config>
diff --git a/src/xml/dyn_motion_seq_file.xsd b/src/xml/dyn_motion_seq_file.xsd
new file mode 100755
index 0000000000000000000000000000000000000000..43fe9d61c1b418ec0725af8b330ce61e3ddff067
--- /dev/null
+++ b/src/xml/dyn_motion_seq_file.xsd
@@ -0,0 +1,44 @@
+<?xml version="1.0"?>
+
+<!--
+
+file : dyn_motion_seq_file.xsd
+author : Sergi Hernandez Juan (shernand@iri.upc.edu)
+copyright : not copyrighted - public domain
+
+-->
+
+<xsd:schema xmlns:xsd="http://www.w3.org/2001/XMLSchema">
+ <xsd:simpleType name="motion_t">
+ <xsd:restriction base="xsd:string">
+ <xsd:enumeration value="absolute"/>
+ <xsd:enumeration value="relative"/>
+ </xsd:restriction>
+ </xsd:simpleType>
+
+ <xsd:complexType name="step_t">
+ <xsd:sequence>
+ <xsd:element name="position" type="xsd:double" maxOccurs="unbounded">
+ </xsd:element>
+ <xsd:element name="velocity" type="xsd:double" maxOccurs="unbounded">
+ </xsd:element>
+ <xsd:element name="mode" type="motion_t">
+ </xsd:element>
+ <xsd:element name="delay" type="xsd:double">
+ </xsd:element>
+ </xsd:sequence>
+ </xsd:complexType>
+
+ <xsd:complexType name="dyn_sequence_t">
+ <xsd:sequence>
+ <xsd:element name="num_motors" type="xsd:unsignedInt">
+ </xsd:element>
+ <xsd:element name="step" type="step_t" maxOccurs="unbounded">
+ </xsd:element>
+ </xsd:sequence>
+ </xsd:complexType>
+
+ <xsd:element name="dyn_motion_seq" type="dyn_sequence_t">
+ </xsd:element>
+
+</xsd:schema>
diff --git a/src/xml/dynamixel_motor_cfg_file.xsd b/src/xml/dynamixel_motor_cfg_file.xsd
index 7d6b76c150f2602fbbaa5755b3a1d7cab4a944ed..d813b05bce3f0162a932da2ec32705ae08f1229b 100755
--- a/src/xml/dynamixel_motor_cfg_file.xsd
+++ b/src/xml/dynamixel_motor_cfg_file.xsd
@@ -24,15 +24,19 @@ copyright : not copyrighted - public domain
<xsd:complexType name="dynamixel_motor_config_t">
<xsd:sequence>
+ <xsd:element name="alarm_shtdwn" type="xsd:unsignedByte">
+ </xsd:element>
+ <xsd:element name="max_angle" type="xsd:float">
+ </xsd:element>
+ <xsd:element name="min_angle" type="xsd:float">
+ </xsd:element>
<xsd:element name="temp_limit" type="xsd:int">
</xsd:element>
<xsd:element name="max_voltage" type="xsd:float">
</xsd:element>
<xsd:element name="min_voltage" type="xsd:float">
</xsd:element>
- <xsd:element name="led_alarms" type="alarm_t" minOccurs="0" maxOccurs="unbounded">
- </xsd:element>
- <xsd:element name="off_alarms" type="alarm_t" minOccurs="0" maxOccurs="unbounded">
+ <xsd:element name="max_torque" type="xsd:float">
</xsd:element>
<xsd:element name="cw_comp_margin" type="xsd:unsignedByte">
</xsd:element>
@@ -44,6 +48,12 @@ copyright : not copyrighted - public domain
</xsd:element>
<xsd:element name="punch" type="xsd:unsignedByte">
</xsd:element>
+ <xsd:element name="kp" type="xsd:unsignedByte">
+ </xsd:element>
+ <xsd:element name="ki" type="xsd:unsignedByte">
+ </xsd:element>
+ <xsd:element name="kd" type="xsd:unsignedByte">
+ </xsd:element>
</xsd:sequence>
</xsd:complexType>
diff --git a/src/xml/dynamixel_motor_group_cfg_file.xsd b/src/xml/dynamixel_motor_group_cfg_file.xsd
new file mode 100755
index 0000000000000000000000000000000000000000..6b416ce9f6eafb7d149af2d534c4ab60652c5c0e
--- /dev/null
+++ b/src/xml/dynamixel_motor_group_cfg_file.xsd
@@ -0,0 +1,36 @@
+<?xml version="1.0"?>
+
+<!--
+
+file : dynamixel_motor_group_cfg_file.xsd
+author : Sergi Hernandez Juan (shernand@iri.upc.edu)
+copyright : not copyrighted - public domain
+
+-->
+
+<xsd:schema xmlns:xsd="http://www.w3.org/2001/XMLSchema">
+
+ <xsd:complexType name="dyn_motor_config_t">
+ <xsd:sequence>
+ <xsd:element name="id" type="xsd:int">
+ </xsd:element>
+ <xsd:element name="config_file" type="xsd:string">
+ </xsd:element>
+ </xsd:sequence>
+ </xsd:complexType>
+
+ <xsd:complexType name="dyn_motor_group_config_t">
+ <xsd:sequence>
+ <xsd:element name="bus_id" type="xsd:string">
+ </xsd:element>
+ <xsd:element name="baudrate" type="xsd:int">
+ </xsd:element>
+ <xsd:element name="dyn_motor_config" type="dyn_motor_config_t" maxOccurs="unbounded">
+ </xsd:element>
+ </xsd:sequence>
+ </xsd:complexType>
+
+ <xsd:element name="dyn_motor_group_config" type="dyn_motor_group_config_t">
+ </xsd:element>
+
+</xsd:schema>
diff --git a/src/xml/sequence1.xml b/src/xml/sequence1.xml
new file mode 100755
index 0000000000000000000000000000000000000000..01f634145cd92e8e987c2aed40c1bb5b62986898
--- /dev/null
+++ b/src/xml/sequence1.xml
@@ -0,0 +1,63 @@
+<?xml version="1.0"?>
+
+<dyn_motion_seq xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
+ xsi:noNamespaceSchemaLocation="dyn_motion_seq_file.xsd">
+ <num_motors>2</num_motors>
+ <step>
+ <position>0.0</position>
+ <position>0.0</position>
+ <velocity>100.0</velocity>
+ <velocity>100.0</velocity>
+ <mode>absolute</mode>
+ <delay>0</delay>
+ </step>
+ <step>
+ <position>30.0</position>
+ <position>30.0</position>
+ <velocity>100.0</velocity>
+ <velocity>100.0</velocity>
+ <mode>absolute</mode>
+ <delay>0</delay>
+ </step>
+ <step>
+ <position>-30.0</position>
+ <position>-30.0</position>
+ <velocity>100.0</velocity>
+ <velocity>100.0</velocity>
+ <mode>absolute</mode>
+ <delay>0</delay>
+ </step>
+ <step>
+ <position>20.0</position>
+ <position>20.0</position>
+ <velocity>20.0</velocity>
+ <velocity>20.0</velocity>
+ <mode>relative</mode>
+ <delay>0</delay>
+ </step>
+ <step>
+ <position>20.0</position>
+ <position>20.0</position>
+ <velocity>20.0</velocity>
+ <velocity>20.0</velocity>
+ <mode>relative</mode>
+ <delay>0</delay>
+ </step>
+ <step>
+ <position>20.0</position>
+ <position>20.0</position>
+ <velocity>20.0</velocity>
+ <velocity>20.0</velocity>
+ <mode>relative</mode>
+ <delay>0</delay>
+ </step>
+ <step>
+ <position>-30.0</position>
+ <position>-30.0</position>
+ <velocity>100.0</velocity>
+ <velocity>100.0</velocity>
+ <mode>absolute</mode>
+ <delay>0</delay>
+ </step>
+</dyn_motion_seq>
+
diff --git a/src/xml/test_motion.xml b/src/xml/test_motion.xml
deleted file mode 100755
index 95e982aaab8d3eb5647b331cfae45c487da8e83a..0000000000000000000000000000000000000000
--- a/src/xml/test_motion.xml
+++ /dev/null
@@ -1,17 +0,0 @@
-<?xml version="1.0"?>
-
-<motion_seq xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
- xsi:noNamespaceSchemaLocation="motion_sequence_file.xsd">
- <num_motors>2</num_motors>
- <num_steps>1</num_steps>
- <control>position</control>
- <motion>absolute</motion>
- <loop>open</loop>
- <step>
- <position>150.0</position>
- <position>150.0</position>
- <velocity>100.0</velocity>
- <velocity>100.0</velocity>
- <delay>0</delay>
- </step>
-</motion_seq>
diff --git a/src/xml/test_motion2.xml b/src/xml/test_motion2.xml
deleted file mode 100755
index 0b474af49661caca00f551e629600034ffd2f2ee..0000000000000000000000000000000000000000
--- a/src/xml/test_motion2.xml
+++ /dev/null
@@ -1,17 +0,0 @@
-<?xml version="1.0"?>
-
-<motion_seq xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
- xsi:noNamespaceSchemaLocation="motion_sequence_file.xsd">
- <num_motors>2</num_motors>
- <num_steps>1</num_steps>
- <control>position</control>
- <motion>absolute</motion>
- <loop>open</loop>
- <step>
- <position>150.0</position>
- <position>150.0</position>
- <velocity>150.0</velocity>
- <velocity>150.0</velocity>
- <delay>0</delay>
- </step>
-</motion_seq>