diff --git a/dynamixel_base/src/dynamixel2.c b/dynamixel_base/src/dynamixel2.c index 85c6cec685ceffbf02dc8bb061bb0d21d08a4065..1a7612e8647149b4cd4bd1577633507853836aa1 100644 --- a/dynamixel_base/src/dynamixel2.c +++ b/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]; -}