diff --git a/src/dynamixel_motor.cpp b/src/dynamixel_motor.cpp
index 6ae7846381652e7b39a8183567286e376b856976..f2cb92e30877a6023117b571991e66106d0c6c3c 100644
--- a/src/dynamixel_motor.cpp
+++ b/src/dynamixel_motor.cpp
@@ -430,8 +430,13 @@ void CDynamixelMotor::cont_load_config(std::string &filename)
// 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());
+ 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());
+ 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());
+ }
this->set_punch(cfg->punch());
}catch (const xml_schema::exception& e){
std::ostringstream os;
@@ -445,14 +450,19 @@ void CDynamixelMotor::cont_save_config(std::string &filename)
{
xml_schema::namespace_infomap map;
float max_voltage, min_voltage;
- unsigned char cw_margin, ccw_margin;
- unsigned char cw_slope, ccw_slope;
+ unsigned char cw_margin=0x00, ccw_margin=0x00;
+ unsigned char cw_slope=0x00, ccw_slope=0x00;
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);
+ if(this->info.model=="EX-106" || this->info.model=="MX-28")
+ this->get_pid(&cw_margin,&ccw_margin,&cw_slope);
+ 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,
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index 8cb70e531d4d274cd444c46073f8f9cafec925d2..f9d55000831510d397cdd77bca238071c1bdc62a 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -33,3 +33,9 @@ ADD_EXECUTABLE(test_dynamixel_motor_group_open_loop test_dynamixel_motor_group_o
# 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})
+
+# 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)
+
+# edit the following line to add the necessary libraries
+TARGET_LINK_LIBRARIES(test_sequence_open_loop dynamixel_motor_cont ${comm_LIBRARY} ${motor_control_LIBRARY})
diff --git a/src/examples/test_dynamixel_motor_group_open_loop.cpp b/src/examples/test_dynamixel_motor_group_open_loop.cpp
index 64a451c3a7eca14bcff4eeed7527b21a2918beec..3e8a09f13ddcf2265c8bbf7424df7113d4a9b416 100755
--- a/src/examples/test_dynamixel_motor_group_open_loop.cpp
+++ b/src/examples/test_dynamixel_motor_group_open_loop.cpp
@@ -9,17 +9,16 @@
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";
int main(int argc, char *argv[])
{
CDynamixelServer *dyn_server=CDynamixelServer::instance();
std::vector<float> pos(2),vel(2),acc(2);
- CDynamixelMotor *cont1,*cont2,*cont3,*cont4;
+ CDynamixelMotor *cont1,*cont2;
CDynamixelMotorGroup *group;
- int dir=1,i=0;
+ unsigned int i=0;
+ int dir=1;
try{
if(dyn_server->get_num_buses()>0)
@@ -40,10 +39,9 @@ int main(int argc, char *argv[])
pos[0]=512;
pos[1]=512;
group->move(pos,vel,acc);
- sleep(5);
+ sleep(2);
for(;;)
{
- std::cout << pos[0] << std::endl;
if(pos[0]>=700)
dir=-1;
else if(pos[0]<=300)
diff --git a/src/examples/test_sequence_open_loop.cpp b/src/examples/test_sequence_open_loop.cpp
new file mode 100755
index 0000000000000000000000000000000000000000..1acfaa42804c21958e194a270672793dbca827de
--- /dev/null
+++ b/src/examples/test_sequence_open_loop.cpp
@@ -0,0 +1,86 @@
+#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 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{
+ CEventServer *event_server=CEventServer::instance();
+ std::list<std::string> events;
+ CMotorGroup 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;
+ }
+}