diff --git a/dynamixel_base/Makefile b/dynamixel_base/Makefile
index d9e8c5ecdd305b5f1f82c4bf880eaf85b18ea66a..ebecf23c994ac721aba9e658010fb9b6b9d0e23a 100755
--- a/dynamixel_base/Makefile
+++ b/dynamixel_base/Makefile
@@ -3,6 +3,7 @@
COMPILE_OPTS = -mlittle-endian -mthumb -mthumb-interwork
COMPILE_OPTS += -Wall -O2 -fno-common
+#COMPILE_OPTS += -Wall -g -fno-common
COMPILE_OPTS += -ffreestanding -nostdlib
COMPILE_OPTS_M4_FPU = -mfloat-abi=hard -mfpu=fpv4-sp-d16 -mcpu=cortex-m4
diff --git a/dynamixel_base/include/dynamixel_master.h b/dynamixel_base/include/dynamixel_master.h
index 5bee28d6ed0fe82d2a70620b6ad169b9f08d8258..44bd9237401c30dc4bae3a597ad7806fe998ad38 100644
--- a/dynamixel_base/include/dynamixel_master.h
+++ b/dynamixel_base/include/dynamixel_master.h
@@ -50,5 +50,6 @@ unsigned char dyn_master_sync_write(TDynamixelMaster *master,unsigned char num,u
unsigned char dyn_master_sync_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data);
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 dyn_master_bulk_write(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data);
+unsigned char dyn_master_relay(TDynamixelMaster *master,unsigned char *inst_pkt,unsigned char *status_pkt);
#endif
diff --git a/dynamixel_base/include/dynamixel_slave.h b/dynamixel_base/include/dynamixel_slave.h
index 5e028f8523984638c36c727f64c0d2d57330a233..d281ad1135479ddf58ba2c00edc85145a50f2013 100644
--- a/dynamixel_base/include/dynamixel_slave.h
+++ b/dynamixel_base/include/dynamixel_slave.h
@@ -26,6 +26,7 @@ typedef struct
return_level_t return_level;
unsigned char packet_ready;
unsigned char received_bytes;
+ unsigned short int rx_timeout_ms;
unsigned short int op_length;
unsigned char tx_buffer[MAX_DYN_SLAVE_TX_BUFFER_LEN];
unsigned char rx_buffer[MAX_DYN_SLAVE_RX_BUFFER_LEN];
@@ -48,6 +49,7 @@ typedef struct
/* public functions */
void dyn_slave_init(TDynamixelSlave *slave,TComm *dev,unsigned char address,TDynVersion version);
+void dyn_slave_set_rx_timeout(TDynamixelSlave *slave,unsigned short int timeout_ms);
inline void dyn_slave_set_address(TDynamixelSlave *slave,unsigned char address);
inline unsigned char dyn_slave_get_address(TDynamixelSlave *slave);
inline void dyn_slave_set_return_delay(TDynamixelSlave *slave,unsigned char delay);
diff --git a/dynamixel_base/src/dynamixel_master.c b/dynamixel_base/src/dynamixel_master.c
index 02251010c1890f5d994542a0106ef66c89f76880..292a25f12f597d26c9f19780e82bd536c7ed64e7 100644
--- a/dynamixel_base/src/dynamixel_master.c
+++ b/dynamixel_base/src/dynamixel_master.c
@@ -105,6 +105,7 @@ unsigned char dyn_master_dma_receive_cb(void *dyn_master)
{
/* enable RX interrupts */
comm_receive_irq(dyn->comm_dev,0);
+ dyn->packet_ready=0x01;
}
return 0x00;
@@ -326,8 +327,16 @@ unsigned char dyn_master_read_table(TDynamixelMaster *master,unsigned char id,un
{
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;
+ if(master->version==DYN_VER1)
+ {
+ if(dyn_get_read_status_data(master->rx_buffer,data)!=length)// not enough data
+ error=DYN_INST_ERROR;
+ }
+ else
+ {
+ if(dyn2_get_read_status_data(master->rx_buffer,data)!=length)// not enough data
+ error=DYN_INST_ERROR;
+ }
}
}
@@ -484,7 +493,7 @@ unsigned char dyn_master_sync_read(TDynamixelMaster *master,unsigned char num,un
if(master->version==DYN_VER2)
{
dyn2_init_sync_read_packet(master->tx_buffer,num,ids,address,length);
- master->rx_num_packets=0x01;
+ master->rx_num_packets=num;
if(master->return_level==no_return)
master->rx_no_answer=0x01;
else
@@ -536,7 +545,7 @@ unsigned char dyn_master_bulk_read(TDynamixelMaster *master,unsigned char num,un
}
else
dyn2_init_bulk_read_packet(master->tx_buffer,num,ids,address,length);
- master->rx_num_packets=0x01;
+ master->rx_num_packets=num;
if(master->return_level==no_return)
master->rx_no_answer=0x01;
else
@@ -608,3 +617,30 @@ unsigned char dyn_master_bulk_write(TDynamixelMaster *master,unsigned char num,u
return error;
}
+unsigned char dyn_master_relay(TDynamixelMaster *master,unsigned char *inst_pkt,unsigned char *status_pkt)
+{
+ unsigned char error;
+
+ dyn2_copy_packet(inst_pkt,master->tx_buffer);
+ master->rx_num_packets=0x01;
+ 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
+ error=dyn_master_wait_reception(master);
+ dyn2_copy_packet(master->rx_buffer,status_pkt);
+
+ return error;
+}
diff --git a/dynamixel_base/src/dynamixel_slave.c b/dynamixel_base/src/dynamixel_slave.c
index 69fb61323b4b43c8da4b0f75f8d502a0d787cfd8..5c3026a716316f970ec3549e8bbaf1b14f822fb6 100644
--- a/dynamixel_base/src/dynamixel_slave.c
+++ b/dynamixel_base/src/dynamixel_slave.c
@@ -6,7 +6,7 @@ unsigned char dyn_slave_irq_receive_cb(void *dyn_slave,unsigned char byte)
TDynamixelSlave *dyn=(TDynamixelSlave *)dyn_slave;
if(dyn->comm_dev->time!=0x00000000)
- time_set_timeout(dyn->comm_dev->time,10000);
+ time_set_timeout(dyn->comm_dev->time,dyn->rx_timeout_ms*1000);
switch(dyn->received_bytes)
{
case 0: if(byte==0xFF)
@@ -285,7 +285,7 @@ void dyn_v2_slave_loop(TDynamixelSlave *slave)
if(id==slave->address || id==DYN_BROADCAST_ID)// the packet is addressed to this device or it is a broadcast
{
// check the packet checksum
- if(dyn2_check_checksum(slave->rx_buffer)==0xFF)// the incomming packet is okay
+ if(dyn2_check_checksum(slave->rx_buffer)==0x01)// the incomming packet is okay
{
// process the packet
switch(dyn2_get_instruction(slave->rx_buffer))
@@ -448,6 +448,7 @@ void dyn_slave_init(TDynamixelSlave *slave,TComm *dev,unsigned char address,TDyn
slave->return_delay=0x00;
slave->return_level=return_all;
slave->packet_ready=0x00;
+ slave->rx_timeout_ms=50;
slave->received_bytes=0x00;
slave->reg_address=0xFFFF;
slave->reg_length=0x0000;
@@ -462,6 +463,11 @@ void dyn_slave_init(TDynamixelSlave *slave,TComm *dev,unsigned char address,TDyn
comm_receive_irq(slave->comm_dev,0);
}
+void dyn_slave_set_rx_timeout(TDynamixelSlave *slave,unsigned short int timeout_ms)
+{
+ slave->rx_timeout_ms=timeout_ms;
+}
+
inline void dyn_slave_set_address(TDynamixelSlave *slave,unsigned char address)
{
slave->address=address;