Added an exception class for the CDynamixelMotor and CDynamixelMotorGroup classes.

Added support for the MX-28 servo model
Added support for the PID control of some servo models
Added an example of open loop control

src/CMakeLists.txt

 ADD_SUBDIRECTORY(xml)
 
 # edit the following line to add all the source code files of the library
-SET(sources dynamixel_motor.cpp dynamixel_motor_group.cpp)
+SET(sources dynamixel_motor.cpp dynamixel_motor_group.cpp dynamixel_motor_exceptions.cpp)
 # edit the following line to add all the header files of the library
-SET(headers dynamixel_motor.h dynamixel_motor_group.h)
+SET(headers dynamixel_motor.h dynamixel_motor_group.h dynamixel_motor_exceptions.h)
 
 FIND_PACKAGE(comm REQUIRED)
 

src/dynamixel_motor.h

@@ -30,6 +30,9 @@ typedef enum {
   up_cal=0x16,
   torque_en=0x18,
   led=0x19,
+  pid_p=0x1A,
+  pid_i=0x1B,
+  pid_d=0x1C,
   cw_comp_margin=0x1A,
   ccw_comp_margin=0x1B,
   cw_comp_slope=0x1C,
@@ -356,6 +359,16 @@ class CDynamixelMotor : public CMotorControl
      *  
      */ 
     void set_punch(short int punch_value);
+    /**
+     * \brief 
+     *  
+     */
+    void get_pid(unsigned char *p,unsigned char *i,unsigned char *d);
+    /**
+     * \brief 
+     *  
+     */
+    void set_pid(unsigned char p,unsigned char i,unsigned char d);
     /**
      * \brief 
      *  

src/dynamixel_motor_group.cpp

-#include "dynamixelexceptions.h"
+#include "dynamixel_motor_exceptions.h"
 #include "dynamixelserver.h"
 #include "eventexceptions.h"
 #include "dynamixel_motor_group.h"
+#include "dynamixel_motor.h"
 #include "ftdiserver.h"
 #include <math.h>
 #include <iostream>
 
-
-
 CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id):CMotorGroup(group_id)
 {
   if(group_id.size()==0)
   {
     /* handle exceptions */
-    throw CDynamixelServerException(_HERE_,"Invalid group identifier - empty string");
+    throw CDynamixelMotorGroupException(_HERE_,"Invalid group identifier - empty string");
   }
   else 
   {
@@ -23,40 +22,31 @@ CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id):CMotorGroup(gr
     this->servo_id[1]=0x04;
     this->servo_id[2]=0x03;
     this->servo_id[3]=0x05;
-    
+
   }
 }
 
-
-
-
-
-
 void CDynamixelMotorGroup::move(std::vector<float> &position,std::vector<float> &velocity,std::vector<float> &acceleration)
 {
-  
+  std::vector< std::vector<unsigned char> > data;
   unsigned char star_addrs;
- 
   unsigned int i=0;
-  int  num_instruc=4;
-  unsigned char const goal_pos=0x1E;
-  std::vector< std::vector<unsigned char> > data;
-  
-  
+
+
   if(position.size()!=this->servo_id.size())
   {
     /* handle errors */
-    throw CDynamixelServerException(_HERE_,"The number of position commands does not coincide with the total number of motors in the group");
+    throw CDynamixelMotorGroupException(_HERE_,"The number of position commands does not coincide with the total number of motors in the group");
   }
   else if(velocity.size()!=this->servo_id.size())
   {
     /* handle exceptions */
-    throw CDynamixelServerException(_HERE_,"The number of velocity commands does not coincide with the total number of motors in the group");
+    throw CDynamixelMotorGroupException(_HERE_,"The number of velocity commands does not coincide with the total number of motors in the group");
   }
   else if(acceleration.size()!=this->servo_id.size())
   {
     /* handle exceptions */
-    throw CDynamixelServerException(_HERE_,"The number of acceleration commands does not coincide with the total number of motors in the group");
+    throw CDynamixelMotorGroupException(_HERE_,"The number of acceleration commands does not coincide with the total number of motors in the group");
   }
   else
   {
@@ -65,34 +55,29 @@ void CDynamixelMotorGroup::move(std::vector<float> &position,std::vector<float>
       data.resize(servo_id.size());
       for(i=0;i<servo_id.size();i++)
       {
-    	  /* Number of instructions in bytes:
-    		  - goal_pos: 2 bytes
-    		  - goal_speed: 2 bytes */
-  	     data[i].resize(num_instruc);	
-  	   }
-  	   for(i=0;i<servo_id.size();i++)
-  	       {
-  	       			            	   	       	    
- 		      data[i][0]=((int)position[i])%256;
-            data[i][1]=position[i]/256;
-            data[i][2]=0xFF;
-            data[i][3]=0x03;
-          }
-        
-        dyn_server->write_sync(servo_id,star_addrs,data);
-        
-      
-      }catch(CException &e){
-      
-  		     std::cout << "Movement aborted" << std::endl;
-      }	
-      
-   }  
-  
+	/* Number of instructions in bytes:
+	   - goal_pos: 2 bytes
+	   - goal_speed: 2 bytes */
+	data[i].resize(4);	
+      }
+      for(i=0;i<servo_id.size();i++)
+      {
+
+	data[i][0]=((int)position[i])%256;
+	data[i][1]=position[i]/256;
+	data[i][2]=0xFF;
+	data[i][3]=0x03;
+      }
+      dyn_server->write_sync(servo_id,star_addrs,data);
+    }catch(CException &e){
+      /* handle exceptions */
+      throw; 
+    }	
+  }  
 }
 
 
 CDynamixelMotorGroup::~CDynamixelMotorGroup()
 {
-  
+
 }

src/examples/CMakeLists.txt

@@ -21,3 +21,9 @@ ADD_EXECUTABLE(test_dynamixel_motor_group test_dynamixel_motor_group.cpp)
 
 # edit the following line to add the necessary libraries
 TARGET_LINK_LIBRARIES(test_dynamixel_motor_group dynamixel_motor_cont ${comm_LIBRARY} ${motor_control_LIBRARY})
+
+# edit the following line to add the source code for the example and the name of the executable
+ADD_EXECUTABLE(test_dynamixel_motor_open_loop test_dynamixel_motor_open_loop.cpp)
+
+# edit the following line to add the necessary libraries
+TARGET_LINK_LIBRARIES(test_dynamixel_motor_open_loop dynamixel_motor_cont ${comm_LIBRARY} ${motor_control_LIBRARY})

src/examples/test_dynamixel_motor.cpp

@@ -23,10 +23,10 @@ int main(int argc, char *argv[])
   try{
     if(dyn_server->get_num_buses()>0)
     {
-      cont2=new CDynamixelMotor(cont2_name,0,1000000,1);
+      cont2=new CDynamixelMotor(cont2_name,0,1000000,11);
       cont2->close();
       delete cont2;
-      cont2=new CDynamixelMotor(cont2_name,0,1000000,1);
+      cont2=new CDynamixelMotor(cont2_name,0,1000000,11);
       enable[0]=true;
       cont2->enable(enable);
 #ifdef _HAVE_XSD
@@ -35,8 +35,8 @@ int main(int argc, char *argv[])
 #else
       events2.push_back(cont2->config_position_feedback(fb_polling,100.0));
 #endif
-      cont2->set_torque(10.0);
-      cont3=new CDynamixelMotor(cont3_name,0,1000000,2);
+      cont2->set_torque(100.0);
+      cont3=new CDynamixelMotor(cont3_name,0,1000000,21);
       enable[0]=true;
       cont3->enable(enable);
 #ifdef _HAVE_XSD

src/examples/test_dynamixel_motor_group.cpp

@@ -19,7 +19,7 @@ int main(int argc, char *argv[])
 	std::vector<float> pos(4),vel(4),acc(4);
 	CDynamixelMotor *cont1,*cont2,*cont3,*cont4;
 	CDynamixelMotorGroup *group;
-	int dir=1,i=0;
+	unsigned int dir=1,i=0;
 	
 	try{
     if(dyn_server->get_num_buses()>0)

src/examples/test_dynamixel_motor_open_loop.cpp

+#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; 
+}

src/xml/base_dyn_config.xml

@@ -22,16 +22,17 @@
   <position_feedback>
     <enabled>1</enabled>
     <mode>polling</mode>
-    <rate>1.0</rate>
+    <rate>100.0</rate>
   </position_feedback>
   <velocity_feedback>
     <enabled>0</enabled>
     <mode>polling</mode>
-    <rate>1.0</rate>
+    <rate>100.0</rate>
   </velocity_feedback>
   <limits_feedback>
     <enabled>0</enabled>
     <mode>polling</mode>
     <rate>1.0</rate>
   </limits_feedback>
+  <open_loop>0</open_loop>
 </motor_config>