Solved some bugs in the protocol version conversion functions.

dynamixel_base/src/dynamixel.c

@@ -51,10 +51,21 @@ unsigned char dyn_convert_v2_inst_packet(unsigned char *source, unsigned char *d
     destination[DYN_HEADER_OFF]=0xFF;
     destination[DYN_HEADER_OFF+1]=0xFF;
     destination[DYN_ID_OFF]=source[DYN2_ID_OFF];
-    destination[DYN_LENGTH_OFF]=source[DYN2_LENGTH_OFF];
+    destination[DYN_LENGTH_OFF]=2;//source[DYN2_LENGTH_OFF];
     destination[DYN_INST_OFF]=source[DYN2_INST_OFF];
-    for(i=0;i<destination[DYN_LENGTH_OFF];i++)
-      destination[DYN_DATA_OFF+i]=source[DYN2_DATA_OFF+i];
+    switch(source[DYN2_INST_OFF])
+    {
+      case DYN_PING: break;
+      case DYN_READ: destination[DYN_DATA_OFF]=source[DYN2_DATA_OFF];
+                     destination[DYN_DATA_OFF+1]=source[DYN2_DATA_OFF+2];
+                     destination[DYN_LENGTH_OFF]+=2;
+                     break;
+      case DYN_WRITE: destination[DYN_DATA_OFF]=source[DYN2_DATA_OFF];
+                      for(i=0;i<source[DYN2_DATA_OFF]-5;i++)
+                        destination[DYN_DATA_OFF+1+i]=source[DYN2_DATA_OFF+2+i];
+                      destination[DYN_LENGTH_OFF]+=source[DYN2_DATA_OFF]-4;
+                      break;
+    }
     dyn_set_checksum(destination);
     return 0x01;
   }
@@ -247,10 +258,10 @@ unsigned char dyn_convert_v2_status_packet(unsigned char *source, unsigned char
     destination[DYN_HEADER_OFF]=0xFF;
     destination[DYN_HEADER_OFF+1]=0xFF;
     destination[DYN_ID_OFF]=source[DYN2_ID_OFF];
-    destination[DYN_LENGTH_OFF]=source[DYN2_LENGTH_OFF];
+    destination[DYN_LENGTH_OFF]=source[DYN2_LENGTH_OFF]-2;
     destination[DYN_ERROR_OFF]=source[DYN2_ERROR_OFF];
-    for(i=0;i<destination[DYN_LENGTH_OFF];i++)
-      destination[DYN_DATA_OFF+i]=source[DYN2_DATA_OFF+i];
+    for(i=0;i<destination[DYN_LENGTH_OFF]-2;i++)
+      destination[DYN_DATA_OFF+i]=source[DYN2_DATA_OFF+1+i];
     dyn_set_checksum(destination); 
     return 0x01;
   }

dynamixel_base/src/dynamixel2.c

@@ -88,11 +88,25 @@ unsigned char dyn2_convert_v1_inst_packet(unsigned char *source, unsigned char *
   destination[DYN2_HEADER_OFF+2]=0xFD;
   destination[DYN2_RESERVED]=0x00;
   destination[DYN2_ID_OFF]=source[DYN_ID_OFF];
-  destination[DYN2_LENGTH_OFF]=source[DYN_LENGTH_OFF];
+  destination[DYN2_LENGTH_OFF]=3;//source[DYN_LENGTH_OFF];
   destination[DYN2_LENGTH_OFF+1]=0x00;
   destination[DYN2_INST_OFF]=source[DYN_INST_OFF];
-  for(i=0;i<source[DYN_LENGTH_OFF];i++)
-    destination[DYN2_DATA_OFF+i]=source[DYN_DATA_OFF+i];
+  switch(source[DYN_INST_OFF])
+  {
+    case DYN_PING: break;
+    case DYN_READ: destination[DYN2_DATA_OFF]=source[DYN_DATA_OFF];
+                   destination[DYN2_DATA_OFF+1]=0x00;
+                   destination[DYN2_DATA_OFF+2]=source[DYN_DATA_OFF+1];
+                   destination[DYN2_DATA_OFF+3]=0x00;
+                   (*((unsigned short int *)&destination[DYN2_LENGTH_OFF]))+=4;
+                   break;
+    case DYN_WRITE: destination[DYN2_DATA_OFF]=source[DYN_DATA_OFF];
+                    destination[DYN2_DATA_OFF+1]=0x00;
+                    for(i=0;i<source[DYN_LENGTH_OFF]-3;i++)
+                      destination[DYN2_DATA_OFF+2+i]=source[DYN_DATA_OFF+1+i];
+                    (*((unsigned short int *)&destination[DYN2_LENGTH_OFF]))+=source[DYN_LENGTH_OFF]-1;
+                    break;
+  }
   dyn2_set_checksum(destination);
 
   return 0x01;
@@ -417,12 +431,12 @@ unsigned char dyn2_convert_v1_status_packet(TDynInstruction inst,unsigned char *
   destination[DYN2_HEADER_OFF+2]=0xFD;
   destination[DYN2_RESERVED]=0x00;
   destination[DYN2_ID_OFF]=source[DYN_ID_OFF];
-  destination[DYN2_LENGTH_OFF]=source[DYN_LENGTH_OFF];
+  destination[DYN2_LENGTH_OFF]=source[DYN_LENGTH_OFF]+2;
   destination[DYN2_LENGTH_OFF+1]=0x00;
   destination[DYN2_INST_OFF]=inst;
   destination[DYN2_ERROR_OFF]=source[DYN_ERROR_OFF];
-  for(i=0;i<source[DYN_LENGTH_OFF];i++)
-    destination[DYN2_DATA_OFF+i]=source[DYN_DATA_OFF+i];
+  for(i=0;i<destination[DYN2_LENGTH_OFF]-4;i++)
+    destination[DYN2_DATA_OFF+1+i]=source[DYN_DATA_OFF+i];
   dyn2_set_checksum(destination);
 
   return 0x01;