Changed some 8 bit control variables to 16 bit variables in the version 2 protocol.

dynamixel_base/src/dynamixel2.c

@@ -94,7 +94,7 @@ inline TDynInstruction dyn2_get_instruction(unsigned char *packet)
 
 unsigned char dyn2_convert_v1_inst_packet(unsigned char *source, unsigned char *destination)
 {
-  unsigned char i;
+  unsigned short int i;
 
   destination[DYN2_HEADER_OFF]=0xFF;
   destination[DYN2_HEADER_OFF+1]=0xFF;
@@ -193,7 +193,7 @@ unsigned short int dyn2_get_write_data(unsigned char *packet,unsigned char *data
 /* registered write instruction */
 void dyn2_init_reg_write_packet(unsigned char *packet,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data)
 {
-  unsigned short int  i;
+  unsigned short int i;
 
   packet[DYN2_HEADER_OFF]=0xFF;
   packet[DYN2_HEADER_OFF+1]=0xFF;
@@ -220,7 +220,7 @@ inline unsigned short int dyn2_get_reg_write_length(unsigned char *packet)
 
 unsigned short int dyn2_get_reg_write_data(unsigned char *packet,unsigned char *data)
 {
-  unsigned char i,length;
+  unsigned short int i,length;
 
   length=dyn2_get_reg_write_length(packet);
   for(i=0;i<length;i++)
@@ -261,7 +261,8 @@ void dyn2_init_reset_packet(unsigned char *packet,unsigned char id,unsigned char
 /* sync write instruction */
 void dyn2_init_sync_write_packet(unsigned char *packet,unsigned char num_servos,unsigned char *servo_ids,unsigned short int address,unsigned short int length,TWriteData *data)
 {
-  unsigned char i,j;
+  unsigned char i;
+  unsigned short int j;
 
   packet[DYN2_HEADER_OFF]=0xFF;
   packet[DYN2_HEADER_OFF+1]=0xFF;
@@ -381,8 +382,8 @@ unsigned char dyn2_bulk_read_id_present(unsigned char *packet,unsigned char id,u
 /* bulk write instruction */
 void dyn2_init_bulk_write_packet(unsigned char *packet,unsigned char num_servos,unsigned char *servo_ids,unsigned short int *address,unsigned short int *length,TWriteData *data)
 {
-  unsigned char i,j;
-  unsigned short int total_length=0;
+  unsigned char i;
+  unsigned short int total_length=0,j;
 
   packet[DYN2_HEADER_OFF]=0xFF;
   packet[DYN2_HEADER_OFF+1]=0xFF;
@@ -432,7 +433,7 @@ unsigned char dyn2_bulk_write_id_present(unsigned char *packet,unsigned char id,
 /* status packet */
 unsigned char dyn2_convert_v1_status_packet(TDynInstruction inst,unsigned char *source, unsigned char *destination)
 {
-  unsigned char i;
+  unsigned short int i;
 
   destination[DYN2_HEADER_OFF]=0xFF;
   destination[DYN2_HEADER_OFF+1]=0xFF;
@@ -452,7 +453,7 @@ unsigned char dyn2_convert_v1_status_packet(TDynInstruction inst,unsigned char *
 
 void dyn2_init_status_packet(unsigned char *packet,unsigned char id,TDynError error,unsigned short int length,unsigned char *data)
 {
-  unsigned char i;
+  unsigned short int i;
 
   packet[DYN2_HEADER_OFF]=0xFF;
   packet[DYN2_HEADER_OFF+1]=0xFF;
@@ -475,7 +476,7 @@ inline TDynError dyn2_get_status_error(unsigned char *packet)
 /* read instruction status packet */
 unsigned short int dyn2_get_read_status_data(unsigned char *packet,unsigned char *data)
 {
-  unsigned char i,length;
+  unsigned short int i,length;
 
   length=*((unsigned short int *)&packet[DYN2_LENGTH_OFF])-4;
   for(i=0;i<length;i++)
@@ -483,8 +484,3 @@ unsigned short int dyn2_get_read_status_data(unsigned char *packet,unsigned char
 
   return length;
 }
-
-inline unsigned char dyn2_get_read_status_id(unsigned char *packet)
-{
-  return packet[DYN2_ID_OFF];
-}