Added the bulk_read, bulk_write and sync_read functions for the version 1 protocol.

Added a module to test the new firmware for AX12 servos.

src/dynamixelserver.cpp

@@ -73,6 +73,9 @@ void CDynamixelServer::send_instruction_packet_v1(dyn_inst_t inst,unsigned char
                    if(len!=0)
                      throw CDynamixelServerException(_HERE_,"Invalid data length");
                    break;
+    case dyn_bulk_read:
+    case dyn_bulk_write:
+    case dyn_sync_read:
     case dyn_sync_write: id=0xFE;
                          length=6+len;
                          packet=new unsigned char[length];
@@ -995,7 +998,7 @@ void CDynamixelServer::write_sync(std::vector<unsigned char>& servo_ids,unsigned
 
 void CDynamixelServer::read_sync(std::vector<unsigned char>& servo_ids,unsigned short int start_addr,unsigned short int length, std::vector< std::vector<unsigned char> >& data,dyn_version_t version)
 {
-  unsigned char *data_int=NULL,*data_out=NULL,error;
+  unsigned char *data_int=NULL,*data_out=NULL,error,length_v1;
   unsigned short length_v2;
   unsigned int i,j=0;
   int length_int=0;
@@ -1005,14 +1008,9 @@ void CDynamixelServer::read_sync(std::vector<unsigned char>& servo_ids,unsigned
     /* handle exceptions */
     throw CDynamixelServerException(_HERE_,"No servos specified");
   }
-  if(version==dyn_version1)
-  {
-    /* handle exception */
-    throw CDynamixelServerException(_HERE_,"Operation not supported in version 1");
-  }
-  else
-  {
-    try{
+  try{
+    if(version==dyn_version2)
+    {
       length_int=4+servo_ids.size();
       data_int=new unsigned char[length_int];
       data_int[0]=start_addr%256;
@@ -1051,14 +1049,53 @@ void CDynamixelServer::read_sync(std::vector<unsigned char>& servo_ids,unsigned
           throw e;
         } 
       }
-    }catch(CException &e){
+    }
+    else
+    {
+      length_int=2+servo_ids.size();
+      data_int=new unsigned char[length_int];
+      data_int[0]=(unsigned char)start_addr;
+      data_int[1]=(unsigned char)length;
+      for(i=0;i<servo_ids.size();i++)
+        data_int[2+i]=servo_ids[i];
+      this->send_instruction_packet_v1(dyn_sync_read,data_int,length_int);
+      /* read all the data from all the servos */
       if(data_int!=NULL)
       {
         delete[] data_int;
-        data_out=NULL;
+        data_int=NULL;
       }
-      throw e;
+      data.resize(servo_ids.size());
+      for(i=0;i<servo_ids.size();i++)
+      {
+        try{
+          error=this->receive_status_packet_v1(&data_out,&length_v1);
+          this->handle_error(error);
+          if(data_out!=NULL)
+          {
+            data[i].resize(length_v1);
+            for(j=0;j<length_v1;j++)
+              data[i][j]=data_out[j];
+            delete[] data_out;
+            data_out=NULL;
+          }
+        }catch(CException &e){
+          if(data_out!=NULL)
+          {
+            delete[] data_out;
+            data_out=NULL;
+          }
+          throw e;
+        } 
+      }
+    }
+  }catch(CException &e){
+    if(data_int!=NULL)
+    {
+      delete[] data_int;
+      data_out=NULL;
     }
+    throw e;
   }
 }
 
@@ -1083,14 +1120,9 @@ void CDynamixelServer::write_bulk(std::vector<unsigned char>& servo_ids,std::vec
     /* handle exceptions */
     throw CDynamixelServerException(_HERE_,"Too many or not enough data blocks");
   }
-  if(version==dyn_version1)
-  {
-    /* handle exception */
-    throw CDynamixelServerException(_HERE_,"Operation not supported in version 1");
-  }
-  else
-  {
-    try{
+  try{
+    if(version==dyn_version2)
+    {
       length=0;
       for(i=0;i<servo_ids.size();i++)
         length+=5+data[i].size();
@@ -1108,20 +1140,38 @@ void CDynamixelServer::write_bulk(std::vector<unsigned char>& servo_ids,std::vec
       }
       this->send_instruction_packet_v2(dyn_bulk_write,data_int,length);
       delete[] data_int;
-    }catch(CException &e){
-      if(data_int!=NULL)
+    }
+    else
+    {
+      length=0;
+      for(i=0;i<servo_ids.size();i++)
+        length+=3+data[i].size();
+      data_int=new unsigned char[length];
+      for(i=0;i<servo_ids.size();i++)
       {
-        delete[] data_int;
-        data_int=NULL;
+        data_int[offset]=servo_ids[i];
+        data_int[offset+1]=(unsigned char)start_addr[i];
+        data_int[offset+2]=(unsigned char)data[i].size();
+        for(j=0;j<data[i].size();j++)
+          data_int[offset+3+j]=data[i][j];
+        offset+=3+data[i].size();
       }
-      throw e;
+      this->send_instruction_packet_v1(dyn_bulk_write,data_int,length);
+      delete[] data_int;
+    }
+  }catch(CException &e){
+    if(data_int!=NULL)
+    {
+      delete[] data_int;
+      data_int=NULL;
     }
+    throw e;
   }
 }
 
 void CDynamixelServer::read_bulk(std::vector<unsigned char>& servo_ids,std::vector<unsigned short int> &start_addr,std::vector<unsigned short int> &length, std::vector< std::vector<unsigned char> >& data,dyn_version_t version)
 {
-  unsigned char *data_int=NULL,*data_out=NULL,error;
+  unsigned char *data_int=NULL,*data_out=NULL,error,length_v1;
   unsigned short int length_v2;
   unsigned int i,j=0;
   int length_int=0;
@@ -1141,14 +1191,9 @@ void CDynamixelServer::read_bulk(std::vector<unsigned char>& servo_ids,std::vect
     /* handle exceptions */
     throw CDynamixelServerException(_HERE_,"Too many or not enough read lengths");
   }
-  if(version==dyn_version1)
-  {
-    /* handle exception */
-    throw CDynamixelServerException(_HERE_,"Operation not supported in version 1");
-  }
-  else
-  {
-    try{
+  try{
+    if(version==dyn_version2)
+    {
       length_int=5*servo_ids.size();
       data_int=new unsigned char[length_int];
       for(i=0;i<servo_ids.size();i++)
@@ -1189,14 +1234,55 @@ void CDynamixelServer::read_bulk(std::vector<unsigned char>& servo_ids,std::vect
           throw e;
         }
       }
-    }catch(CException &e){
+    }
+    else
+    {
+      length_int=3*servo_ids.size();
+      data_int=new unsigned char[length_int];
+      for(i=0;i<servo_ids.size();i++)
+      {
+        data_int[i*3]=servo_ids[i];
+        data_int[i*3+1]=(unsigned char)start_addr[i];
+        data_int[i*3+2]=(unsigned char)length[i];
+      }
+      this->send_instruction_packet_v1(dyn_bulk_read,data_int,length_int);
+      /* read all the data from all the servos */
       if(data_int!=NULL)
       {
         delete[] data_int;
         data_int=NULL;
       }
-      throw e;
+      data.resize(servo_ids.size());
+      for(i=0;i<servo_ids.size();i++)
+      {
+        error=this->receive_status_packet_v1(&data_out,&length_v1);
+        try{
+          this->handle_error(error);
+          if(data_out!=NULL)
+          {
+            data[i].resize(length_v1);
+            for(j=0;j<length_v1;j++)
+              data[i][j]=data_out[j];
+            delete[] data_out;
+            data_out=NULL;
+          }
+        }catch(CException &e){
+          if(data_out!=NULL)
+          {
+            delete[] data_out;
+            data_out=NULL;
+          }
+          throw e;
+        }
+      }
     }
+  }catch(CException &e){
+    if(data_int!=NULL)
+    {
+      delete[] data_int;
+      data_int=NULL;
+    }
+    throw e;
   }
 }
 

src/examples/CMakeLists.txt

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

src/examples/test_dynamixel_bulk_read.cpp

@@ -21,11 +21,11 @@ int main(int argc, char *argv[])
       events.push_back(dyn_server->get_scan_done_event_id());
       events.push_back(dyn_server->get_scan_error_event_id());
       dyn_server->config_bus(0,1000000);
-      dyn_server->start_scan(dyn_version2);
+      dyn_server->start_scan(dyn_version1);
       event_id=event_server->wait_first(events);
       if(event_id==0)
       {
-        devices=dyn_server->get_device_ids(dyn_version2);
+        devices=dyn_server->get_device_ids(dyn_version1);
         num_servos=devices.size();
         start_addr.resize(num_servos);
         length.resize(num_servos); 
@@ -36,7 +36,7 @@ int main(int argc, char *argv[])
           start_addr[i]=0x1B;
           length[i]=3;
         }
-        dyn_server->read_bulk(servo_ids,start_addr,length,data,dyn_version2);
+        dyn_server->read_bulk(servo_ids,start_addr,length,data,dyn_version1);
         for(i=0;i<num_servos;i++)
         {
           std::cout << "PID parameters for the servo " << (int)servo_ids[i] << std::endl;

src/examples/test_dynamixel_server.cpp

@@ -41,6 +41,7 @@ int main(int argc, char *argv[])
     }
     else
       std::cout << "Error while scanning the bus: " << dyn_server->get_scan_error() << std::endl;
+    return 0;
     try{
       dyn_server->config_bus(0,1000000);
       dyn_server->start_scan();

src/examples/test_dynamixel_sync.cpp

@@ -8,7 +8,7 @@ int main(int argc, char *argv[])
   CEventServer *event_server=CEventServer::instance();
   std::vector< std::vector<unsigned char> > data;
   std::vector<unsigned char> servo_ids;
-  unsigned short int star_addrs=0x1E;
+  unsigned short int star_addrs=0x1C;
   int i,num_servos,pos=0,event_id;
   std::list<std::string> events;
   std::vector<int> devices;
@@ -31,21 +31,25 @@ int main(int argc, char *argv[])
         for(i=0;i<num_servos;i++)
         {
           servo_ids[i]=devices[i];
-	  data[i].resize(4);	
-          data[i][0]=pos%256;
-          data[i][1]=pos/256;
-          data[i][2]=0x00;
-          data[i][3]=0x02;
+	  data[i].resize(6);	
+          data[i][0]=0x00;
+          data[i][1]=0x01;
+          data[i][2]=pos%256;
+          data[i][3]=pos/256;
+          data[i][4]=0xFF;
+          data[i][5]=0x03;
         }
         dyn_server->write_sync(servo_ids,star_addrs,data,dyn_version1);
         sleep(2);
         pos=1023;
         for(i=0;i<num_servos;i++)
         {
-          data[i][0]=pos%256;
-          data[i][1]=pos/256;
-          data[i][2]=0x00;
-          data[i][3]=0x01;
+          data[i][0]=0x00;
+          data[i][1]=0x01;
+          data[i][2]=pos%256;
+          data[i][3]=pos/256;
+          data[i][4]=0xFF;
+          data[i][5]=0x03;
         }
         dyn_server->write_sync(servo_ids,star_addrs,data,dyn_version1);
         sleep(5);

src/examples/test_new_firmware.cpp

+#include "eventexceptions.h"
+#include "dynamixelserver_ftdi.h"
+#include "eventserver.h"
+#include <iostream>
+
+int main(int argc, char *argv[])
+{
+  CDynamixelServerFTDI *dyn_server=CDynamixelServerFTDI::instance();
+  CEventServer *event_server=CEventServer::instance();
+  int i,j,num_buses,event_id,num_dev,baudrate;
+  std::list<std::string> events;
+  std::vector<int> devices;
+  unsigned short int model;
+  unsigned char data[53];
+  CDynamixel *servo;
+
+  num_buses=dyn_server->get_num_buses();
+  std::cout << "Num. buses: " << num_buses << std::endl;
+  if(num_buses>0)
+  {
+    try{
+      events.push_back(dyn_server->get_scan_done_event_id());
+      events.push_back(dyn_server->get_scan_error_event_id());
+      dyn_server->config_bus(0,1000000);
+      dyn_server->start_scan();
+      event_id=event_server->wait_first(events);
+      if(event_id==0)
+      {
+        num_dev=dyn_server->get_num_devices();
+        std::cout << "Num. devices: " << std::dec << num_dev << std::endl;
+        baudrate=dyn_server->get_baudrate();
+        std::cout << "Baudrate: " << std::dec << baudrate << " bps" << std::endl;
+        devices=dyn_server->get_device_ids();
+        for(i=0;i<num_dev;i++)
+        {
+          servo=dyn_server->get_device(devices[i]);
+          servo->read_word_register(0x00,&model);
+          std::cout << "servo " << std::dec << devices[i] << " model " << (int)model << std::endl;
+          servo->read_registers(0x00,data,53);
+          for(j=0;j<53;j++)
+          {
+            std::cout << "  Reg " << std::hex << j << " value: " << std::hex << (int)data[j] << std::endl;
+          }
+          dyn_server->free_device(devices[i]);
+          delete servo;
+        }
+      }
+      else
+        std::cout << "Error while scanning the bus: " << dyn_server->get_scan_error() << std::endl;
+    }catch(CException &e){
+      std::cout << e.what() << std::endl;
+    }	
+  }
+}
+
+
+
+
+
+
+