Added sync_write comand to the dynamixel server class

src/dynamixelserver.cpp

@@ -534,3 +534,65 @@ void CDynamixelServer::close(void)
   this->bus_info.baud_rate=-1;
   this->dynamixel_access.exit();
 }
+
+void CDynamixelServer::write_sync(std::vector<unsigned char>& servo_ids,unsigned char start_addr, std::vector< std::vector<unsigned char> >& data)
+{
+  unsigned char *packet;
+  unsigned int i,j=0;
+  int  length=0;
+  
+  if(this->comm_dev!=NULL)
+  {
+    try{
+  	   if(servo_ids.size()>0)
+  	   {
+        length=8+(data[0].size()+1)*servo_ids.size();
+        packet=new unsigned char[length];
+        packet[0]=0xFF;
+        packet[1]=0xFF;
+        packet[2]=0XFE;
+        packet[3]=length-4;
+        packet[4]=0x83;
+        packet[5]=start_addr;
+        packet[6]=data[0].size();
+		 
+        for(i=0;i<servo_ids.size();i++)
+        {
+          packet[7+(data[i].size()+1)*i]= servo_ids[i];
+       	
+          for(j=0;j<data[i].size();j++)
+          {
+            packet[7+(data[i].size()+1)*i+1+j]=data[i][j];
+            
+          
+          }
+        }
+        packet[length-1]=0x00;
+        packet[length-1]=this->compute_checksum(packet,length); 
+        this->dynamixel_access.enter();
+      
+        if(this->comm_dev->write(packet,length)!=length)
+         {
+           delete[] packet;
+           this->dynamixel_access.exit();
+           throw CDynamixelServerException(_HERE_,"Unexpected error while writing to the communication device");
+         }
+         delete[] packet;
+         this->dynamixel_access.exit();
+      }
+  	 }catch(CException &e){
+     /* handle exceptions */
+     throw;
+    }
+  }else
+  {
+    /* handle exceptions */
+    throw CDynamixelServerException(_HERE_,"The communication device is not ready to send information");
+  }
+}
+
+
+
+
+
+

src/dynamixelserver.h

@@ -240,6 +240,11 @@ class CDynamixelServer
      *
      */
     void close(void); 
+    /**
+     * \brief
+     *
+     */    
+    void write_sync(std::vector<unsigned char>& servo_ids,unsigned char start_addr, std::vector< std::vector<unsigned char> >& data);
 };
 
 #endif

src/examples/CMakeLists.txt

@@ -9,3 +9,9 @@ ADD_EXECUTABLE(test_dynamixel_server_no_scan test_dynamixel_server_no_scan.cpp)
 
 # edit the following line to add the necessary libraries
 TARGET_LINK_LIBRARIES(test_dynamixel_server_no_scan dynamixel)
+
+# edit the following line to add the source code for the example and the name of the executable
+ADD_EXECUTABLE(test_dynamixel_sync test_dynamixel_sync.cpp)
+
+# edit the following line to add the necessary libraries
+TARGET_LINK_LIBRARIES(test_dynamixel_sync dynamixel)

src/examples/test_dynamixel_sync.cpp

+#include "eventexceptions.h"
+#include "dynamixelserver.h"
+#include <iostream>
+
+int main(int argc, char *argv[])
+{
+  CDynamixelServer *dyn_server=CDynamixelServer::instance();
+  CEventServer *event_server=CEventServer::instance();
+  int i,num_servos,num_instruc,dir,pos;
+  unsigned char const goal_pos=0x1E;
+  unsigned char star_addrs;
+  std::list<std::string> events;
+  std::vector< std::vector<unsigned char> > data;
+  std::vector<unsigned char> devices(3);
+  
+  try{
+    if(dyn_server->get_num_buses()>0)
+    {
+    
+      //events.push_back(dyn_server->get_scan_done_event_id());
+      dyn_server->config_bus(0,1000000);
+      //dyn_server->start_scan();
+      //event_server->wait_first(events);
+      //devices=dyn_server->get_device_ids();
+      data.resize(devices.size());
+      num_servos=data.size();
+      star_addrs=goal_pos;
+      pos=512;
+      dir=1;
+      devices[0]=0x0F;
+      devices[1]=0x01;
+      devices[2]=0x0C;
+      for(i=0;i<num_servos;i++)
+      {
+    	  /* Number of instructions in bytes:
+    		  - goal_pos: 2 bytes
+    		  - goal_speed: 2 bytes */
+        num_instruc=4;
+  	     data[i].resize(num_instruc);	
+  	   }
+  	   for(i=0;i<num_servos;i++)
+  	       {
+  	       	
+  		      data[i][0]=pos%256;
+            data[i][1]=(int)(pos/256);
+            data[i][2]=0x00;
+            data[i][3]=0x02;
+          }
+  	   dyn_server->write_sync(devices,star_addrs,data);
+      usleep(2000000);
+      std::cout << "move" << std::endl;
+  	   for(;;) 	   	
+  	   {	
+  	     if(pos>=1000)
+  	     	 dir=-1;
+  	     else if(pos<=24)
+  	     	 dir=1;	 
+  	     
+  	     
+  	     for(i=0;i<num_servos;i++)
+  	       {
+  	       	
+  		      data[i][0]=pos%256;
+            data[i][1]=(int)(pos/256);
+            data[i][2]=0xFF;
+            data[i][3]=0x03;
+          }
+          std::cout << pos << std::endl;
+        pos=pos+dir*2;
+        
+        dyn_server->write_sync(devices,star_addrs,data);
+        usleep(10000);std::cout << pos << std::endl;
+      }
+   }  
+  }catch(CException &e){
+  std::cout << "Movement aborted" << std::endl;
+  }	
+}
+ 
+ 
+ 
+ 
+ 
+ 
+