The Dynamixel motor group class is working with degrees, degrees/s and degrees/s2.

Added the open_loop confioguration to the TMotorConfig Structure

src/dynamixel_motor.cpp

@@ -53,6 +53,7 @@ CDynamixelMotor::CDynamixelMotor(std::string& cont_id,unsigned char bus_id,int b
     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();

src/dynamixel_motor_group.cpp

@@ -27,11 +27,11 @@ CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id):CMotorGroup(gr
 
 void CDynamixelMotorGroup::move(std::vector<float> &position,std::vector<float> &velocity,std::vector<float> &acceleration)
 {
+  std::vector<float> pos_count,vel_count,accel_count;
   std::vector< std::vector<unsigned char> > data;
-  unsigned char star_addrs;
+  std::vector<short int> count_value;
   unsigned int i=0;
 
-
   if(position.size()!=this->servo_id.size())
   {
     /* handle errors */
@@ -50,7 +50,6 @@ void CDynamixelMotorGroup::move(std::vector<float> &position,std::vector<float>
   else
   {
     try{
-      star_addrs=goal_pos;   
       data.resize(servo_id.size());
       for(i=0;i<servo_id.size();i++)
       {
@@ -59,15 +58,18 @@ void CDynamixelMotorGroup::move(std::vector<float> &position,std::vector<float>
 	   - goal_speed: 2 bytes */
 	data[i].resize(4);	
       }
+      // convert the input data
+      this->angles_to_controller(position,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)position[i])%256;
-	data[i][1]=position[i]/256;
+	data[i][0]=((int)(pos_count[i])%256);
+	data[i][1]=(int)(pos_count[i]/256);
 	data[i][2]=0x00;
 	data[i][3]=0x00;
       }
-      dyn_server->write_sync(servo_id,star_addrs,data);
+      dyn_server->write_sync(servo_id,goal_pos,data);
     }catch(CException &e){
       /* handle exceptions */
       throw; 

src/examples/test_dynamixel_motor_group.cpp

@@ -17,65 +17,60 @@ std::string config_file="../src/xml/base_dyn_config.xml";
 
 int main(int argc, char *argv[])
 {
-	CDynamixelServer *dyn_server=CDynamixelServer::instance();
-//	std::vector<float> pos(4),vel(4),acc(4);
-	std::vector<float> pos(2),vel(2),acc(2);
-	CDynamixelMotor *cont1,*cont2,*cont3,*cont4;
-	CDynamixelMotorGroup *group;
-	int dir=1,i=0;
-        std::string name;
-	
-	try{
+  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;
+  CMotorGroup *group;
+  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);
-//      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=new CMotorGroup(group_name);
       name=group->add_motor_control(cont1);
       group->config_motor_control(name,config_file);
       name=group->add_motor_control(cont2);
-      group->config_motor_control(name,config_file);
-//      group->add_motor_control(cont3);
-//      group->add_motor_control(cont4);
-      pos[0]=512;
-      pos[1]=512;
-//      pos[2]=512;
-//      pos[3]=512;
+
+      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);
-      usleep(1000000);
-      for(;;)
+      for(i=0;1<10;i++)
       {
-        if(pos[0]>=700)
-  	     	 dir=-1;
-  	     else if(pos[0]<=300)
-  	     	 dir=1;	  
-  	     for(i=0;i<pos.size();i++)
-  	       {
-  	       	pos[i]+=5*dir;      
-          }
+        pos[0]=100;
+        pos[1]=100;
+        event_server->wait_all(events);
         group->move(pos,vel,acc);
-        usleep(20000);
-       
-      }
+        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;
+    std::cout << e.what() << std::endl;
   }
 }
-      
-      
+
+

src/examples/test_dynamixel_motor_group_open_loop.cpp

@@ -15,6 +15,7 @@ int main(int argc, char *argv[])
 {
   CDynamixelServer *dyn_server=CDynamixelServer::instance();
   std::vector<float> pos(2),vel(2),acc(2);
+  std::vector<bool> enable(2,true);
   CDynamixelMotor *cont1,*cont2;
   CDynamixelMotorGroup *group;
   unsigned int i=0;
@@ -36,19 +37,20 @@ int main(int argc, char *argv[])
       group->add_motor_control(cont1);
       group->add_motor_control(cont2);
       group->use_open_loop_control();
-      pos[0]=512;
-      pos[1]=512;
+      pos[0]=150.0;
+      pos[1]=150.0;
       group->move(pos,vel,acc);
       sleep(2);
       for(;;)
       {
-	if(pos[0]>=700)
+        std::cout << pos[0] << std::endl;
+	if(pos[0]>=290)
 	  dir=-1;
-	else if(pos[0]<=300)
+	else if(pos[0]<=10)
 	  dir=1;	  
 	for(i=0;i<pos.size();i++)
 	{
-	  pos[i]+=5*dir;      
+	  pos[i]+=1.5*dir;      
 	}
 	group->move(pos,vel,acc);
 	usleep(20000);