diff --git a/dynamixel_base/include/dynamixel_common.h b/dynamixel_base/include/dynamixel_common.h index 8ad6ab0f8e7fc90169f9e5e22b3a43d66bc3fde4..d8310e95182a860d7fb9ac461faa832717e9887d 100644 --- a/dynamixel_base/include/dynamixel_common.h +++ b/dynamixel_base/include/dynamixel_common.h @@ -14,6 +14,10 @@ typedef struct{ unsigned char *data_addr; }TWriteData; +typedef enum{no_return=0x00, + return_only_read=0x01, + return_all=0x02} return_level_t; + // possible packet types typedef enum{DYN_PING=0x01, DYN_READ=0x02, diff --git a/dynamixel_base/include/dynamixel_master.h b/dynamixel_base/include/dynamixel_master.h index c375c47962624d4b41d4e4a7c2bc27c6c1e92e0b..87eb846ad79a3cea7040373196e75542711feda8 100644 --- a/dynamixel_base/include/dynamixel_master.h +++ b/dynamixel_base/include/dynamixel_master.h @@ -12,6 +12,7 @@ typedef struct TComm *comm_dev; unsigned char tx_buffer[MAX_TX_BUFFER_LEN]; unsigned char rx_buffer[MAX_RX_BUFFER_LEN]; + return_level_t return_level; unsigned char packet_ready; unsigned char received_bytes; void (*set_tx_mode)(void); @@ -24,6 +25,8 @@ typedef struct /* public functions */ void dyn_master_init(TDynamixelMaster *master,TComm *dev); void dyn_master_set_rx_timeout(TDynamixelMaster *master,unsigned short int timeout_ms); +inline void dyn_master_set_return_level(TDynamixelMaster *master,return_level_t level); +inline return_level_t dyn_master_get_return_level(TDynamixelMaster *master); void dyn_master_scan(TDynamixelMaster *master,unsigned char *num,unsigned char *ids); unsigned char dyn_master_ping(TDynamixelMaster *master,unsigned char id); unsigned char dyn_master_read_byte(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned char *data); diff --git a/dynamixel_base/include/dynamixel_slave.h b/dynamixel_base/include/dynamixel_slave.h index 69572202c5cc4638f93b58e988d54f679c9630ae..34ddc8e38a795d033e12dd26f6dbbb53f08d9612 100644 --- a/dynamixel_base/include/dynamixel_slave.h +++ b/dynamixel_base/include/dynamixel_slave.h @@ -16,10 +16,6 @@ #define MAX_DYN_SLAVE_REG_BUFFER_LEN 1024 #endif -typedef enum{no_return=0x00, - return_only_read=0x01, - return_all=0x02} return_level_t; - typedef struct { TComm *comm_dev; diff --git a/dynamixel_base/src/dynamixel_master.c b/dynamixel_base/src/dynamixel_master.c index a3d32d45741d514171adb054201ac47343da3909..7295fe5bdbe28b8e695e48233a4414aaf14c6643 100644 --- a/dynamixel_base/src/dynamixel_master.c +++ b/dynamixel_base/src/dynamixel_master.c @@ -90,7 +90,6 @@ void dummy_dyn_master_set_rx_mode(void) unsigned char dyn_master_wait_transmission(TDynamixelMaster *master) { - unsigned short int timeout_left=master->rx_timeout_ms*1000; unsigned char error; while((error=comm_is_send_done(master->comm_dev))==COMM_BUSY); @@ -160,6 +159,7 @@ void dyn_master_init(TDynamixelMaster *master,TComm *dev) master->rx_timeout_ms=50; master->rx_no_answer=0x00; master->rx_num_packets=0x00; + master->return_level=return_all; master->set_rx_mode(); } @@ -169,6 +169,16 @@ void dyn_master_set_rx_timeout(TDynamixelMaster *master,unsigned short int timeo master->rx_timeout_ms=timeout_ms; } +inline void dyn_master_set_return_level(TDynamixelMaster *master,return_level_t level) +{ + master->return_level=level; +} + +inline return_level_t dyn_master_get_return_level(TDynamixelMaster *master) +{ + return master->return_level; +} + void dyn_master_scan(TDynamixelMaster *master,unsigned char *num,unsigned char *ids) { @@ -204,51 +214,227 @@ unsigned char dyn_master_ping(TDynamixelMaster *master,unsigned char id) unsigned char dyn_master_read_byte(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned char *data) { - + return dyn_master_read_table(master,id,address,1,data); } unsigned char dyn_master_read_word(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int *data) { + unsigned char error; + unsigned char data_int[2]; + // generate the ping packet for the desired device + error=dyn_master_read_table(master,id,address,2,data_int); + (*data)=data_int[0]+data_int[1]*256; + + return error; } unsigned char dyn_master_read_table(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data) { + unsigned char error; + + // generate the read packet for the desired device + dyn_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; + 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 && id!=DYN_BROADCAST_ID) + { + if((error=dyn_master_wait_reception(master))==DYN_SUCCESS) + { + if(dyn_get_read_status_data(master->rx_buffer,data)!=length)// not enough data + error=DYN_INST_ERROR; + } + } + return error; } unsigned char dyn_master_write_byte(TDynamixelMaster *master,unsigned char id, unsigned short int address, unsigned char data) { - + return dyn_master_write_table(master,id,address,1,&data); } unsigned char dyn_master_write_word(TDynamixelMaster *master,unsigned char id, unsigned short int address, unsigned short int data) { + unsigned char data_int[2]; + data_int[0]=data%256; + data_int[1]=data/256; + return dyn_master_write_table(master,id,address,2,data_int); } unsigned char dyn_master_write_table(TDynamixelMaster *master,unsigned char id, unsigned short int address, unsigned short int length, unsigned char *data) { + unsigned char error; + + // generate the write packet for the desired device + dyn_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; + else + 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; + } + // wait for the replay within the given timeout + if(master->return_level==return_all && id!=DYN_BROADCAST_ID) + error=dyn_master_wait_reception(master); + return error; } unsigned char dyn_master_reg_write(TDynamixelMaster *master,unsigned char id, unsigned short int address, unsigned short int length, unsigned char *data) { + unsigned char error; + + // generate the registered write packet for the desired device + dyn_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; + else + 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; + } + // wait for the replay within the given timeout + if(master->return_level==return_all && id!=DYN_BROADCAST_ID) + error=dyn_master_wait_reception(master); + return error; } 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); + 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; } 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 error; + + // generate the write packet for the desired device + dyn_init_sync_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; } 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; + + // generate the read packet for the desired device + dyn_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; + 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(dyn_get_read_status_data(master->rx_buffer,data[i].data_addr)!=length[i])// not enough data + error=DYN_INST_ERROR; + } + } + } + return error; }