Full implementation of the version 2 protocol. Still untested.

dynamixel_base/include/dynamixel_master.h

@@ -3,16 +3,24 @@
 
 #include "comm.h"
 #include "dynamixel.h"
+#include "dynamixel2.h"
 
-#define       MAX_TX_BUFFER_LEN      1024
-#define       MAX_RX_BUFFER_LEN      1024
+
+#ifndef MAX_DYN_SLAVE_TX_BUFFER_LEN
+  #define       MAX_DYN_MASTER_TX_BUFFER_LEN      1024
+#endif
+
+#ifndef MAX_DYN_SLAVE_TX_BUFFER_LEN
+  #define       MAX_DYN_MASTER_RX_BUFFER_LEN      1024
+#endif
 
 typedef struct
 {
   TComm *comm_dev;
   TDynVersion version;
-  unsigned char tx_buffer[MAX_TX_BUFFER_LEN];
-  unsigned char rx_buffer[MAX_RX_BUFFER_LEN];
+  unsigned short int op_length;
+  unsigned char tx_buffer[MAX_DYN_MASTER_TX_BUFFER_LEN];
+  unsigned char rx_buffer[MAX_DYN_MASTER_RX_BUFFER_LEN];
   return_level_t return_level;
   unsigned char packet_ready;
   unsigned char received_bytes;
@@ -39,6 +47,8 @@ unsigned char dyn_master_write_table(TDynamixelMaster *master,unsigned char id,
 unsigned char dyn_master_reg_write(TDynamixelMaster *master,unsigned char id, unsigned short int address, unsigned short int length, unsigned char *data);
 unsigned char dyn_master_action(TDynamixelMaster *master);
 unsigned char dyn_master_sync_write(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data);
+unsigned char dyn_master_sync_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data);
 unsigned char dyn_master_bulk_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data);
+unsigned char dyn_master_bulk_write(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data);
 
 #endif

dynamixel_base/src/dynamixel_master.c

@@ -23,18 +23,50 @@ unsigned char dyn_master_irq_receive_cb(void *dyn_master,unsigned char byte)
             else
               dyn->received_bytes--;
             break;
-    case 2: if(byte!=0xFF)
+    case 2: if(byte==0xFD)// version 2 header
+            {
+              if(dyn->version==DYN_VER2)// the module is configured for version 2
+              {
+                dyn->rx_buffer[dyn->received_bytes]=byte;
+                dyn->received_bytes++;
+              }
+              else
+                dyn->received_bytes=0;// ignore packet and restart synchronization
+            }
+            else if(byte!=0xFF)
             {
               dyn->rx_buffer[dyn->received_bytes]=byte;
               dyn->received_bytes++;
             }
             break;
     case 3: dyn->rx_buffer[dyn->received_bytes]=byte;
+            if(dyn->version==DYN_VER1)
+            {
+              dyn->op_length=byte;
+              dyn->received_bytes=0;
+              /* finish reception by IRQ */
+              comm_cancel_irq_receive(dyn->comm_dev);
+              /* enable dma RX */
+              comm_receive_dma(dyn->comm_dev,&dyn->rx_buffer[4],dyn->op_length);
+            }
+            else
+              dyn->received_bytes++;
+            break;
+    case 4: dyn->rx_buffer[dyn->received_bytes]=byte;
+            dyn->received_bytes++;
+            break;
+    case 5: dyn->rx_buffer[dyn->received_bytes]=byte;
+            dyn->received_bytes++;
+            dyn->op_length=byte;
+            break;
+    case 6: dyn->rx_buffer[dyn->received_bytes]=byte;
+            dyn->received_bytes++;
+            dyn->op_length+=(byte<<8);
             dyn->received_bytes=0;
             /* finish reception by IRQ */
             comm_cancel_irq_receive(dyn->comm_dev);
             /* enable dma RX */
-            comm_receive_dma(dyn->comm_dev,&dyn->rx_buffer[4],byte);
+            comm_receive_dma(dyn->comm_dev,&dyn->rx_buffer[7],dyn->op_length);
             break;
     default: break;
   }
@@ -48,7 +80,7 @@ unsigned char dyn_master_dma_send_cb(void *dyn_master)
 
   dyn->set_rx_mode();
   if(dyn->rx_no_answer)
-      dyn->rx_no_answer=0x00;
+    dyn->rx_no_answer=0x00;
   else
   {
     /* enable RX interrupts */
@@ -143,10 +175,20 @@ unsigned char dyn_master_wait_reception(TDynamixelMaster *master)
   }
   master->packet_ready=0x00;
   // check the input packet checksum
-  if(dyn_check_checksum(master->rx_buffer)==0xFF)
-    return dyn_get_status_error(master->rx_buffer);
+  if(master->version==DYN_VER1)
+  {
+    if(dyn_check_checksum(master->rx_buffer)==0xFF)
+      return dyn_get_status_error(master->rx_buffer);
+    else
+      return DYN_CHECKSUM_ERROR;
+  }
   else
-    return DYN_CHECKSUM_ERROR;
+  {
+    if(dyn2_check_checksum(master->rx_buffer)==0xFF)
+      return dyn2_get_status_error(master->rx_buffer);
+    else
+      return DYN_CHECKSUM_ERROR;
+  }
 }
 
 /* public functions */
@@ -208,7 +250,10 @@ unsigned char dyn_master_ping(TDynamixelMaster *master,unsigned char id)
   unsigned char error;
 
   // generate the ping packet for the desired device
-  dyn_init_ping_packet(master->tx_buffer,id);
+  if(master->version==DYN_VER1)
+    dyn_init_ping_packet(master->tx_buffer,id);
+  else
+    dyn2_init_ping_packet(master->tx_buffer,id);
   master->rx_num_packets=0x01;
   master->rx_no_answer=0x00;
   // enable transmission
@@ -253,7 +298,10 @@ unsigned char dyn_master_read_table(TDynamixelMaster *master,unsigned char id,un
   unsigned char error;
 
   // generate the read packet for the desired device
-  dyn_init_read_packet(master->tx_buffer,id,address,length);
+  if(master->version==DYN_VER1)
+    dyn_init_read_packet(master->tx_buffer,id,address,length);
+  else
+    dyn2_init_read_packet(master->tx_buffer,id,address,length);
   master->rx_num_packets=0x01;
   if(master->return_level==no_return || id==DYN_BROADCAST_ID)
     master->rx_no_answer=0x01;
@@ -305,7 +353,10 @@ unsigned char dyn_master_write_table(TDynamixelMaster *master,unsigned char id,
   unsigned char error;
 
   // generate the write packet for the desired device
-  dyn_init_write_packet(master->tx_buffer,id,address,length,data);
+  if(master->version==DYN_VER1)
+    dyn_init_write_packet(master->tx_buffer,id,address,length,data);
+  else
+    dyn2_init_write_packet(master->tx_buffer,id,address,length,data);
   master->rx_num_packets=0x01;
   if(master->return_level==return_all && id!=DYN_BROADCAST_ID)
     master->rx_no_answer=0x00;
@@ -337,7 +388,10 @@ unsigned char dyn_master_reg_write(TDynamixelMaster *master,unsigned char id, un
   unsigned char error;
 
   // generate the registered write packet for the desired device
-  dyn_init_reg_write_packet(master->tx_buffer,id,address,length,data);
+  if(master->version==DYN_VER1)
+    dyn_init_reg_write_packet(master->tx_buffer,id,address,length,data);
+  else
+    dyn2_init_reg_write_packet(master->tx_buffer,id,address,length,data);
   master->rx_num_packets=0x01;
   if(master->return_level==return_all && id!=DYN_BROADCAST_ID)
     master->rx_no_answer=0x00;
@@ -369,7 +423,10 @@ unsigned char dyn_master_action(TDynamixelMaster *master)
   unsigned char error;
 
   // generate the action packet for the desired device
-  dyn_init_action_packet(master->tx_buffer);
+  if(master->version==DYN_VER1)
+    dyn_init_action_packet(master->tx_buffer);
+  else
+    dyn2_init_action_packet(master->tx_buffer);
   master->rx_num_packets=0x01;
   master->rx_no_answer=0x01;
   // enable transmission
@@ -395,7 +452,10 @@ unsigned char dyn_master_sync_write(TDynamixelMaster *master,unsigned char num,u
   unsigned char error;
 
   // generate the write packet for the desired device
-  dyn_init_sync_write_packet(master->tx_buffer,num,ids,address,length,data);
+  if(master->version==DYN_VER1)
+    dyn_init_sync_write_packet(master->tx_buffer,num,ids,address,length,data);
+  else
+    dyn2_init_sync_write_packet(master->tx_buffer,num,ids,address,length,data);
   master->rx_num_packets=0x01;
   master->rx_no_answer=0x01;
   // enable transmission
@@ -416,12 +476,66 @@ unsigned char dyn_master_sync_write(TDynamixelMaster *master,unsigned char num,u
   return error;
 }
 
+unsigned char dyn_master_sync_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data)
+{
+  unsigned char error=DYN_SUCCESS,i;
+
+  // generate the read packet for the desired device
+  if(master->version==DYN_VER2)
+  {
+    dyn2_init_sync_read_packet(master->tx_buffer,num,ids,address,length);
+    master->rx_num_packets=0x01;
+    if(master->return_level==no_return)
+      master->rx_no_answer=0x01;
+    else
+      master->rx_no_answer=0x00;
+    // enable transmission
+    master->set_tx_mode();
+    // send the data
+    if((error=dyn_master_send(master))!=DYN_SUCCESS)
+    {
+      master->set_rx_mode();
+      return error;
+    }
+    // wait for the transmission to end
+    if((error=dyn_master_wait_transmission(master))!=DYN_SUCCESS)
+    {
+      master->set_rx_mode();
+      return error;
+    }
+    // wait for the replay within the given timeout
+    if(master->return_level!=no_return)
+    {
+      for(i=0;i<num;i++)
+      {
+        if((error=dyn_master_wait_reception(master))==DYN_SUCCESS)
+        {
+          if(dyn2_get_read_status_data(master->rx_buffer,data[i].data_addr)!=length)// not enough data
+            error=DYN_INST_ERROR;
+        }
+      }
+    }
+  }
+
+  return error;
+}
+
 unsigned char dyn_master_bulk_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data)
 {
-  unsigned char error,i;
+  unsigned char error,i,ver1_address[255],ver1_length[255];
 
   // generate the read packet for the desired device
-  dyn_init_bulk_read_packet(master->tx_buffer,num,ids,address,length);
+  if(master->version==DYN_VER1)
+  {
+    for(i=0;i<num;i++)
+    {
+      ver1_address[i]=address[i];
+      ver1_length[i]=length[i];
+    }
+    dyn_init_bulk_read_packet(master->tx_buffer,num,ids,ver1_address,ver1_length);
+  }
+  else
+    dyn2_init_bulk_read_packet(master->tx_buffer,num,ids,address,length);
   master->rx_num_packets=0x01;
   if(master->return_level==no_return)
     master->rx_no_answer=0x01;
@@ -448,8 +562,16 @@ unsigned char dyn_master_bulk_read(TDynamixelMaster *master,unsigned char num,un
     {
       if((error=dyn_master_wait_reception(master))==DYN_SUCCESS)
       {
-        if(dyn_get_read_status_data(master->rx_buffer,data[i].data_addr)!=length[i])// not enough data
-          error=DYN_INST_ERROR;
+        if(master->version==DYN_VER1)
+        {
+          if(dyn_get_read_status_data(master->rx_buffer,data[i].data_addr)!=length[i])// not enough data
+            error=DYN_INST_ERROR;
+        }
+        else
+        {
+          if(dyn2_get_read_status_data(master->rx_buffer,data[i].data_addr)!=length[i])// not enough data
+            error=DYN_INST_ERROR;
+        }
       }
     }
   }
@@ -457,3 +579,32 @@ unsigned char dyn_master_bulk_read(TDynamixelMaster *master,unsigned char num,un
   return error;
 }
 
+unsigned char dyn_master_bulk_write(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data)
+{
+  unsigned char error=DYN_SUCCESS;
+
+  // generate the write packet for the desired device
+  if(master->version==DYN_VER2)
+  {
+    dyn2_init_bulk_write_packet(master->tx_buffer,num,ids,address,length,data);
+    master->rx_num_packets=0x01;
+    master->rx_no_answer=0x01;
+    // enable transmission
+    master->set_tx_mode();
+    // send the data
+    if((error=dyn_master_send(master))!=DYN_SUCCESS)
+    {
+      master->set_rx_mode();
+      return error;
+    }
+    // wait for the transmission to end
+    if((error=dyn_master_wait_transmission(master))!=DYN_SUCCESS)
+    {
+      master->set_rx_mode();
+      return error;
+    }
+  }
+
+  return error;
+}
+