diff --git a/dynamixel_base/include/dynamixel_slave.h b/dynamixel_base/include/dynamixel_slave.h index d281ad1135479ddf58ba2c00edc85145a50f2013..7529f8afeff70391dcc4c84a181dfd0fdcf50190 100644 --- a/dynamixel_base/include/dynamixel_slave.h +++ b/dynamixel_base/include/dynamixel_slave.h @@ -6,15 +6,15 @@ #include "dynamixel2.h" #ifndef MAX_DYN_SLAVE_TX_BUFFER_LEN - #define MAX_DYN_SLAVE_TX_BUFFER_LEN 1024 + #define MAX_DYN_SLAVE_TX_BUFFER_LEN 256 #endif #ifndef MAX_DYN_SLAVE_RX_BUFFER_LEN - #define MAX_DYN_SLAVE_RX_BUFFER_LEN 1024 + #define MAX_DYN_SLAVE_RX_BUFFER_LEN 256 #endif #ifndef MAX_DYN_SLAVE_REG_BUFFER_LEN - #define MAX_DYN_SLAVE_REG_BUFFER_LEN 1024 + #define MAX_DYN_SLAVE_REG_BUFFER_LEN 256 #endif typedef struct diff --git a/dynamixel_base/src/dynamixel2.c b/dynamixel_base/src/dynamixel2.c index 72748eff9ecf0218e23bf0b78b5e320ccc7cc578..a65f9f804f79ef9ec90c921840ef934ccb3e8c86 100644 --- a/dynamixel_base/src/dynamixel2.c +++ b/dynamixel_base/src/dynamixel2.c @@ -41,27 +41,29 @@ void dyn2_set_checksum(unsigned char *packet) { unsigned short int crc_accum=0x0000,i,j,length; - length=*((unsigned short int *)&packet[DYN2_LENGTH_OFF])+5; + length=dyn2_get_length(packet)+5; for(j=0;j<length;j++) { i=((unsigned short)(crc_accum>>8)^packet[j])&0xFF; crc_accum=(crc_accum<<8)^crc_table[i]; } - - *((unsigned short int *)&packet[length])=crc_accum; + + packet[length]=crc_accum%256; + packet[length+1]=crc_accum/256; } unsigned char dyn2_check_checksum(unsigned char *packet) { unsigned short int crc_accum=0x0000,i,j,length; - length=*((unsigned short int *)&packet[DYN2_LENGTH_OFF])+5; + length=dyn2_get_length(packet)+5; for(j=0;j<length;j++) { i=((unsigned short)(crc_accum>>8)^packet[j])&0xFF; crc_accum=(crc_accum<<8)^crc_table[i]; } - if(*((unsigned short int *)&packet[length])==crc_accum) + crc_accum-=(packet[length]+packet[length+1]*256); + if(crc_accum==0) return 0x01; else return 0x00; @@ -72,7 +74,7 @@ void dyn2_copy_packet(unsigned char *source, unsigned char *destination) { unsigned short int i,length; - length=*((unsigned short int *)&source[DYN2_LENGTH_OFF])+7; + length=dyn2_get_length(source)+7; for(i=0;i<length;i++) destination[i]=source[i]; } @@ -84,7 +86,7 @@ inline unsigned char dyn2_get_id(unsigned char *packet) inline unsigned short int dyn2_get_length(unsigned char *packet) { - return *((unsigned short int *)&packet[DYN2_LENGTH_OFF]); + return (packet[DYN2_LENGTH_OFF]+packet[DYN2_LENGTH_OFF+1]*256); } inline TDynInstruction dyn2_get_instruction(unsigned char *packet) @@ -136,19 +138,21 @@ void dyn2_init_read_packet(unsigned char *packet,unsigned char id,unsigned short packet[DYN2_LENGTH_OFF]=0x07; packet[DYN2_LENGTH_OFF+1]=0x00; packet[DYN2_INST_OFF]=DYN_READ; - *((unsigned short int *)&packet[DYN2_DATA_OFF])=address; - *((unsigned short int *)&packet[DYN2_DATA_OFF+2])=length; + packet[DYN2_DATA_OFF]=address%256; + packet[DYN2_DATA_OFF+1]=address/256; + packet[DYN2_DATA_OFF+2]=length%256; + packet[DYN2_DATA_OFF+3]=length/256; dyn2_set_checksum(packet); } inline unsigned short int dyn2_get_read_length(unsigned char *packet) { - return *((unsigned short int *)&packet[DYN2_DATA_OFF+2]); + return (packet[DYN2_DATA_OFF+2]+packet[DYN2_DATA_OFF+3]*256); } inline unsigned short int dyn2_get_read_address(unsigned char *packet) { - return *((unsigned short int *)&packet[DYN2_DATA_OFF]); + return (packet[DYN2_DATA_OFF]+packet[DYN2_DATA_OFF+1]*256); } /* write instruction */ @@ -161,9 +165,11 @@ void dyn2_init_write_packet(unsigned char *packet,unsigned char id,unsigned shor packet[DYN2_HEADER_OFF+2]=0xFD; packet[DYN2_RESERVED]=0x00; packet[DYN2_ID_OFF]=id; - *((unsigned short int *)&packet[DYN2_LENGTH_OFF])=5+length; + packet[DYN2_LENGTH_OFF]=(length+5)%256; + packet[DYN2_LENGTH_OFF+1]=(length+5)/256; packet[DYN2_INST_OFF]=DYN_WRITE; - *((unsigned short int *)&packet[DYN2_DATA_OFF])=address; + packet[DYN2_DATA_OFF]=address%256; + packet[DYN2_DATA_OFF+1]=address/256; for(i=0;i<length;i++) packet[DYN2_DATA_OFF+i+2]=data[i]; dyn2_set_checksum(packet); @@ -171,12 +177,12 @@ void dyn2_init_write_packet(unsigned char *packet,unsigned char id,unsigned shor inline unsigned short int dyn2_get_write_address(unsigned char *packet) { - return *((unsigned short int *)&packet[DYN2_DATA_OFF]); + return (packet[DYN2_DATA_OFF]+packet[DYN2_DATA_OFF+1]*256); } inline unsigned short int dyn2_get_write_length(unsigned char *packet) { - return *((unsigned short int *)&packet[DYN2_LENGTH_OFF])-5; + return (packet[DYN2_LENGTH_OFF]+packet[DYN2_LENGTH_OFF+1]*256)-5; } unsigned short int dyn2_get_write_data(unsigned char *packet,unsigned char *data) @@ -200,9 +206,11 @@ void dyn2_init_reg_write_packet(unsigned char *packet,unsigned char id,unsigned packet[DYN2_HEADER_OFF+2]=0xFD; packet[DYN2_RESERVED]=0x00; packet[DYN2_ID_OFF]=id; - *((unsigned short int *)&packet[DYN2_LENGTH_OFF])=5+length; + packet[DYN2_LENGTH_OFF]=(length+5)%256; + packet[DYN2_LENGTH_OFF+1]=(length+5)/256; packet[DYN2_INST_OFF]=DYN_REG_WRITE; - *((unsigned short int *)&packet[DYN2_DATA_OFF])=address; + packet[DYN2_DATA_OFF]=address%256; + packet[DYN2_DATA_OFF+1]=address/256; for(i=0;i<length;i++) packet[DYN2_DATA_OFF+i+2]=data[i]; dyn2_set_checksum(packet); @@ -210,12 +218,12 @@ void dyn2_init_reg_write_packet(unsigned char *packet,unsigned char id,unsigned inline unsigned short int dyn2_get_reg_write_address(unsigned char *packet) { - return *((unsigned short int *)&packet[DYN2_DATA_OFF]); + return (packet[DYN2_DATA_OFF]+packet[DYN2_DATA_OFF+1]*256); } inline unsigned short int dyn2_get_reg_write_length(unsigned char *packet) { - return *((unsigned short int *)&packet[DYN2_LENGTH_OFF])-5; + return (packet[DYN2_LENGTH_OFF]+packet[DYN2_LENGTH_OFF+1]*256)-5; } unsigned short int dyn2_get_reg_write_data(unsigned char *packet,unsigned char *data) @@ -262,17 +270,21 @@ void dyn2_init_reset_packet(unsigned char *packet,unsigned char id,unsigned char 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; - unsigned short int j; + unsigned short int j,len; packet[DYN2_HEADER_OFF]=0xFF; packet[DYN2_HEADER_OFF+1]=0xFF; packet[DYN2_HEADER_OFF+2]=0xFD; packet[DYN2_RESERVED]=0x00; packet[DYN2_ID_OFF]=0xFE; - *((unsigned short int *)&packet[DYN2_LENGTH_OFF])=7+(length+1)*num_servos; + len=7+(length+1)*num_servos; + packet[DYN2_LENGTH_OFF]=len%256; + packet[DYN2_LENGTH_OFF+1]=len/256; packet[DYN2_INST_OFF]=DYN_SYNC_WRITE; - *((unsigned short int *)&packet[DYN2_DATA_OFF])=address; - *((unsigned short int *)&packet[DYN2_DATA_OFF+2])=length; + packet[DYN2_DATA_OFF]=address%256; + packet[DYN2_DATA_OFF+1]=address/256; + packet[DYN2_DATA_OFF+2]=length%256; + packet[DYN2_DATA_OFF+3]=length/256; for(i=0;i<num_servos;i++) { packet[DYN2_DATA_OFF+4+(length+1)*i]=servo_ids[i]; @@ -287,13 +299,13 @@ unsigned char dyn2_sync_write_id_present(unsigned char *packet,unsigned char id, unsigned char num_servos,i; unsigned short int j,len; - len=*((unsigned short int *)&packet[DYN2_DATA_OFF+2]); - num_servos=(*((unsigned short int *)&packet[DYN2_LENGTH_OFF])-7)/(len+1); + len=packet[DYN2_DATA_OFF+2]+packet[DYN2_DATA_OFF+3]*256; + num_servos=((packet[DYN2_LENGTH_OFF]+packet[DYN2_LENGTH_OFF+1]*256)-7)/(len+1); for(i=0;i<num_servos;i++) if(packet[DYN2_DATA_OFF+4+(len+1)*i]==id) { - (*address)=*((unsigned short int *)&packet[DYN2_DATA_OFF]); - (*length)=*((unsigned short int *)&packet[DYN2_DATA_OFF+2]); + (*address)=packet[DYN2_DATA_OFF]+packet[DYN2_DATA_OFF+1]*256; + (*length)=packet[DYN2_DATA_OFF+2]+packet[DYN2_DATA_OFF+3]*256; for(j=0;j<(*length);j++) data[j]=packet[DYN2_DATA_OFF+4+(len+1)*i+j+1]; return 0x01; @@ -311,10 +323,13 @@ void dyn2_init_sync_read_packet(unsigned char *packet,unsigned char num_servos,u packet[DYN2_HEADER_OFF+2]=0xFD; packet[DYN2_RESERVED]=0x00; packet[DYN2_ID_OFF]=0xFE; - *((unsigned short int *)&packet[DYN2_LENGTH_OFF])=7+num_servos; + packet[DYN2_LENGTH_OFF]=(7+num_servos)%256; + packet[DYN2_LENGTH_OFF+1]=(7+num_servos)/256; packet[DYN2_INST_OFF]=DYN_SYNC_READ; - *((unsigned short int *)&packet[DYN2_DATA_OFF])=address; - *((unsigned short int *)&packet[DYN2_DATA_OFF+2])=length; + packet[DYN2_DATA_OFF]=address%256; + packet[DYN2_DATA_OFF+1]=address/256; + packet[DYN2_DATA_OFF+2]=length%256; + packet[DYN2_DATA_OFF+3]=length/256; for(i=0;i<num_servos;i++) packet[DYN2_DATA_OFF+4+i]=servo_ids[i]; dyn2_set_checksum(packet); @@ -324,12 +339,12 @@ unsigned char dyn2_sync_read_id_present(unsigned char *packet,unsigned char id,u { unsigned char num_servos,i; - num_servos=(*((unsigned short int *)&packet[DYN2_LENGTH_OFF])-7); + num_servos=(packet[DYN2_LENGTH_OFF]+packet[DYN2_LENGTH_OFF+1]*256)-7; for(i=0;i<num_servos;i++) if(packet[DYN2_DATA_OFF+4+i]==id) { - (*address)=*((unsigned short int *)&packet[DYN2_DATA_OFF]); - (*length)=*((unsigned short int *)&packet[DYN2_DATA_OFF+2]); + (*address)=packet[DYN2_DATA_OFF]+packet[DYN2_DATA_OFF+1]*256; + (*length)=packet[DYN2_DATA_OFF+2]+packet[DYN2_DATA_OFF+3]*256; if(i==0) return 0x00; else @@ -342,19 +357,24 @@ unsigned char dyn2_sync_read_id_present(unsigned char *packet,unsigned char id,u void dyn2_init_bulk_read_packet(unsigned char *packet,unsigned char num_servos,unsigned char *servo_ids,unsigned short int *address,unsigned short int *length) { unsigned char i; + unsigned short int len; packet[DYN2_HEADER_OFF]=0xFF; packet[DYN2_HEADER_OFF+1]=0xFF; packet[DYN2_HEADER_OFF+2]=0xFD; packet[DYN2_RESERVED]=0x00; packet[DYN2_ID_OFF]=0xFE; - *((unsigned short int *)&packet[DYN2_LENGTH_OFF])=3+5*num_servos; + len=3+5*num_servos; + packet[DYN2_LENGTH_OFF]=len%256; + packet[DYN2_LENGTH_OFF+1]=len/256; packet[DYN2_INST_OFF]=DYN_BULK_READ; for(i=0;i<num_servos;i++) { packet[DYN2_DATA_OFF+i*5]=servo_ids[i]; - *((unsigned short int *)&packet[DYN2_DATA_OFF+i*5+1])=address[i]; - *((unsigned short int *)&packet[DYN2_DATA_OFF+i*5+3])=length[i]; + packet[DYN2_DATA_OFF+i*5+1]=address[i]%256; + packet[DYN2_DATA_OFF+i*5+2]=address[i]/256; + packet[DYN2_DATA_OFF+i*5+3]=length[i]%256; + packet[DYN2_DATA_OFF+i*5+4]=length[i]/256; } dyn2_set_checksum(packet); } @@ -363,12 +383,12 @@ unsigned char dyn2_bulk_read_id_present(unsigned char *packet,unsigned char id,u { unsigned char num_servos,i,prev_id=0xFF; - num_servos=(*((unsigned short int *)&packet[DYN2_LENGTH_OFF])-3)/5; + num_servos=(packet[DYN2_LENGTH_OFF]+packet[DYN2_LENGTH_OFF+1]*256-3)/5; for(i=0;i<num_servos;i++) if(packet[DYN2_DATA_OFF+i*5]==id) { - (*address)=*((unsigned short int *)&packet[DYN2_DATA_OFF+i*5+1]); - (*length)=*((unsigned short int *)&packet[DYN2_DATA_OFF+i*5+3]); + (*address)=packet[DYN2_DATA_OFF+i*5+1]+packet[DYN2_DATA_OFF+i*5+2]*256; + (*length)=packet[DYN2_DATA_OFF+i*5+3]+packet[DYN2_DATA_OFF+i*5+4]*256; if(i==0) return 0x00; else @@ -392,15 +412,17 @@ void dyn2_init_bulk_write_packet(unsigned char *packet,unsigned char num_servos, packet[DYN2_ID_OFF]=0xFE; for(i=0;i<num_servos;i++) total_length+=length[i]+5; - *((unsigned short int *)&packet[DYN2_LENGTH_OFF])=3+total_length; + packet[DYN2_LENGTH_OFF]=(3+total_length)%256; + packet[DYN2_LENGTH_OFF+1]=(3+total_length)/256; packet[DYN2_INST_OFF]=DYN_BULK_WRITE; total_length=0; for(i=0;i<num_servos;i++) { packet[DYN2_DATA_OFF+total_length]=servo_ids[i]; - *((unsigned short int *)&packet[DYN2_DATA_OFF+total_length+1])=address[i]; - *((unsigned short int *)&packet[DYN2_DATA_OFF+total_length+3])=length[i]; - packet[DYN2_DATA_OFF+total_length+4]=(length[i]>>8)&0x00FF; + packet[DYN2_DATA_OFF+total_length+1]=address[i]%256; + packet[DYN2_DATA_OFF+total_length+2]=address[i]/256; + packet[DYN2_DATA_OFF+total_length+3]=length[i]%256; + packet[DYN2_DATA_OFF+total_length+4]=length[i]/256; for(j=0;j<length[i];j++) packet[DYN2_DATA_OFF+total_length+5+j]=data[i].data_addr[j]; total_length+=length[i]+5; @@ -412,19 +434,19 @@ unsigned char dyn2_bulk_write_id_present(unsigned char *packet,unsigned char id, { unsigned short int len,offset=0,i; - len=*((unsigned short int *)&packet[DYN2_LENGTH_OFF])-3; + len=packet[DYN2_LENGTH_OFF]+packet[DYN2_LENGTH_OFF+1]*256-3; while(offset<len) { if(packet[DYN2_DATA_OFF+offset]==id) { - (*address)=*((unsigned short int *)&packet[DYN2_DATA_OFF+offset+1]); - (*length)=*((unsigned short int *)&packet[DYN2_DATA_OFF+offset+3]); - for(i=0;i<*((unsigned short int *)&packet[DYN2_DATA_OFF+offset+3]);i++) + (*address)=packet[DYN2_DATA_OFF+offset+1]+packet[DYN2_DATA_OFF+offset+2]*256; + (*length)=packet[DYN2_DATA_OFF+offset+3]+packet[DYN2_DATA_OFF+offset+4]*256; + for(i=0;i<(*length);i++) data[i]=packet[DYN2_DATA_OFF+offset+5+i]; return 0x01; } else - offset+=*((unsigned short int *)&packet[DYN2_DATA_OFF+offset+3])+5; + offset+=packet[DYN2_DATA_OFF+offset+3]+packet[DYN2_DATA_OFF+offset+4]*256+5; } return 0x00; @@ -460,7 +482,8 @@ void dyn2_init_status_packet(unsigned char *packet,unsigned char id,TDynError er packet[DYN2_HEADER_OFF+2]=0xFD; packet[DYN2_RESERVED]=0x00; packet[DYN2_ID_OFF]=id; - *((unsigned short int *)&packet[DYN2_LENGTH_OFF])=4+length; + packet[DYN2_LENGTH_OFF]=(4+length)%256; + packet[DYN2_LENGTH_OFF+1]=(4+length)/256; packet[DYN2_INST_OFF]=0x55; packet[DYN2_DATA_OFF]=error; for(i=0;i<length;i++) @@ -478,7 +501,7 @@ unsigned short int dyn2_get_read_status_data(unsigned char *packet,unsigned char { unsigned short int i,length; - length=*((unsigned short int *)&packet[DYN2_LENGTH_OFF])-4; + length=packet[DYN2_LENGTH_OFF]+packet[DYN2_LENGTH_OFF+1]*256-4; for(i=0;i<length;i++) data[i]=packet[DYN2_DATA_OFF+1+i]; diff --git a/dynamixel_base/src/dynamixel_slave.c b/dynamixel_base/src/dynamixel_slave.c index 269316c0ea34837531c5fa7521797df94fc47f32..3824869689d25caee4eba823f857d872d4c3e269 100644 --- a/dynamixel_base/src/dynamixel_slave.c +++ b/dynamixel_base/src/dynamixel_slave.c @@ -375,6 +375,7 @@ void dyn_v2_slave_loop(TDynamixelSlave *slave) case DYN_BULK_WRITE: if(dyn2_bulk_write_id_present(slave->rx_buffer,slave->address,&address,&length,data)) error=slave->on_write(address,length,data); break; + default: break; }