From 9cde4bad28edb2f7e171711eb5e2c30f3a159ed2 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A0ndez=20Juan?= <shernand@iri.upc.edu>
Date: Fri, 22 Jul 2011 08:09:49 +0000
Subject: [PATCH] Merged the sergi_upgrades branch to the trunk
---
src/CMakeLists.txt | 10 +-
src/dynamixel_motor.cpp | 862 +++++++--------------
src/dynamixel_motor.h | 214 +----
src/examples/CMakeLists.txt | 11 +
src/examples/test_dynamixel_motor.cpp | 146 ++--
src/examples/test_dynamixel_motor_scan.cpp | 52 ++
src/examples/test_sequence.cpp | 63 ++
src/xml/CMakeLists.txt | 45 ++
src/xml/base_dyn_config.xml | 37 +
src/xml/dyn_config.xml | 14 +
src/xml/dynamixel_motor_cfg_file.xsd | 53 ++
src/xml/test_motion.xml | 16 +
12 files changed, 685 insertions(+), 838 deletions(-)
create mode 100755 src/examples/test_dynamixel_motor_scan.cpp
create mode 100755 src/examples/test_sequence.cpp
create mode 100755 src/xml/CMakeLists.txt
create mode 100755 src/xml/base_dyn_config.xml
create mode 100755 src/xml/dyn_config.xml
create mode 100755 src/xml/dynamixel_motor_cfg_file.xsd
create mode 100755 src/xml/test_motion.xml
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index b14067c..40b6ae3 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,3 +1,5 @@
+ADD_SUBDIRECTORY(xml)
+
# edit the following line to add all the source code files of the library
SET(sources dynamixel_motor.cpp)
# edit the following line to add all the header files of the library
@@ -12,10 +14,14 @@ FIND_PACKAGE(motor_control REQUIRED)
# edit the following line to add the necessary include directories
INCLUDE_DIRECTORIES(${comm_INCLUDE_DIR} ${dynamixel_INCLUDE_DIR} ${motor_control_INCLUDE_DIR} .)
-ADD_LIBRARY(dynamixel_motor_cont SHARED ${sources})
+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 ${comm_LIBRARY} ${dynamixel_LIBRARY} ${motor_control_LIBRARY})
+TARGET_LINK_LIBRARIES(dynamixel_motor_cont ${comm_LIBRARY} ${dynamixel_LIBRARY} ${motor_control_LIBRARY} ${XSD_LIBRARY})
+
+ADD_DEPENDENCIES(dynamixel_motor_cont xsd_files_gen)
INSTALL(TARGETS dynamixel_motor_cont
RUNTIME DESTINATION bin
diff --git a/src/dynamixel_motor.cpp b/src/dynamixel_motor.cpp
index 6e6c300..cf4b305 100644
--- a/src/dynamixel_motor.cpp
+++ b/src/dynamixel_motor.cpp
@@ -6,12 +6,17 @@
#include <math.h>
#include <iostream>
#include <sstream>
+#include <fstream>
+#ifdef _HAVE_XSD
+#include "xml/dynamixel_motor_cfg_file.hxx"
+#endif
CDynamixelMotor::CDynamixelMotor(std::string& cont_id,unsigned char bus_id,unsigned char dev_id):CMotorControl(cont_id,1)
{
CEventServer *event_server=CEventServer::instance();
- std::vector<float> max,min,rate;
- std::list<std::string> events;
+ std::vector<float> max,min,gear;
+ std::vector<int> enc_res;
+ TMotorConfig config;
int num_buses=0;
this->info.model="";
@@ -24,254 +29,148 @@ CDynamixelMotor::CDynamixelMotor(std::string& cont_id,unsigned char bus_id,unsig
this->dynamixel_dev=NULL;
this->alarms=0x24;
this->torque_control=false;
- if(!this->dyn_server->is_scan_done(bus_id))
- {
- if((num_buses=this->dyn_server->get_num_buses())>0)
- {
- events.push_back(this->dyn_server->get_scan_done_event_id());
- this->dyn_server->start_scan(bus_id);
- event_server->wait_all(events);
- }
- else
- {
- /* handle exceptions */
- }
- }
- else
- {
- if(this->dyn_server->get_bus_id()==bus_id)
- {
- this->dynamixel_dev=this->dyn_server->get_device(dev_id);
- this->config(NULL);
- this->alarms=this->get_turn_off_alarms();
- this->info.model=this->get_model();
- this->info.firmware_ver=this->get_firmware_version();
- this->get_position_range(0x0001,min,max);
- if(min[0]==0.0 && max[0]==0.0)
- this->enable_torque_control();
- else
- {
- rate.push_back(100.0);
- this->enable_position_feedback(0x0001,rate);
- this->torque_control=false;
- }
- }
+ try{
+ this->dyn_server->set_bus_id(bus_id);
+ this->dyn_server->set_baudrate(1000000);
+ this->dynamixel_dev=this->dyn_server->get_device(dev_id);
+ enc_res.push_back(1);
+ 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.model=this->get_model();
+ this->info.firmware_ver=this->get_firmware_version();
+ this->get_position_range(min,max);
+ 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];
+ this->config(&config);
+ if(min[0]==0.0 && max[0]==0.0)
+ this->enable_torque_control();
else
{
- /* handle exceptions */
+ this->config_position_feedback(fb_polling,100.0);
+ this->enable_position_feedback();
+ this->torque_control=false;
}
+ this->current_position=this->cont_get_position()[0];
+ }catch(CException &e){
+ /* handle exceptions */
+ if(this->dynamixel_dev!=NULL)
+ delete this->dynamixel_dev;
+ this->dynamixel_dev=NULL;
+ throw;
}
}
-void CDynamixelMotor::angles_to_controller(short int motors,std::vector<float>& angles,std::vector<long int>& counts)
+void CDynamixelMotor::angles_to_controller(std::vector<float>& angles,std::vector<float>& counts)
{
- long int count_value=0;
-
- if(motors!=motor1)
+ if(angles.size()!=1)
{
- /* handle exceptions */
+ /* handle exceptions */
}
else
{
- if(angles.size()!=1)
- {
- /* handle exceptions */
- }
- else
- {
- counts.clear();
- count_value=(long int)(angles[0]*1023.0/300.0);
- counts.push_back(count_value);
- }
+ counts.clear();
+ counts.push_back(angles[0]*1023.0/300.0);
}
}
-void CDynamixelMotor::speeds_to_controller(short int motors,std::vector<float>& speeds,std::vector<long int>& counts)
+void CDynamixelMotor::speeds_to_controller(std::vector<float>& speeds,std::vector<float>& counts)
{
- long int count_value=0;
-
- if(motors!=motor1)
+ if(speeds.size()!=1)
{
- /* handle exceptions */
+ /* handle exceptions */
}
else
{
- if(speeds.size()!=1)
- {
- /* handle exceptions */
- }
- else
- {
- counts.clear();
- if(speeds[0]>0.0)
- count_value=(long int)(fabs(speeds[0])*1023.0/(114.0*6.0));
- else
- count_value=(long int)(fabs(speeds[0])*1023.0/(114.0*6.0));
- counts.push_back(count_value);
- }
+ counts.clear();
+ if(speeds[0]>0.0)
+ counts.push_back(fabs(speeds[0])*1023.0/(114.0*6.0));
+ else
+ counts.push_back(fabs(speeds[0])*1023.0/(114.0*6.0));
}
}
-void CDynamixelMotor::accels_to_controller(short int motors,std::vector<float>& accels,std::vector<long int>& counts)
+void CDynamixelMotor::accels_to_controller(std::vector<float>& accels,std::vector<float>& counts)
{
- if(motors!=motor1)
+ if(accels.size()!=1)
{
- /* handle exceptions */
+ /* handle exceptions */
}
else
{
- if(accels.size()!=1)
- {
- /* handle exceptions */
- }
- else
- {
- counts.clear();
- counts.push_back((long int)accels[0]);
- }
+ counts.clear();
+ counts.push_back((long int)accels[0]);
}
}
-void CDynamixelMotor::controller_to_angles(short int motors,std::vector<long int>& counts,std::vector<float>& angles)
+void CDynamixelMotor::controller_to_angles(std::vector<float>& counts,std::vector<float>& angles)
{
- float angle_value=0.0;
-
- if(motors!=motor1)
+ if(counts.size()!=1)
{
- /* handle exceptions */
+ /* handle exceptions */
}
else
{
- if(counts.size()!=1)
- {
- /* handle exceptions */
- }
- else
- {
- angles.clear();
- angle_value=((float)counts[0]*300.0/1023.0);
- angles.push_back(angle_value);
- }
+ angles.clear();
+ angles.push_back(counts[0]*300.0/1023.0);
}
}
-void CDynamixelMotor::controller_to_speeds(short int motors,std::vector<long int>& counts,std::vector<float>& speeds)
+void CDynamixelMotor::controller_to_speeds(std::vector<float>& counts,std::vector<float>& speeds)
{
- float speed_value=0.0;
-
- if(motors!=motor1)
+ if(counts.size()!=1)
{
- /* handle exceptions */
+ /* handle exceptions */
}
else
{
- if(counts.size()!=1)
- {
- /* handle exceptions */
- }
- else
- {
- speeds.clear();
- speed_value=((float)counts[0]*114.0*6.0/1023.0);
- speeds.push_back(speed_value);
- }
+ speeds.clear();
+ speeds.push_back(counts[0]*114.0*6.0/1023.0);
}
}
-void CDynamixelMotor::controller_to_accels(short int motors,std::vector<long int>& counts,std::vector<float>& accels)
+void CDynamixelMotor::controller_to_accels(std::vector<float>& counts,std::vector<float>& accels)
{
- if(motors!=motor1)
+ if(counts.size()!=1)
{
- /* handle exceptions */
+ /* handle exceptions */
}
else
{
- if(counts.size()!=1)
- {
- /* handle exceptions */
- }
- else
- {
- accels.clear();
- accels.push_back((float)counts[0]);
- }
+ accels.clear();
+ accels.push_back((float)counts[0]);
}
}
-void CDynamixelMotor::cont_set_accel_range(short int motors,std::vector<long int>& min, std::vector<long int>& max)
-{
- /* do nothing */
-}
-
-void CDynamixelMotor::cont_set_velocity_range(short int motors,std::vector<long int>& min, std::vector<long int>& max)
-{
- /* do nothing */
-}
-
-void CDynamixelMotor::cont_set_position_range(short int motors,std::vector<long int>& min, std::vector<long int>& max)
+void CDynamixelMotor::cont_set_position_range(std::vector<float>& min, std::vector<float>& max)
{
unsigned short int current_position;
- if(motors!=0x0001)
- {
- /* handle exceptions */
- }
- else
+ if(this->dynamixel_dev==NULL)
{
- if(this->dynamixel_dev==NULL)
- {
- /* handle exceptions */
- }
- else
- {
- if(min[0]<0x0000 || max[0]>0x03FF)
- {
- /* handle exceptions */
- }
- 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;
- }
- }
- }
- }
-}
-
-void CDynamixelMotor::cont_set_encoder_resolution(short int motors,std::vector<int>& enc_res)
-{
- /* do nothing */
-}
-
-void CDynamixelMotor::cont_set_gear_factor(short int motors,std::vector<float>& gear)
-{
- /* do nothing */
-}
-
-void CDynamixelMotor::cont_set_current_limit(short int motors,std::vector<float>& current)
-{
- unsigned short int current_position;
-
- if(motors!=0x0001)
- {
- /* handle exceptions */
+ /* handle exceptions */
}
else
{
- if(this->dynamixel_dev==NULL)
+ if(min[0]<0x0000 || max[0]>0x03FF)
{
/* handle exceptions */
}
else
{
try{
- this->dynamixel_dev->write_word_register(torque_limit,(unsigned short int)current[0]);
+ 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)
@@ -282,512 +181,283 @@ void CDynamixelMotor::cont_set_current_limit(short int motors,std::vector<float>
}
}
-void CDynamixelMotor::cont_get_accel_range(short int motors,std::vector<long int>& min, std::vector<long int>& max)
+void CDynamixelMotor::cont_get_position_range(std::vector<float>& min, std::vector<float>& max)
{
- min.clear();
- max.clear();
- min.push_back(0);
- max.push_back(0);
-}
+ unsigned short int angle_limit;
-void CDynamixelMotor::cont_get_velocity_range(short int motors,std::vector<long int>& min, std::vector<long int>& max)
-{
min.clear();
max.clear();
- min.push_back(0);
- max.push_back(0);
-}
-
-void CDynamixelMotor::cont_get_position_range(short int motors,std::vector<long int>& min, std::vector<long int>& max)
-{
- unsigned short int angle_limit;
-
- if(motors!=0x0001)
- {
- /* handle exceptions */
- }
- else
- {
- min.clear();
- max.clear();
- if(this->dynamixel_dev==NULL)
- {
- /* handle exceptions */
- }
- 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;
- }
- }
- }
-}
-
-void CDynamixelMotor::cont_get_encoder_resolution(short int motors,std::vector<int>& enc_res)
-{
- if(motors!=motor1)
- {
- /* handle exceptions */
- }
- else
- {
- enc_res.clear();
- enc_res.push_back(1);
- }
-}
-
-void CDynamixelMotor::cont_get_gear_factor(short int motors,std::vector<float>& gear)
-{
- if(motors!=motor1)
+ if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
}
else
{
- gear.clear();
- gear.push_back(1.0);
- }
-}
-
-void CDynamixelMotor::cont_get_current_limit(short int motors,std::vector<float>& current)
-{
- unsigned short int current_limit;
-
- if(motors!=0x0001)
- {
- /* handle exceptions */
- }
- else
- {
- current.clear();
- if(this->dynamixel_dev==NULL)
- {
- /* handle exceptions */
+ 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;
}
- else
- {
- try{
- this->dynamixel_dev->read_word_register(torque_limit,¤t_limit);
- current.push_back(current_limit&0x03FF);
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms)
- this->reset_motor();
- throw;
- }
- }
}
}
-void CDynamixelMotor::cont_set_absolute_motion(short int motors)
-{
- /* do nothing */
-}
-
-void CDynamixelMotor::cont_set_relative_motion(short int motors)
-{
- /* do nothing */
-}
-
-void CDynamixelMotor::cont_enable_position_feedback(short int motors,std::vector<float>& sample_rate)
-{
- /* do nothing */
-}
-
-void CDynamixelMotor::cont_enable_velocity_feedback(short int motors,std::vector<float>& sample_rate)
-{
- /* do nothing */
-}
-
-void CDynamixelMotor::cont_enable_acceleration_feedback(short int motors,std::vector<float>& sample_rate)
-{
- /* do nothing */
-}
-
-void CDynamixelMotor::cont_enable_current_feedback(short int motors,std::vector<float>& sample_rate)
-{
- /* do nothing */
-}
-
-void CDynamixelMotor::cont_enable_limits_feedback(short int motors,std::vector<float>& sample_rate)
-{
- /* do nothing */
-}
-
-void CDynamixelMotor::cont_disable_position_feedback(short int motors)
-{
- /* do nothing */
-}
-
-void CDynamixelMotor::cont_disable_velocity_feedback(short int motors)
+void CDynamixelMotor::cont_get_encoder_resolution(std::vector<int>& enc_res)
{
- /* do nothing */
-}
-
-void CDynamixelMotor::cont_disable_acceleration_feedback(short int motors)
-{
- /* do nothing */
-}
-
-void CDynamixelMotor::cont_disable_current_feedback(short int motors)
-{
- /* do nothing */
+ enc_res.clear();
+ enc_res.push_back(1);
}
-void CDynamixelMotor::cont_disable_limits_feedback(short int motors)
+void CDynamixelMotor::cont_get_gear_factor(std::vector<float>& gear)
{
- /* do nothing */
+ gear.clear();
+ gear.push_back(1);
}
-void CDynamixelMotor::cont_get_position(short int motors,std::vector<long int>& position)
+std::vector<float> CDynamixelMotor::cont_get_position(void)
{
unsigned short int current_position;
+ std::vector<float> position;
- if(motors!=motor1)
- {
+ position.clear();
+ if(this->dynamixel_dev==NULL)
+ {
/* handle exceptions */
}
else
{
- position.clear();
- if(this->dynamixel_dev==NULL)
- {
- /* handle exceptions */
- }
- else
- {
- try{
- this->dynamixel_dev->read_word_register(current_pos,¤t_position);
- position.push_back(current_position&0x03FF);
- }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);
+ position.push_back(current_position&0x03FF);
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms)
+ this->reset_motor();
+ throw;
}
}
+
+ return position;
}
-void CDynamixelMotor::cont_get_velocity(short int motors,std::vector<long int>& velocity)
+std::vector<float> CDynamixelMotor::cont_get_velocity(void)
{
unsigned short int current_velocity;
+ std::vector<float> velocity;
- if(motors!=motor1)
+ velocity.clear();
+ if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
}
else
{
- velocity.clear();
- if(this->dynamixel_dev==NULL)
- {
- /* handle exceptions */
- }
- else
- {
- try{
- this->dynamixel_dev->read_word_register(current_speed,¤t_velocity);
- velocity.push_back(current_velocity&0x03FF);
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms)
- this->reset_motor();
- throw;
- }
+ try{
+ this->dynamixel_dev->read_word_register(current_speed,¤t_velocity);
+ velocity.push_back(current_velocity&0x03FF);
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms)
+ this->reset_motor();
+ throw;
}
}
-}
-void CDynamixelMotor::cont_get_acceleration(short int motors,std::vector<long int>& accel)
-{
- if(motors!=motor1)
- {
- /* handle exceptions */
- }
- else
- {
- accel.clear();
- accel.push_back(0x0000);
- }
+ return velocity;
}
-void CDynamixelMotor::cont_get_current(short int motors,std::vector<float>& current)
+void CDynamixelMotor::cont_enable(std::vector<bool> &enable)
{
- unsigned short int current_current;
+ unsigned char value;
- if(motors!=motor1)
+ if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
}
else
{
- current.clear();
- if(this->dynamixel_dev==NULL)
- {
- /* handle exceptions */
- }
- else
- {
- try{
- this->dynamixel_dev->read_word_register(current_load,¤t_current);
- current_current=current_current&0x07FF;
- if(current_current>1023)
- {
- current_current-=1024;
- current_current=-current_current;
- }
- current.push_back(current_current);
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms)
- this->reset_motor();
- throw;
- }
+ 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;
}
}
}
-void CDynamixelMotor::cont_get_limits(short int motors,std::vector<short int>& limits)
+void CDynamixelMotor::cont_disable(std::vector<bool> &disable)
{
- if(motors!=motor1)
+ unsigned char value;
+
+ if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
}
else
{
- limits.clear();
- limits.push_back(0x0000);
+ 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;
+ }
}
}
-void CDynamixelMotor::cont_wait_for_events(std::vector<short int>& events)
-{
- sleep(1);
- events.clear();
-}
-
-void CDynamixelMotor::cont_set_gpio(int gpio_id,gpio_type type)
-{
- /* do nothing */
-}
-
-void CDynamixelMotor::cont_write_gpio(int gpio_id,bool value)
-{
- /* do nothing */
-}
-
-void CDynamixelMotor::cont_write_gpio(int gpio_id,float value)
-{
- /* do nothing */
-}
-
-float CDynamixelMotor::cont_read_gpio(int gpio_id)
-{
- /* do nothing */
-}
-
-void CDynamixelMotor::cont_flush_hardware_queue(short int motors)
-{
- /* do nothing */
-}
-
-void CDynamixelMotor::cont_config(void *config)
+void CDynamixelMotor::cont_load(std::vector<float>& position, std::vector<float>& velocity, std::vector<float>& accel)
{
- short int motors=0x0001;
- std::vector<float> gear;
- std::vector<int> enc_res;
-
- enc_res.push_back(1);
- gear.push_back(1.0);
- this->set_encoder_resolution(motors,enc_res);
- this->set_gear_factor(motors,gear);
-}
-
-void CDynamixelMotor::cont_save_config(std::string& filename)
-{
- /* do nothing */
-}
-
-void CDynamixelMotor::cont_load_config(std::string& filename)
-{
- /* do nothing */
-}
+ std::vector<bool> relative;
-void CDynamixelMotor::cont_enable(short int motors)
-{
- unsigned short int current_position;
- unsigned char value=0x01;
-
- if(motors!=motor1)
+ if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
}
else
{
- if(this->dynamixel_dev==NULL)
- {
- /* handle exceptions */
- }
- else
- {
- try{
- 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{
+ 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;
}
}
}
-void CDynamixelMotor::cont_disable(short int motors)
+void CDynamixelMotor::cont_load(std::vector<float>& velocity, std::vector<float>& accel)
{
- unsigned short int current_position;
- unsigned char value=0x00;
-
- if(motors!=motor1)
+ if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
}
else
{
- if(this->dynamixel_dev==NULL)
- {
- /* handle exceptions */
- }
- else
- {
- try{
- 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;
- }
- }
+ /* handle exceptions */
+ // velocity control is not supported
}
}
-void CDynamixelMotor::cont_home(short int motors)
+void CDynamixelMotor::cont_move(void)
{
- /* do nothing */
+ this->dyn_server->action();
}
-void CDynamixelMotor::cont_load(short int motors,std::vector<long int>& position, std::vector<long int>& velocity, std::vector<long int>& accel)
-{
- if(motors!=motor1)
+void CDynamixelMotor::cont_stop(void)
+{
+ unsigned short int current_position;
+
+ if(this->dynamixel_dev==NULL)
{
/* handle exceptions */
}
else
{
- if(this->dynamixel_dev==NULL)
- {
- /* handle exceptions */
- }
- else
- {
- try{
- if(!this->torque_control)
- {
- this->dynamixel_dev->write_word_register(goal_speed,(unsigned short int)velocity[0]);
- this->dynamixel_dev->registered_word_write(goal_pos,(unsigned short int)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(current_pos,¤t_position);
+ this->dynamixel_dev->write_word_register(goal_pos,current_position);
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms)
+ this->reset_motor();
+ throw;
}
}
}
-void CDynamixelMotor::cont_load(short int motors,std::vector<long int>& velocity, std::vector<long int>& accel)
+void CDynamixelMotor::cont_close(void)
{
- if(motors!=motor1)
+ if(this->dynamixel_dev!=NULL)
{
- /* handle exceptions */
- }
- else
- {
- if(this->dynamixel_dev==NULL)
- {
- /* handle exceptions */
- }
- else
- {
- /* handle exceptions */
- // velocity control is not supported
- }
+ this->dyn_server->free_device(this->dynamixel_dev->get_id());
+ delete this->dynamixel_dev;
+ this->dynamixel_dev=NULL;
}
}
-void CDynamixelMotor::cont_move(short int motors)
+#ifdef _HAVE_XSD
+void CDynamixelMotor::cont_load_config(std::string &filename)
{
- if(motors!=motor1)
- {
+ // 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));
+ // configure the parameters of the controller
+ this->set_temperature_limit(cfg->temp_limit());
+ this->set_voltage_limits(cfg->min_voltage(),cfg->max_voltage());
+ this->set_compliance_margin(cfg->cw_comp_margin(),cfg->ccw_comp_margin());
+ this->set_compliance_slope(cfg->cw_comp_slope(),cfg->ccw_comp_slope());
+ this->set_punch(cfg->punch());
+ }catch (const xml_schema::exception& e){
+ std::ostringstream os;
+ os << e;
/* handle exceptions */
- }
- else
- {
- this->dyn_server->action();
+ throw CMotorConfigException(_HERE_,os.str());
}
}
-void CDynamixelMotor::cont_reset(void)
+void CDynamixelMotor::cont_save_config(std::string &filename)
{
- /* do nothing */
-}
-
-void CDynamixelMotor::cont_stop(short int motors)
-{
- unsigned short int current_position;
+ xml_schema::namespace_infomap map;
+ float max_voltage, min_voltage;
+ unsigned char cw_margin, ccw_margin;
+ unsigned char cw_slope, ccw_slope;
- if(motors!=motor1)
- {
+ try{
+ std::ofstream output_file(filename.c_str(),std::ios_base::out);
+ this->get_voltage_limits(&min_voltage,&max_voltage);
+ 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());
+ map[""].name="";
+ map[""].schema="dynamixel_motor_config.xsd";
+ dynamixel_motor_config(output_file,dyn_cfg,map);
+ }catch (const xml_schema::exception& e){
+ std::ostringstream os;
+ os << e;
/* handle exceptions */
- }
- else
- {
- if(this->dynamixel_dev==NULL)
- {
- /* handle exceptions */
- }
- else
- {
- try{
- this->dynamixel_dev->read_word_register(current_pos,¤t_position);
- this->dynamixel_dev->write_word_register(goal_pos,current_position);
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms)
- this->reset_motor();
- throw;
- }
- }
+ CMotorConfigException(_HERE_,os.str());
}
}
-
-void CDynamixelMotor::cont_close(void)
-{
-}
+#endif
void CDynamixelMotor::reset_motor(void)
{
@@ -843,7 +513,6 @@ int CDynamixelMotor::get_baudrate(void)
unsigned char CDynamixelMotor::get_node_address(void)
{
- unsigned short int current_position;
unsigned char address=0x00;
if(this->dynamixel_dev==NULL)
@@ -865,6 +534,24 @@ unsigned char CDynamixelMotor::get_node_address(void)
return (unsigned char)address;
}
+void CDynamixelMotor::set_node_address(unsigned char dev_id)
+{
+ if(this->dynamixel_dev==NULL)
+ {
+ /* handle exceptions */
+ }
+ 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;
+ }
+ }
+}
std::string CDynamixelMotor::get_model(void)
{
@@ -1423,19 +1110,20 @@ void CDynamixelMotor::enable_torque_control(void)
min.push_back(0.0);
max.push_back(0.0);
- this->set_position_range(0x0001,min,max);
+ this->set_position_range(min,max);
+ this->disable_position_feedback();
this->torque_control=true;
}
void CDynamixelMotor::disable_torque_control(void)
{
- std::vector<float> rate,min,max;
+ std::vector<float> min,max;
min.push_back(0.0);
max.push_back(300.0);
- this->set_position_range(0x0001,min,max);
- rate.push_back(100.0);
- this->enable_position_feedback(0x0001,rate);
+ this->set_position_range(min,max);
+ this->config_position_feedback(fb_polling,100.0);
+ this->enable_position_feedback();
this->torque_control=false;
}
@@ -1481,5 +1169,11 @@ CDynamixelMotor::~CDynamixelMotor()
this->control.ccw_compliance_margin=0x00;
this->control.cw_compliance_slope=0x20;
this->control.ccw_compliance_slope=0x20;
+ if(this->dynamixel_dev!=NULL)
+ {
+ this->dyn_server->free_device(this->dynamixel_dev->get_id());
+ delete this->dynamixel_dev;
+ this->dynamixel_dev=NULL;
+ }
}
diff --git a/src/dynamixel_motor.h b/src/dynamixel_motor.h
index 4fba3cb..decd03c 100644
--- a/src/dynamixel_motor.h
+++ b/src/dynamixel_motor.h
@@ -106,272 +106,119 @@ class CDynamixelMotor : public CMotorControl
*
*/
bool torque_control;
- protected:
/**
* \brief
*
*/
- virtual void angles_to_controller(short int motors,std::vector<float>& angles,std::vector<long int>& counts);
+ long int current_position;
+ protected:
/**
* \brief
*
*/
- virtual void speeds_to_controller(short int motors,std::vector<float>& angles,std::vector<long int>& counts);
- /**
- * \brief
- *
- */
- virtual void accels_to_controller(short int motors,std::vector<float>& angles,std::vector<long int>& counts);
+ virtual void angles_to_controller(std::vector<float>& angles,std::vector<float>& counts);
/**
* \brief
*
*/
- virtual void controller_to_angles(short int motors,std::vector<long int>& counts,std::vector<float>& angles);
+ virtual void speeds_to_controller(std::vector<float>& angles,std::vector<float>& counts);
/**
- * \brief
+ * \brief
*
*/
- virtual void controller_to_speeds(short int motors,std::vector<long int>& counts,std::vector<float>& angles);
+ virtual void accels_to_controller(std::vector<float>& angles,std::vector<float>& counts);
/**
* \brief
*
*/
- virtual void controller_to_accels(short int motors,std::vector<long int>& counts,std::vector<float>& angles);
- /**
- * \brief
- *
- */
- virtual void cont_set_accel_range(short int motors,std::vector<long int>& min, std::vector<long int>& max);
+ virtual void controller_to_angles(std::vector<float>& counts,std::vector<float>& angles);
/**
- * \brief
- *
- */
- virtual void cont_set_velocity_range(short int motors,std::vector<long int>& min, std::vector<long int>& max);
- /**
- * \brief
- *
- */
- virtual void cont_set_position_range(short int motors,std::vector<long int>& min, std::vector<long int>& max);
- /**
- * \brief
+ * \brief
*
*/
- virtual void cont_set_encoder_resolution(short int motors,std::vector<int>& enc_res);
+ virtual void controller_to_speeds(std::vector<float>& counts,std::vector<float>& angles);
/**
- * \brief
+ * \brief
*
*/
- virtual void cont_set_gear_factor(short int motors,std::vector<float>& gear);
+ virtual void controller_to_accels(std::vector<float>& counts,std::vector<float>& angles);
/**
* \brief
*
*/
- virtual void cont_set_current_limit(short int motors,std::vector<float>& current);
+ virtual void cont_set_position_range(std::vector<float>& min, std::vector<float>& max);
/**
* \brief
*
*/
- virtual void cont_get_accel_range(short int motors,std::vector<long int>& min, std::vector<long int>& max);
+ virtual void cont_get_position_range(std::vector<float>& min, std::vector<float>& max);
/**
* \brief
*
*/
- virtual void cont_get_velocity_range(short int motors,std::vector<long int>& min, std::vector<long int>& max);
+ virtual void cont_get_encoder_resolution(std::vector<int>& enc_res);
/**
* \brief
*
*/
- virtual void cont_get_position_range(short int motors,std::vector<long int>& min, std::vector<long int>& max);
- /**
- * \brief
- *
- */
- virtual void cont_get_encoder_resolution(short int motors,std::vector<int>& enc_res);
- /**
- * \brief
- *
- */
- virtual void cont_get_gear_factor(short int motors,std::vector<float>& gear);
- /**
- * \brief
- *
- */
- virtual void cont_get_current_limit(short int motors,std::vector<float>& current);
- /**
- * \brief
- *
- */
- virtual void cont_set_absolute_motion(short int motors);
- /**
- * \brief
- *
- */
- virtual void cont_set_relative_motion(short int motors);
- /**
- * \brief
- *
- */
- virtual void cont_enable_position_feedback(short int motors,std::vector<float>& sample_rate);
- /**
- * \brief
- *
- */
- virtual void cont_enable_velocity_feedback(short int motors,std::vector<float>& sample_rate);
- /**
- * \brief
- *
- */
- virtual void cont_enable_acceleration_feedback(short int motors,std::vector<float>& sample_rate);
- /**
- * \brief
- *
- */
- virtual void cont_enable_current_feedback(short int motors,std::vector<float>& sample_rate);
- /**
- * \brief
- *
- */
- virtual void cont_enable_limits_feedback(short int motors,std::vector<float>& sample_rate);
- /**
- * \brief
- *
- */
- virtual void cont_disable_position_feedback(short int motors);
- /**
- * \brief
- *
- */
- virtual void cont_disable_velocity_feedback(short int motors);
- /**
- * \brief
- *
- */
- virtual void cont_disable_acceleration_feedback(short int motors);
- /**
- * \brief
- *
- */
- virtual void cont_disable_current_feedback(short int motors);
- /**
- * \brief
- *
- */
- virtual void cont_disable_limits_feedback(short int motors);
- /**
- * \brief
- *
- */
- virtual void cont_get_position(short int motors,std::vector<long int>& position);
- /**
- * \brief
- *
- */
- virtual void cont_get_velocity(short int motors,std::vector<long int>& velocity);
- /**
- * \brief
- *
- */
- virtual void cont_get_acceleration(short int motors,std::vector<long int>& accel);
- /**
- * \brief
- *
- */
- virtual void cont_get_current(short int motors,std::vector<float>& current);
- /**
- * \brief
- *
- */
- virtual void cont_get_limits(short int motors,std::vector<short int>& limits);
+ virtual void cont_get_gear_factor(std::vector<float>& gear);
/**
* \brief
*
*/
- virtual void cont_wait_for_events(std::vector<short int>& events);
+ virtual std::vector<float> cont_get_position(void);
/**
* \brief
*
*/
- virtual void cont_set_gpio(int gpio_id,gpio_type type);
+ virtual std::vector<float> cont_get_velocity(void);
/**
* \brief
*
*/
- virtual void cont_write_gpio(int gpio_id,bool value);
+ virtual void cont_enable(std::vector<bool> &enable);
/**
* \brief
*
*/
- virtual void cont_write_gpio(int gpio_id,float value);
+ virtual void cont_disable(std::vector<bool> &disable);
/**
* \brief
*
*/
- virtual float cont_read_gpio(int gpio_id);
+ virtual void cont_load(std::vector<float>& position, std::vector<float>& velocity, std::vector<float>& accel);
/**
* \brief
*
*/
- virtual void cont_flush_hardware_queue(short int motors);
+ virtual void cont_load(std::vector<float>& velocity, std::vector<float>& accel);
/**
* \brief
*
*/
- virtual void cont_config(void *config);
+ virtual void cont_move(void);
/**
* \brief
*
*/
- virtual void cont_save_config(std::string& filename);
+ virtual void cont_stop(void);
/**
* \brief
*
*/
- virtual void cont_load_config(std::string& filename);
- /**
- * \brief
- *
- */
- virtual void cont_enable(short int motors);
- /**
- * \brief
- *
- */
- virtual void cont_disable(short int motors);
- /**
- * \brief
- *
- */
- virtual void cont_home(short int motors);
- /**
- * \brief
- *
- */
- virtual void cont_load(short int motors,std::vector<long int>& position, std::vector<long int>& velocity, std::vector<long int>& accel);
- /**
- * \brief
- *
- */
- virtual void cont_load(short int motors,std::vector<long int>& velocity, std::vector<long int>& accel);
- /**
- * \brief
- *
- */
- virtual void cont_move(short int motors);
- /**
- * \brief
- *
- */
- virtual void cont_reset(void);
+ virtual void cont_close(void);
+#ifdef _HAVE_XSD
/**
* \brief
*
*/
- virtual void cont_stop(short int motors);
+ virtual void cont_load_config(std::string &filename);
/**
* \brief
*
*/
- virtual void cont_close(void);
+ virtual void cont_save_config(std::string &filename);
+#endif
/**
* \brief
*
@@ -393,6 +240,11 @@ class CDynamixelMotor : public CMotorControl
*
*/
unsigned char get_node_address(void);
+ /**
+ * \brief
+ *
+ */
+ void set_node_address(unsigned char dev_id);
/**
* \brief
*
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index c70cb6c..640087b 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -4,3 +4,14 @@ 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})
diff --git a/src/examples/test_dynamixel_motor.cpp b/src/examples/test_dynamixel_motor.cpp
index 5d9eed1..e42c476 100755
--- a/src/examples/test_dynamixel_motor.cpp
+++ b/src/examples/test_dynamixel_motor.cpp
@@ -5,93 +5,97 @@
#include "dynamixel_motor.h"
#include <iostream>
-std::string cont1_name="AX-12+_1";
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::vector<std::string> motion_done_1;
- std::vector<int> ids;
- std::list<std::string> events;
- CDynamixelMotor *cont1;
- std::vector<float> pos1,vel1,acc1;
- std::vector<float> pos2,max,min;
- std::vector<float> position;
- std::vector<float> current;
- short int motors=0x0001;
+ std::list<std::string> events1,events2,events3;
+ CDynamixelMotor *cont1,*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)
{
- events.push_back(dyn_server->get_scan_done_event_id());
- dyn_server->start_scan(0);
- 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,ids[0]);
- cont1->enable(motors);
- cont1->get_position_range(motors,min,max);
- std::cout << min[0] << "," << max[0] << std::endl;
- min[0]=0.0;
- max[0]=300.0;
- cont1->set_position_range(motors,min,max);
- cont1->get_position(motors,position);
- std::cout << "Current position of device " << ids[0] << ": " << position[0] << std::endl;
- cont1->get_blend_complete_event(motors,motion_done_1);
- events.clear();
- events.push_back(motion_done_1[0]);
- pos1.push_back(position[0]+50.0);
- pos2.push_back(position[0]-50.0);
- vel1.push_back(10.0);
- acc1.push_back(10.0);
+ cont2=new CDynamixelMotor(cont2_name,0,1);
+ cont2->close();
+ delete cont2;
+ cont2=new CDynamixelMotor(cont2_name,0,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
+ cont3=new CDynamixelMotor(cont3_name,0,15);
+ 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 14: " << position[0] << std::endl;
+ position=cont3->get_position();
+ std::cout << "Current position of device 15: " << position[0] << std::endl;
+ vel1[0]=100.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);
- for(i=0;i<10;i++)
+ pos1[0]=290.0;
+ pos2[0]=10.0;
+ for(i=0;i<2;i++)
{
- event_server->wait_all(events);
- std::cout << "moving left ..." << std::endl;
- cont1->load(motors,pos1,vel1,acc1);
- cont1->move(motors);
- event_server->wait_all(events);
- std::cout << "moving right..." << std::endl;
- cont1->load(motors,pos2,vel1,acc1);
- cont1->move(motors);
+ event_server->wait_all(events2);
+ std::cout << "servo 14 moving left ..." << std::endl;
+ cont2->load(pos1,vel1,acc1);
+ cont2->move();
+ event_server->wait_all(events2);
+ std::cout << "servo 14 moving right ..." << std::endl;
+ cont2->load(pos2,vel1,acc1);
+ cont2->move();
+ event_server->wait_all(events2);
+ std::cout << "servo 14 centering ..." << std::endl;
+ cont2->load(pos0,vel1,acc1);
+ cont2->move();
+ sleep(1);
+ event_server->wait_all(events3);
+ std::cout << "servo 15 moving left ..." << std::endl;
+ cont3->load(pos1,vel1,acc1);
+ cont3->move();
+ event_server->wait_all(events3);
+ std::cout << "servo 15 moving right ..." << std::endl;
+ cont3->load(pos2,vel1,acc1);
+ cont3->move();
+ event_server->wait_all(events3);
+ std::cout << "servo 15 centering ..." << std::endl;
+ cont3->load(pos0,vel1,acc1);
+ cont3->move();
+ sleep(1);
}
}
}catch(CException &e){
std::cout << e.what() << std::endl;
}
return 0;
-*/
- try{
- if(dyn_server->get_num_buses()>0)
- {
- events.push_back(dyn_server->get_scan_done_event_id());
- dyn_server->start_scan(0);
- 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,ids[0]);
- cont1->enable(motors);
- cont1->enable_torque_control();
- cont1->get_position(motors,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_dynamixel_motor_scan.cpp b/src/examples/test_dynamixel_motor_scan.cpp
new file mode 100755
index 0000000..ee41657
--- /dev/null
+++ b/src/examples/test_dynamixel_motor_scan.cpp
@@ -0,0 +1,52 @@
+#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->set_bus_id(0);
+ 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,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
new file mode 100755
index 0000000..93a088b
--- /dev/null
+++ b/src/examples/test_sequence.cpp
@@ -0,0 +1,63 @@
+#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 seq_name="test_sequence";
+std::string seq_file="../src/xml/test_motion.xml";
+std::string seq_file2="../src/xml/test_motion2.xml";
+std::string group_name="test";
+std::string cont_config_file="../src/xml/base_dyn_config.xml";
+
+int main(int argc, char * argv[])
+{
+ try{
+ CEventServer *event_server=CEventServer::instance();
+ std::list<std::string> events;
+ CMotorGroup group(group_name);
+ CMotionSequence sequence(seq_name);
+ CDynamixelMotor cont1(cont_name1,0,1);
+ CDynamixelMotor cont2(cont_name2,0,15);
+
+ 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);
+ 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);
+ }
+#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
new file mode 100755
index 0000000..5112a62
--- /dev/null
+++ b/src/xml/CMakeLists.txt
@@ -0,0 +1,45 @@
+#check the existance of the xsd library
+IF(EXISTS "/usr/include/xsd/cxx")
+ SET(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -D_HAVE_XSD" PARENT_SCOPE)
+ SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -D_HAVE_XSD" PARENT_SCOPE)
+ SET(XSD_FOUND TRUE)
+ MESSAGE(STATUS "Found the XML library ... adding support for XML files")
+ FIND_LIBRARY(XSD_LIBRARY
+ NAMES xerces-c
+ PATHS /usr/lib /usr/local/lib)
+ELSE(EXISTS "/usr/include/xsd/cxx")
+ MESSAGE(STATUS "XML library not found ... it will be impossible to handle XML files")
+ENDIF(EXISTS "/usr/include/xsd/cxx")
+
+IF(XSD_FOUND)
+ SET(XSD_LIBRARY ${XSD_LIBRARY} PARENT_SCOPE)
+ SET(XSD_FILES dynamixel_motor_cfg_file.xsd)
+
+ IF(XSD_FILES)
+ SET(XSD_PATH ${CMAKE_CURRENT_SOURCE_DIR})
+
+ FOREACH(xsd_file ${XSD_FILES})
+ STRING(REGEX REPLACE "xsd" "cxx" xsd_source ${xsd_file})
+ SET(XSD_SOURCES_INT ${XSD_SOURCES_INT} ${XSD_PATH}/${xsd_source})
+ SET(XSD_SOURCES ${XSD_SOURCES} ${XSD_PATH}/${xsd_source})
+ STRING(REGEX REPLACE "xsd" "hxx" xsd_header ${xsd_file})
+ SET(XSD_HEADERS_INT ${XSD_HEADERS_INT} ${XSD_PATH}/${xsd_header})
+ SET(XSD_HEADERS ${XSD_HEADERS} ${XSD_PATH}/${xsd_header})
+ SET(XSD_PATH_FILES ${XSD_PATH_FILES} ${XSD_PATH}/${xsd_file})
+ ENDFOREACH(xsd_file)
+
+ SET(XSD_SOURCES ${XSD_SOURCES_INT} PARENT_SCOPE)
+ SET(XSD_HEADERS ${XSD_HEADERS_INT} PARENT_SCOPE)
+
+ 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}
+ WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
+ DEPENDS ${XSD_PATH_FILES}
+ COMMENT "Parsing the xml template file ${XSD_FILES}")
+
+ INSTALL(FILES ${XSD_PATH_FILES} DESTINATION include/iridrivers/xml)
+ INSTALL(FILES ${XSD_HEADERS_INT} DESTINATION include/iridrivers/xml)
+ ENDIF(XSD_FILES)
+ENDIF(XSD_FOUND)
diff --git a/src/xml/base_dyn_config.xml b/src/xml/base_dyn_config.xml
new file mode 100755
index 0000000..3d684bb
--- /dev/null
+++ b/src/xml/base_dyn_config.xml
@@ -0,0 +1,37 @@
+<?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>../src/xml/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>10.0</rate>
+ </limits_feedback>
+</motor_config>
diff --git a/src/xml/dyn_config.xml b/src/xml/dyn_config.xml
new file mode 100755
index 0000000..4a7cbbd
--- /dev/null
+++ b/src/xml/dyn_config.xml
@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+
+<dynamixel_motor_config xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
+ xsi:noNamespaceSchemaLocation="dynamixel_motor_cfg_file.xsd">
+
+ <temp_limit>85</temp_limit>
+ <max_voltage>190</max_voltage>
+ <min_voltage>60</min_voltage>
+ <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>
+</dynamixel_motor_config>
diff --git a/src/xml/dynamixel_motor_cfg_file.xsd b/src/xml/dynamixel_motor_cfg_file.xsd
new file mode 100755
index 0000000..7d6b76c
--- /dev/null
+++ b/src/xml/dynamixel_motor_cfg_file.xsd
@@ -0,0 +1,53 @@
+<?xml version="1.0"?>
+
+<!--
+
+file : dynamixel_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:simpleType name="alarm_t">
+ <xsd:restriction base="xsd:string">
+ <xsd:enumeration value="instruction_error"/>
+ <xsd:enumeration value="overload_error"/>
+ <xsd:enumeration value="checksum_error"/>
+ <xsd:enumeration value="range_error"/>
+ <xsd:enumeration value="overheating_error"/>
+ <xsd:enumeration value="angle_limit_error"/>
+ <xsd:enumeration value="input_voltage_error"/>
+ </xsd:restriction>
+ </xsd:simpleType>
+
+ <xsd:complexType name="dynamixel_motor_config_t">
+ <xsd:sequence>
+ <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>
+ <xsd:element name="cw_comp_margin" type="xsd:unsignedByte">
+ </xsd:element>
+ <xsd:element name="ccw_comp_margin" type="xsd:unsignedByte">
+ </xsd:element>
+ <xsd:element name="cw_comp_slope" type="xsd:unsignedByte">
+ </xsd:element>
+ <xsd:element name="ccw_comp_slope" type="xsd:unsignedByte">
+ </xsd:element>
+ <xsd:element name="punch" type="xsd:unsignedByte">
+ </xsd:element>
+ </xsd:sequence>
+ </xsd:complexType>
+
+ <xsd:element name="dynamixel_motor_config" type="dynamixel_motor_config_t">
+ </xsd:element>
+
+</xsd:schema>
diff --git a/src/xml/test_motion.xml b/src/xml/test_motion.xml
new file mode 100755
index 0000000..6002e3a
--- /dev/null
+++ b/src/xml/test_motion.xml
@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+
+<sequence xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
+ xsi:noNamespaceSchemaLocation="motion_sequence_file.xsd">
+ <num_motors>2</num_motors>
+ <num_steps>10</num_steps>
+ <control>position</control>
+ <motion>absolute</motion>
+ <step>
+ <position>0.0</position>
+ <position>150.0</position>
+ <velocity>100.0</velocity>
+ <velocity>100.0</velocity>
+ <delay>0</delay>
+ </step>
+</sequence>
--
GitLab