Added class dynamixel_motor_group

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)
+SET(sources dynamixel_motor.cpp dynamixel_motor_group.cpp)
 # edit the following line to add all the header files of the library
-SET(headers dynamixel_motor.h)
+SET(headers dynamixel_motor.h dynamixel_motor_group.h)
 
 FIND_PACKAGE(comm REQUIRED)
 

src/dynamixel_motor.cpp

@@ -382,7 +382,8 @@ void CDynamixelMotor::cont_stop(void)
   {
     try{
       this->dynamixel_dev->read_word_register(current_pos,&current_position);
-      this->dynamixel_dev->write_word_register(goal_pos,current_position);
+    this->dynamixel_dev->write_word_register(goal_pos,current_position);
+	
     }catch(CDynamixelAlarmException &e){
       /* handle dynamixel exception */
       if(e.get_alarm()&this->alarms)

src/dynamixel_motor_group.cpp

+#include "dynamixelexceptions.h"
+#include "dynamixelserver.h"
+#include "eventexceptions.h"
+#include "dynamixel_motor_group.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");
+  }
+  else 
+  {
+    this->dyn_server=CDynamixelServer::instance();
+    this->servo_id.resize(4);
+    this->servo_id[0]=0x02;
+    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)
+{
+  
+  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");
+  }
+  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");
+  }
+  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");
+  }
+  else
+  {
+    try{
+      star_addrs=goal_pos;   
+      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;
+      }	
+      
+   }  
+  
+}
+
+
+CDynamixelMotorGroup::~CDynamixelMotorGroup()
+{
+  
+}

src/dynamixel_motor_group.h

+#ifndef _DYNAMIXEL_MOTOR_GROUP
+#define _DYNAMIXEL_MOTOR_GROUP
+
+#include "threadserver.h"
+#include "eventserver.h"
+#include "motor_group.h"
+#include "mutex.h"
+#include <vector>
+
+
+class CDynamixelMotorGroup  : public CMotorGroup
+{
+
+		private:
+			
+			std::vector<unsigned char> servo_id;
+			
+			CDynamixelServer *dyn_server;
+			
+		public: 
+		
+			CDynamixelMotorGroup(std::string &group_id);
+			
+			
+		   void move(std::vector<float> &position,std::vector<float> &velocity,std::vector<float> &acceleration);	
+
+
+			virtual ~CDynamixelMotorGroup();
+};
+
+
+#endif

src/examples/CMakeLists.txt

@@ -15,3 +15,9 @@ 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})
+
+# 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})

src/examples/test_dynamixel_motor_group.cpp

+#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 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(4),vel(4),acc(4);
+	CDynamixelMotor *cont1,*cont2,*cont3,*cont4;
+	CDynamixelMotorGroup *group;
+	int dir=1,i=0;
+	
+	try{
+    if(dyn_server->get_num_buses()>0)
+    {
+      cont1=new CDynamixelMotor(cont1_name,0,1000000,2);
+      cont2=new CDynamixelMotor(cont2_name,0,1000000,4);
+      cont3=new CDynamixelMotor(cont3_name,0,1000000,3);
+      cont4=new CDynamixelMotor(cont4_name,0,1000000,5);
+      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);
+      cont3->set_compliance_slope(254,254);
+      cont3->set_punch(16);
+      cont3->set_compliance_margin(1,1);
+      cont4->set_compliance_slope(254,254);
+      cont4->set_punch(32);
+      cont4->set_compliance_margin(1,1);
+      
+      group=new CDynamixelMotorGroup(group_name);
+      group->add_motor_control(cont1);
+      group->add_motor_control(cont2);
+      group->add_motor_control(cont3);
+      group->add_motor_control(cont4);
+      pos[0]=512;
+      pos[1]=512;
+      pos[2]=512;
+      pos[3]=512;
+      group->move(pos,vel,acc);
+      usleep(1000000);
+      for(;;)
+      {
+        if(pos[0]>=700)
+  	     	 dir=-1;
+  	     else if(pos[0]<=300)
+  	     	 dir=1;	  
+  	     for(i=0;i<pos.size();i++)
+  	       {
+  	       	pos[i]+=5*dir;      
+          }
+        group->move(pos,vel,acc);
+        usleep(20000);
+       
+      }
+    }
+  }catch(CException &e){
+  std::cout << e.what() << std::endl;
+  }
+}
+      
+      

src/xml/base_dyn_config.xml

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

src/xml/dyn_config.xml

@@ -6,9 +6,9 @@
   <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>
+  <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>
 </dynamixel_motor_config>