diff --git a/dynamixel_base/include/dynamixel_common.h b/dynamixel_base/include/dynamixel_common.h
index 4a30685c297db6b5017d2a95a3ddd9d24c7f6bb3..8ad6ab0f8e7fc90169f9e5e22b3a43d66bc3fde4 100644
--- a/dynamixel_base/include/dynamixel_common.h
+++ b/dynamixel_base/include/dynamixel_common.h
@@ -8,6 +8,7 @@
#define DYN_BAD_FORMAT 1
#define DYN_NO_DEVICE 2
#define DYN_TIMEOUT 3
+#define DYN_COMM_ERROR 4
typedef struct{
unsigned char *data_addr;
diff --git a/dynamixel_base/include/dynamixel_master.h b/dynamixel_base/include/dynamixel_master.h
index f51ce3f18e2ce2543e3d8b4ede1ff3260964429c..c375c47962624d4b41d4e4a7c2bc27c6c1e92e0b 100644
--- a/dynamixel_base/include/dynamixel_master.h
+++ b/dynamixel_base/include/dynamixel_master.h
@@ -2,7 +2,7 @@
#define _DYNAMIXEL_MASTER_H
#include "comm.h"
-#include "dynamixel_common.h"
+#include "dynamixel.h"
#define MAX_TX_BUFFER_LEN 1024
#define MAX_RX_BUFFER_LEN 1024
@@ -10,26 +10,31 @@
typedef struct
{
TComm *comm_dev;
- TDynVersion version;
unsigned char tx_buffer[MAX_TX_BUFFER_LEN];
unsigned char rx_buffer[MAX_RX_BUFFER_LEN];
+ unsigned char packet_ready;
+ unsigned char received_bytes;
void (*set_tx_mode)(void);
void (*set_rx_mode)(void);
+ unsigned short int rx_timeout_ms;
+ unsigned char rx_no_answer;
+ unsigned char rx_num_packets;
}TDynamixelMaster;
/* public functions */
-void dyn_master_init(TDynamixelMaster *dyn_master,TComm *dev,TDynVersion ver);
-void dyn_master_scan(TDynamixelMaster *dyn_master,unsigned char *num,unsigned char *ids);
-unsigned char dyn_master_ping(TDynamixelMaster *dyn_master,unsigned char id);
-unsigned char dyn_master_read_byte(TDynamixelMaster *dyn_master,unsigned char id,unsigned short int address,unsigned char *data);
-unsigned char dyn_master_read_word(TDynamixelMaster *dyn_master,unsigned char id,unsigned short int address,unsigned short int *data);
-unsigned char dyn_master_read_table(TDynamixelMaster *dyn_master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data);
-unsigned char dyn_master_write_byte(TDynamixelMaster *dyn_master,unsigned char id, unsigned short int address, unsigned char data);
-unsigned char dyn_master_write_word(TDynamixelMaster *dyn_master,unsigned char id, unsigned short int address, unsigned short int data);
-unsigned char dyn_master_write_table(TDynamixelMaster *dyn_master,unsigned char id, unsigned short int address, unsigned short int length, unsigned char *data);
-unsigned char dyn_master_reg_write(TDynamixelMaster *dyn_master,unsigned char id, unsigned short int address, unsigned short int length, unsigned char *data);
-unsigned char dyn_master_action(TDynamixelMaster *dyn_master);
-unsigned char dyn_master_sync_write(TDynamixelMaster *dyn_master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data);
-unsigned char dyn_master_bulk_read(TDynamixelMaster *dyn_master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data);
+void dyn_master_init(TDynamixelMaster *master,TComm *dev);
+void dyn_master_set_rx_timeout(TDynamixelMaster *master,unsigned short int timeout_ms);
+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);
+unsigned char dyn_master_read_word(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int *data);
+unsigned char dyn_master_read_table(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data);
+unsigned char dyn_master_write_byte(TDynamixelMaster *master,unsigned char id, unsigned short int address, unsigned char data);
+unsigned char dyn_master_write_word(TDynamixelMaster *master,unsigned char id, unsigned short int address, unsigned short int data);
+unsigned char dyn_master_write_table(TDynamixelMaster *master,unsigned char id, unsigned short int address, unsigned short int length, unsigned char *data);
+unsigned char dyn_master_reg_write(TDynamixelMaster *master,unsigned char id, unsigned short int address, unsigned short int length, unsigned char *data);
+unsigned char dyn_master_action(TDynamixelMaster *master);
+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 dyn_master_bulk_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data);
#endif
diff --git a/dynamixel_base/src/dynamixel_master.c b/dynamixel_base/src/dynamixel_master.c
index f9aceb22b5144bd58f9faa68e496156cddf57d14..a3d32d45741d514171adb054201ac47343da3909 100644
--- a/dynamixel_base/src/dynamixel_master.c
+++ b/dynamixel_base/src/dynamixel_master.c
@@ -1,93 +1,253 @@
#include "dynamixel_master.h"
/* private functions */
-unsigned char irq_send_cb(void *dyn_master)
+unsigned char dyn_master_irq_receive_cb(void *dyn_master,unsigned char byte)
{
+ TDynamixelMaster *dyn=(TDynamixelMaster *)dyn_master;
+ if(dyn->comm_dev->time!=0x00000000)
+ time_set_timeout(dyn->comm_dev->time,dyn->rx_timeout_ms*1000);
+ switch(dyn->received_bytes)
+ {
+ case 0: if(byte==0xFF)
+ {
+ dyn->rx_buffer[dyn->received_bytes]=byte;
+ dyn->received_bytes++;
+ }
+ break;
+ case 1: if(byte==0xFF)
+ {
+ dyn->rx_buffer[dyn->received_bytes]=byte;
+ dyn->received_bytes++;
+ }
+ else
+ dyn->received_bytes--;
+ break;
+ case 2: if(byte!=0xFF)
+ {
+ dyn->rx_buffer[dyn->received_bytes]=byte;
+ dyn->received_bytes++;
+ }
+ break;
+ case 3: dyn->rx_buffer[dyn->received_bytes]=byte;
+ dyn->received_bytes=0;
+ /* finish reception by IRQ */
+ comm_cancel_irq_receive(dyn->comm_dev);
+ /* enable dma RX */
+ comm_receive_dma(dyn->comm_dev,&dyn->rx_buffer[4],byte);
+ break;
+ default: break;
+ }
+
+ return 0x00;
+}
+
+unsigned char dyn_master_dma_send_cb(void *dyn_master)
+{
+ TDynamixelMaster *dyn=(TDynamixelMaster *)dyn_master;
+
+ dyn->set_rx_mode();
+ if(dyn->rx_no_answer)
+ dyn->rx_no_answer=0x00;
+ else
+ {
+ /* enable RX interrupts */
+ comm_receive_irq(dyn->comm_dev,0);
+ }
+
+ return 0x00;
+}
+
+unsigned char dyn_master_dma_receive_cb(void *dyn_master)
+{
+ TDynamixelMaster *dyn=(TDynamixelMaster *)dyn_master;
+
+ dyn->rx_num_packets--;
+ if(dyn->rx_num_packets==0x00)
+ {
+ if(dyn->comm_dev->time!=0x00000000)
+ time_cancel_timeout(dyn->comm_dev->time);
+ dyn->packet_ready=0x01;
+ }
+ else
+ {
+ /* enable RX interrupts */
+ comm_receive_irq(dyn->comm_dev,0);
+ }
+
+ return 0x00;
+}
+
+void dummy_dyn_master_set_tx_mode(void)
+{
+
+}
+
+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);
+
+ if(error==COMM_SUCCESS)
+ return DYN_SUCCESS;
+ else
+ return DYN_COMM_ERROR;
+}
+
+unsigned char dyn_master_send(TDynamixelMaster *master)
+{
+ unsigned char error;
+
+ // wait until any previous transmission ends
+ if((error=dyn_master_wait_transmission(master))!=DYN_SUCCESS)
+ return error;
+ // set the DMA transfer
+ comm_send_dma(master->comm_dev,master->tx_buffer,dyn_get_length(master->tx_buffer)+4);
+ return DYN_SUCCESS;
+}
+
+unsigned char dyn_master_wait_reception(TDynamixelMaster *master)
+{
+ if(master->comm_dev->time!=0x00000000)
+ {
+ // start the new timeout
+ time_set_timeout(master->comm_dev->time,master->rx_timeout_ms*1000);
+ }
+ // wait for the status packet
+ while(!master->packet_ready)
+ {
+ if(master->comm_dev->time!=0x00000000)
+ {
+ if(time_is_timeout(master->comm_dev->time))
+ {
+ comm_cancel_irq_receive(master->comm_dev);
+ comm_cancel_dma_receive(master->comm_dev);
+ master->received_bytes=0x00;
+ return DYN_TIMEOUT;
+ }
+ }
+ }
+ master->packet_ready=0x00;
+ // check the input packet checksum
+ if(dyn_check_checksum(master->rx_buffer)==0xFF)
+ return dyn_get_status_error(master->rx_buffer);
+ else
+ return DYN_CHECKSUM_ERROR;
}
-unsigned char irq_receive_cb(void *dyn_master,unsigned char byte)
-{
-
-}
-
-unsigned char irq_error_cb(void *dyn_master,unsigned char error)
-{
-
-}
-
-unsigned char dma_send_cb(void *dyn_master)
+/* public functions */
+void dyn_master_init(TDynamixelMaster *master,TComm *dev)
{
+ /* assign communication functions */
+ dev->irq_receive_cb=dyn_master_irq_receive_cb;
+ dev->dma_send_cb=dyn_master_dma_send_cb;
+ dev->dma_receive_cb=dyn_master_dma_receive_cb;
+ master->comm_dev=dev;
+ dev->data=master;
+ /* initialize the internal callbacks */
+ master->set_tx_mode=dummy_dyn_master_set_tx_mode;
+ master->set_rx_mode=dummy_dyn_master_set_rx_mode;
+ /* initialize internal variables */
+ master->packet_ready=0x00;
+ master->received_bytes=0x00;
+ master->rx_timeout_ms=50;
+ master->rx_no_answer=0x00;
+ master->rx_num_packets=0x00;
+ master->set_rx_mode();
}
-unsigned char dma_receive_cb(void *dyn_master)
+void dyn_master_set_rx_timeout(TDynamixelMaster *master,unsigned short int timeout_ms)
{
-
+ master->rx_timeout_ms=timeout_ms;
}
-/* public functions */
-void dyn_master_init(TDynamixelMaster *dyn_master,TComm *dev,TDynVersion ver)
+void dyn_master_scan(TDynamixelMaster *master,unsigned char *num,unsigned char *ids)
{
}
-void dyn_master_scan(TDynamixelMaster *dyn_master,unsigned char *num,unsigned char *ids)
+unsigned char dyn_master_ping(TDynamixelMaster *master,unsigned char id)
{
+ unsigned char error;
-}
-
-unsigned char dyn_master_ping(TDynamixelMaster *dyn_master,unsigned char id)
-{
+ // generate the ping packet for the desired device
+ dyn_init_ping_packet(master->tx_buffer,id);
+ 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);
+ return error;
}
-unsigned char dyn_master_read_byte(TDynamixelMaster *dyn_master,unsigned char id,unsigned short int address,unsigned char *data)
+unsigned char dyn_master_read_byte(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned char *data)
{
}
-unsigned char dyn_master_read_word(TDynamixelMaster *dyn_master,unsigned char id,unsigned short int address,unsigned short int *data)
+unsigned char dyn_master_read_word(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int *data)
{
}
-unsigned char dyn_master_read_table(TDynamixelMaster *dyn_master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data)
+unsigned char dyn_master_read_table(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data)
{
}
-unsigned char dyn_master_write_byte(TDynamixelMaster *dyn_master,unsigned char id, unsigned short int address, unsigned char data)
+unsigned char dyn_master_write_byte(TDynamixelMaster *master,unsigned char id, unsigned short int address, unsigned char data)
{
}
-unsigned char dyn_master_write_word(TDynamixelMaster *dyn_master,unsigned char id, unsigned short int address, unsigned short int data)
+unsigned char dyn_master_write_word(TDynamixelMaster *master,unsigned char id, unsigned short int address, unsigned short int data)
{
}
-unsigned char dyn_master_write_table(TDynamixelMaster *dyn_master,unsigned char id, unsigned short int address, unsigned short int length, unsigned char *data)
+unsigned char dyn_master_write_table(TDynamixelMaster *master,unsigned char id, unsigned short int address, unsigned short int length, unsigned char *data)
{
}
-unsigned char dyn_master_reg_write(TDynamixelMaster *dyn_master,unsigned char id, unsigned short int address, unsigned short int length, unsigned char *data)
+unsigned char dyn_master_reg_write(TDynamixelMaster *master,unsigned char id, unsigned short int address, unsigned short int length, unsigned char *data)
{
}
-unsigned char dyn_master_action(TDynamixelMaster *dyn_master)
+unsigned char dyn_master_action(TDynamixelMaster *master)
{
}
-unsigned char dyn_master_sync_write(TDynamixelMaster *dyn_master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data)
+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 dyn_master_bulk_read(TDynamixelMaster *dyn_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)
{
}
diff --git a/dynamixel_base/src/dynamixel_slave.c b/dynamixel_base/src/dynamixel_slave.c
index e6bce20161ab936ae8f0510f03dc140dd1f0447a..99a3f2f756c77f5d41782988f43561234e29bfbe 100644
--- a/dynamixel_base/src/dynamixel_slave.c
+++ b/dynamixel_base/src/dynamixel_slave.c
@@ -1,7 +1,7 @@
#include "dynamixel_slave.h"
/* private functions */
-unsigned char irq_receive_cb(void *dyn_slave,unsigned char byte)
+unsigned char dyn_slave_irq_receive_cb(void *dyn_slave,unsigned char byte)
{
TDynamixelSlave *dyn=(TDynamixelSlave *)dyn_slave;
@@ -42,7 +42,7 @@ unsigned char irq_receive_cb(void *dyn_slave,unsigned char byte)
return 0x00;
}
-unsigned char dma_send_cb(void *dyn_slave)
+unsigned char dyn_slave_dma_send_cb(void *dyn_slave)
{
TDynamixelSlave *dyn=(TDynamixelSlave *)dyn_slave;
@@ -52,7 +52,7 @@ unsigned char dma_send_cb(void *dyn_slave)
return 0x00;
}
-unsigned char dma_receive_cb(void *dyn_slave)
+unsigned char dyn_slave_dma_receive_cb(void *dyn_slave)
{
TDynamixelSlave *dyn=(TDynamixelSlave *)dyn_slave;
@@ -64,32 +64,32 @@ unsigned char dma_receive_cb(void *dyn_slave)
return 0x00;
}
-void dummy_set_tx_mode(void)
+void dummy_dyn_slave_set_tx_mode(void)
{
}
-void dummy_set_rx_mode(void)
+void dummy_dyn_slave_set_rx_mode(void)
{
}
-void dummy_on_ping(void)
+void dummy_dyn_slave_on_ping(void)
{
}
-unsigned char dummy_on_read(unsigned short int address,unsigned short int length,unsigned char *data)
+unsigned char dummy_dyn_slave_on_read(unsigned short int address,unsigned short int length,unsigned char *data)
{
return 0x00;
}
-unsigned char dummy_on_write(unsigned short int address,unsigned short int length,unsigned char *data)
+unsigned char dummy_dyn_slave_on_write(unsigned short int address,unsigned short int length,unsigned char *data)
{
return 0x00;
}
-unsigned char on_reset(void)
+unsigned char dummy_dyn_slave_on_reset(void)
{
return 0x00;
}
@@ -113,17 +113,18 @@ void dyn_slave_send_status_packet(TDynamixelSlave *slave,unsigned char error,uns
void dyn_slave_init(TDynamixelSlave *slave,TComm *dev,unsigned char address)
{
/* assign communication functions */
- dev->irq_receive_cb=irq_receive_cb;
- dev->dma_send_cb=dma_send_cb;
- dev->dma_receive_cb=dma_receive_cb;
+ dev->irq_receive_cb=dyn_slave_irq_receive_cb;
+ dev->dma_send_cb=dyn_slave_dma_send_cb;
+ dev->dma_receive_cb=dyn_slave_dma_receive_cb;
slave->comm_dev=dev;
dev->data=slave;
/* initialize the internal callbacks */
- slave->set_tx_mode=dummy_set_tx_mode;
- slave->set_rx_mode=dummy_set_rx_mode;
- slave->on_ping=dummy_on_ping;
- slave->on_read=dummy_on_read;
- slave->on_write=dummy_on_write;
+ slave->set_tx_mode=dummy_dyn_slave_set_tx_mode;
+ slave->set_rx_mode=dummy_dyn_slave_set_rx_mode;
+ slave->on_ping=dummy_dyn_slave_on_ping;
+ slave->on_read=dummy_dyn_slave_on_read;
+ slave->on_write=dummy_dyn_slave_on_write;
+ slave->on_reset=dummy_dyn_slave_on_reset;
/* initialize internal variables */
slave->address=address;
slave->return_delay=0x00;