diff --git a/dynamixel_base/include/dynamixel.h b/dynamixel_base/include/dynamixel.h
index f2f6182616cf27c824d68305421dc9a9310477a0..8b73f9c5db022f826b4715432de4ab606dd7923e 100755
--- a/dynamixel_base/include/dynamixel.h
+++ b/dynamixel_base/include/dynamixel.h
@@ -3,6 +3,13 @@
#include "dynamixel_common.h"
+#define DYN_HEADER_OFF 0
+#define DYN_ID_OFF 2
+#define DYN_LENGTH_OFF 3
+#define DYN_INST_OFF 4
+#define DYN_ERROR_OFF 4
+#define DYN_DATA_OFF 5
+
void dyn_copy_packet(unsigned char *source, unsigned char *destination);
inline unsigned char dyn_get_id(unsigned char *packet);
inline unsigned char dyn_get_length(unsigned char *packet);
@@ -20,6 +27,7 @@ typedef struct{
unsigned char checksum;
}TDynInst;
*/
+unsigned char dyn_convert_v2_inst_packet(unsigned char *source, unsigned char *destination);
/* ping instruction */
void dyn_init_ping_packet(unsigned char *packet,unsigned char id);
/* read instruction */
@@ -59,6 +67,7 @@ typedef struct{
}TDynStatus;
*/
+unsigned char dyn_convert_v2_status_packet(unsigned char *source, unsigned char *destination);
void dyn_init_status_packet(unsigned char *packet,unsigned char id,TDynError error,unsigned char length,unsigned char *data);
inline TDynError dyn_get_status_error(unsigned char *packet);
/* read instruction status packet */
diff --git a/dynamixel_base/include/dynamixel2.h b/dynamixel_base/include/dynamixel2.h
index f34c6ee81e3a0cfe7d66d7a66863612a443a0c75..f51473c7c62e342bd9f12875f184f165e9944f5b 100644
--- a/dynamixel_base/include/dynamixel2.h
+++ b/dynamixel_base/include/dynamixel2.h
@@ -3,6 +3,15 @@
#include "dynamixel_common.h"
+#define DYN2_HEADER_OFF 0
+#define DYN2_RESERVED 3
+#define DYN2_ID_OFF 4
+#define DYN2_LENGTH_OFF 5
+#define DYN2_INST_OFF 7
+#define DYN2_ERROR_OFF 8
+#define DYN2_DATA_OFF 8
+
+
void dyn2_copy_packet(unsigned char *source, unsigned char *destination);
inline unsigned char dyn2_get_id(unsigned char *packet);
inline unsigned short int dyn2_get_length(unsigned char *packet);
@@ -20,6 +29,7 @@ typedef struct{
unsigned char checksum;
}TDynInst;
*/
+unsigned char dyn2_convert_v1_inst_packet(unsigned char *source, unsigned char *destination);
/* ping instruction */
void dyn2_init_ping_packet(unsigned char *packet,unsigned char id);
/* read instruction */
@@ -65,6 +75,7 @@ typedef struct{
}TDynStatus;
*/
+unsigned char dyn2_convert_v1_status_packet(TDynInstruction inst,unsigned char *source, unsigned char *destination);
void dyn2_init_status_packet(unsigned char *packet,unsigned char id,TDynError error,unsigned short int length,unsigned char *data);
inline TDynError dyn2_get_status_error(unsigned char *packet);
/* read instruction status packet */
diff --git a/dynamixel_base/include/dynamixel_common.h b/dynamixel_base/include/dynamixel_common.h
index d8310e95182a860d7fb9ac461faa832717e9887d..ea125c550f3593b27a54b4f6469a947ac5f14446 100644
--- a/dynamixel_base/include/dynamixel_common.h
+++ b/dynamixel_base/include/dynamixel_common.h
@@ -3,6 +3,8 @@
#define MAX_DATA_LENGTH 255
+#define MAX_HEADER_LENGTH 6
+
// errors
#define DYN_SUCCESS 0
#define DYN_BAD_FORMAT 1
diff --git a/dynamixel_base/include/dynamixel_master.h b/dynamixel_base/include/dynamixel_master.h
index 87eb846ad79a3cea7040373196e75542711feda8..52887cc38689835f8916032d8c0018ad1b96d2e7 100644
--- a/dynamixel_base/include/dynamixel_master.h
+++ b/dynamixel_base/include/dynamixel_master.h
@@ -10,6 +10,7 @@
typedef struct
{
TComm *comm_dev;
+ TDynVersion version;
unsigned char tx_buffer[MAX_TX_BUFFER_LEN];
unsigned char rx_buffer[MAX_RX_BUFFER_LEN];
return_level_t return_level;
@@ -23,7 +24,7 @@ typedef struct
}TDynamixelMaster;
/* public functions */
-void dyn_master_init(TDynamixelMaster *master,TComm *dev);
+void dyn_master_init(TDynamixelMaster *master,TComm *dev,TDynVersion version);
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);
diff --git a/dynamixel_base/include/dynamixel_slave.h b/dynamixel_base/include/dynamixel_slave.h
index 34ddc8e38a795d033e12dd26f6dbbb53f08d9612..5e028f8523984638c36c727f64c0d2d57330a233 100644
--- a/dynamixel_base/include/dynamixel_slave.h
+++ b/dynamixel_base/include/dynamixel_slave.h
@@ -3,6 +3,7 @@
#include "comm.h"
#include "dynamixel.h"
+#include "dynamixel2.h"
#ifndef MAX_DYN_SLAVE_TX_BUFFER_LEN
#define MAX_DYN_SLAVE_TX_BUFFER_LEN 1024
@@ -19,11 +20,13 @@
typedef struct
{
TComm *comm_dev;
+ TDynVersion version;
unsigned char address;
unsigned char return_delay;
return_level_t return_level;
unsigned char packet_ready;
unsigned char received_bytes;
+ 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];
unsigned short int reg_address;
@@ -40,16 +43,18 @@ typedef struct
unsigned char (*on_read)(unsigned short int address,unsigned short int length,unsigned char *data);
unsigned char (*on_write)(unsigned short int address,unsigned short int length,unsigned char *data);
unsigned char (*on_reset)(void);
+ unsigned char (*on_relay)(TDynVersion version,unsigned char *inst_pkt,unsigned char *status_pkt);
}TDynamixelSlave;
/* public functions */
-void dyn_slave_init(TDynamixelSlave *slave,TComm *dev,unsigned char address);
+void dyn_slave_init(TDynamixelSlave *slave,TComm *dev,unsigned char address,TDynVersion version);
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);
inline unsigned char dyn_slave_get_return_delay(TDynamixelSlave *slave);
inline void dyn_slave_set_return_level(TDynamixelSlave *slave,return_level_t level);
inline return_level_t dyn_slave_get_return_level(TDynamixelSlave *slave);
+inline TDynVersion dyn_slave_get_version(TDynamixelSlave *slave);
void dyn_slave_loop(TDynamixelSlave *slave);
diff --git a/dynamixel_base/src/dynamixel.c b/dynamixel_base/src/dynamixel.c
index 5ab4ec6a69ea220a83cad562f7c1c7496ace1362..8a72020d537468250434b8229995895bd09335ef 100755
--- a/dynamixel_base/src/dynamixel.c
+++ b/dynamixel_base/src/dynamixel.c
@@ -1,13 +1,5 @@
#include "dynamixel.h"
-
-#define MAX_HEADER_LENGTH 6
-
-#define DYN_HEADER_OFF 0
-#define DYN_ID_OFF 2
-#define DYN_LENGTH_OFF 3
-#define DYN_INST_OFF 4
-#define DYN_ERROR_OFF 4
-#define DYN_DATA_OFF 5
+#include "dynamixel2.h"
// provate functions
void dyn_set_checksum(unsigned char *packet)
@@ -63,6 +55,26 @@ inline TDynInstruction dyn_get_instruction(unsigned char *packet)
return packet[DYN_INST_OFF];
}
+unsigned char dyn_convert_v2_inst_packet(unsigned char *source, unsigned char *destination)
+{
+ unsigned char i;
+
+ if(dyn2_get_length(source)>255)
+ return 0x00;
+ else
+ {
+ destination[DYN_HEADER_OFF]=0xFF;
+ destination[DYN_HEADER_OFF+1]=0xFF;
+ destination[DYN_ID_OFF]=source[DYN2_ID_OFF];
+ destination[DYN_LENGTH_OFF]=source[DYN2_LENGTH_OFF];
+ destination[DYN_INST_OFF]=source[DYN2_INST_OFF];
+ for(i=0;i<destination[DYN_LENGTH_OFF];i++)
+ destination[DYN_DATA_OFF+i]=source[DYN2_DATA_OFF+i];
+ dyn_set_checksum(destination);
+ return 0x01;
+ }
+}
+
/* ping instruction*/
void dyn_init_ping_packet(unsigned char *packet,unsigned char id)
{
@@ -269,6 +281,26 @@ unsigned char dyn_bulk_read_id_present(unsigned char *packet,unsigned char id,un
return 0xFF;
}
+unsigned char dyn_convert_v2_status_packet(unsigned char *source, unsigned char *destination)
+{
+ unsigned char i;
+
+ if(dyn2_get_length(source)>255)
+ return 0x00;
+ else
+ {
+ destination[DYN_HEADER_OFF]=0xFF;
+ destination[DYN_HEADER_OFF+1]=0xFF;
+ destination[DYN_ID_OFF]=source[DYN2_ID_OFF];
+ destination[DYN_LENGTH_OFF]=source[DYN2_LENGTH_OFF];
+ destination[DYN_ERROR_OFF]=source[DYN2_ERROR_OFF];
+ for(i=0;i<destination[DYN_LENGTH_OFF];i++)
+ destination[DYN_DATA_OFF+i]=source[DYN2_DATA_OFF+i];
+ dyn_set_checksum(destination);
+ return 0x01;
+ }
+}
+
void dyn_init_status_packet(unsigned char *packet,unsigned char id,TDynError error,unsigned char length,unsigned char *data)
{
unsigned char i;
diff --git a/dynamixel_base/src/dynamixel2.c b/dynamixel_base/src/dynamixel2.c
index 7731e5e933ebd545578e07627dbeafbf06bd80fe..b85db07a219a7e99d4a6e17114b40f7b73c6f865 100644
--- a/dynamixel_base/src/dynamixel2.c
+++ b/dynamixel_base/src/dynamixel2.c
@@ -1,15 +1,6 @@
+#include "dynamixel.h"
#include "dynamixel2.h"
-#define MAX_HEADER_LENGTH 6
-
-#define DYN2_HEADER_OFF 0
-#define DYN2_RESERVED 3
-#define DYN2_ID_OFF 4
-#define DYN2_LENGTH_OFF 5
-#define DYN2_INST_OFF 7
-#define DYN2_ERROR_OFF 8
-#define DYN2_DATA_OFF 8
-
const unsigned short crc_table[256] = {
0x0000, 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011,
0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, 0x0022,
@@ -45,7 +36,7 @@ const unsigned short crc_table[256] = {
0x8213, 0x0216, 0x021C, 0x8219, 0x0208, 0x820D, 0x8207, 0x0202
};
-// provate functions
+// private functions
void dyn2_set_checksum(unsigned char *packet)
{
unsigned short int crc_accum=0x0000,i,j,length;
@@ -101,6 +92,25 @@ inline TDynInstruction dyn2_get_instruction(unsigned char *packet)
return packet[DYN2_INST_OFF];
}
+unsigned char dyn2_convert_v1_inst_packet(unsigned char *source, unsigned char *destination)
+{
+ unsigned char i;
+
+ destination[DYN2_HEADER_OFF]=0xFF;
+ destination[DYN2_HEADER_OFF+1]=0xFF;
+ destination[DYN2_HEADER_OFF+2]=0xFD;
+ destination[DYN2_RESERVED]=0x00;
+ destination[DYN2_ID_OFF]=source[DYN_ID_OFF];
+ destination[DYN2_LENGTH_OFF]=source[DYN_LENGTH_OFF];
+ destination[DYN2_LENGTH_OFF+1]=0x00;
+ destination[DYN2_INST_OFF]=source[DYN_INST_OFF];
+ for(i=0;i<source[DYN_LENGTH_OFF];i++)
+ destination[DYN2_DATA_OFF+i]=source[DYN_DATA_OFF+i];
+ dyn2_set_checksum(destination);
+
+ return 0x01;
+}
+
/* ping instruction*/
void dyn2_init_ping_packet(unsigned char *packet,unsigned char id)
{
@@ -420,6 +430,26 @@ 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;
+
+ destination[DYN2_HEADER_OFF]=0xFF;
+ destination[DYN2_HEADER_OFF+1]=0xFF;
+ destination[DYN2_HEADER_OFF+2]=0xFD;
+ destination[DYN2_RESERVED]=0x00;
+ destination[DYN2_ID_OFF]=source[DYN_ID_OFF];
+ destination[DYN2_LENGTH_OFF]=source[DYN_LENGTH_OFF];
+ destination[DYN2_LENGTH_OFF+1]=0x00;
+ destination[DYN2_INST_OFF]=inst;
+ destination[DYN2_ERROR_OFF]=source[DYN_ERROR_OFF];
+ for(i=0;i<source[DYN_LENGTH_OFF];i++)
+ destination[DYN2_DATA_OFF+i]=source[DYN_DATA_OFF+i];
+ dyn2_set_checksum(destination);
+
+ return 0x01;
+}
+
void dyn2_init_status_packet(unsigned char *packet,unsigned char id,TDynError error,unsigned short int length,unsigned char *data)
{
unsigned char i;
diff --git a/dynamixel_base/src/dynamixel_master.c b/dynamixel_base/src/dynamixel_master.c
index 0d38df5832a5ca9dc93e747e31ba1d8d710a2ca9..ad3a4431364737d1c05bfc36c2d37b6369152791 100644
--- a/dynamixel_base/src/dynamixel_master.c
+++ b/dynamixel_base/src/dynamixel_master.c
@@ -108,8 +108,16 @@ unsigned char dyn_master_send(TDynamixelMaster *master)
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;
+ if(master->version==DYN_VER1)
+ {
+ comm_send_dma(master->comm_dev,master->tx_buffer,dyn_get_length(master->tx_buffer)+4);
+ return DYN_SUCCESS;
+ }
+ else
+ {
+ comm_send_dma(master->comm_dev,master->tx_buffer,dyn2_get_length(master->tx_buffer)+7);
+ return DYN_SUCCESS;
+ }
}
unsigned char dyn_master_wait_reception(TDynamixelMaster *master)
@@ -142,13 +150,14 @@ unsigned char dyn_master_wait_reception(TDynamixelMaster *master)
}
/* public functions */
-void dyn_master_init(TDynamixelMaster *master,TComm *dev)
+void dyn_master_init(TDynamixelMaster *master,TComm *dev,TDynVersion version)
{
/* 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;
+ master->version=version;
dev->data=master;
/* initialize the internal callbacks */
master->set_tx_mode=dummy_dyn_master_set_tx_mode;
diff --git a/dynamixel_base/src/dynamixel_slave.c b/dynamixel_base/src/dynamixel_slave.c
index 99a3f2f756c77f5d41782988f43561234e29bfbe..0dc791c52a28029911f0ed276dabb26472e92796 100644
--- a/dynamixel_base/src/dynamixel_slave.c
+++ b/dynamixel_base/src/dynamixel_slave.c
@@ -23,19 +23,51 @@ unsigned char dyn_slave_irq_receive_cb(void *dyn_slave,unsigned char byte)
else
dyn->received_bytes--;
break;
- case 2: if(byte!=0xFF)
+ case 2: if(byte==0xFD)// version 2 header
+ {
+ if(dyn->version==DYN_VER2)// the module is configured for version 2
+ {
+ dyn->rx_buffer[dyn->received_bytes]=byte;
+ dyn->received_bytes++;
+ }
+ else
+ dyn->received_bytes=0;// ignore packet and restart synchronization
+ }
+ else if(byte!=0xFF)
{
dyn->rx_buffer[dyn->received_bytes]=byte;
dyn->received_bytes++;
}
break;
case 3: dyn->rx_buffer[dyn->received_bytes]=byte;
+ if(dyn->version==DYN_VER1)
+ {
+ dyn->op_length=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],dyn->op_length);
+ }
+ else
+ dyn->received_bytes++;
+ break;
+ case 4: dyn->rx_buffer[dyn->received_bytes]=byte;
+ dyn->received_bytes++;
+ break;
+ case 5: dyn->rx_buffer[dyn->received_bytes]=byte;
+ dyn->received_bytes++;
+ dyn->op_length=byte;
+ break;
+ case 6: dyn->rx_buffer[dyn->received_bytes]=byte;
+ dyn->received_bytes++;
+ dyn->op_length+=(byte<<8);
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;
+ comm_receive_dma(dyn->comm_dev,&dyn->rx_buffer[7],dyn->op_length);
+ break;
default: break;
}
@@ -94,29 +126,314 @@ unsigned char dummy_dyn_slave_on_reset(void)
return 0x00;
}
-void dyn_slave_send_status_packet(TDynamixelSlave *slave,unsigned char error,unsigned char length, unsigned char *data)
+unsigned char dummy_dyn_slave_on_relay(TDynVersion version,unsigned char *inst_pkt,unsigned char *status_pkt)
+{
+ return 0x00;
+}
+
+
+void dyn_slave_send_status_packet(TDynamixelSlave *slave,unsigned char error,unsigned short int length, unsigned char *data)
{
// wait until the previous transmission has ended (if any)
while(comm_is_send_done(slave->comm_dev)==COMM_BUSY);
if(slave->return_delay>0)
if(slave->comm_dev->time!=0x00000000)
time_delay_us(slave->comm_dev->time,slave->return_delay<<1);
- // create the status packet
- dyn_init_status_packet(slave->tx_buffer,slave->address,error,length,data);
- // set the tx mode, if necessary
- slave->set_tx_mode();
- // start transmission by DMA
- comm_send_dma(slave->comm_dev,slave->tx_buffer,dyn_get_length(slave->tx_buffer)+4);
+ if(slave->version==DYN_VER1)
+ {
+ // create the status packet
+ dyn_init_status_packet(slave->tx_buffer,slave->address,error,length,data);
+ // set the tx mode, if necessary
+ slave->set_tx_mode();
+ // start transmission by DMA
+ comm_send_dma(slave->comm_dev,slave->tx_buffer,dyn_get_length(slave->tx_buffer)+4);
+ }
+ else
+ {
+ // create the status packet
+ dyn2_init_status_packet(slave->tx_buffer,slave->address,error,length,data);
+ // set the tx mode, if necessary
+ slave->set_tx_mode();
+ // start transmission by DMA
+ comm_send_dma(slave->comm_dev,slave->tx_buffer,dyn2_get_length(slave->tx_buffer)+7);
+ }
+}
+
+void dyn_v1_slave_loop(TDynamixelSlave *slave)
+{
+ static unsigned char data[MAX_DYN_SLAVE_TX_BUFFER_LEN],error,length,address,prev_id,id;
+
+ id=dyn_get_id(slave->rx_buffer);
+ 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(dyn_check_checksum(slave->rx_buffer)==0xFF)// the incomming packet is okay
+ {
+ // process the packet
+ switch(dyn_get_instruction(slave->rx_buffer))
+ {
+ case DYN_PING: slave->on_ping();
+ if(id!=DYN_BROADCAST_ID)
+ dyn_slave_send_status_packet(slave,DYN_NO_ERROR,0,data);
+ break;
+ case DYN_READ: error=slave->on_read(dyn_get_read_address(slave->rx_buffer),dyn_get_read_length(slave->rx_buffer),data);
+ if(slave->return_level!=no_return && id!=DYN_BROADCAST_ID)
+ {
+ if(error==DYN_NO_ERROR)
+ dyn_slave_send_status_packet(slave,DYN_NO_ERROR,dyn_get_read_length(slave->rx_buffer),data);
+ else
+ dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
+ }
+ break;
+ case DYN_WRITE: length=dyn_get_write_data(slave->rx_buffer,data);
+ error=slave->on_write(dyn_get_write_address(slave->rx_buffer),length,data);
+ if(slave->return_level==return_all && id!=DYN_BROADCAST_ID)
+ {
+ if(error==DYN_NO_ERROR)
+ dyn_slave_send_status_packet(slave,DYN_NO_ERROR,0,data);
+ else
+ dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
+ }
+ break;
+ case DYN_REG_WRITE: slave->reg_length=dyn_get_reg_write_data(slave->rx_buffer,slave->reg_buffer);
+ slave->reg_address=dyn_get_reg_write_address(slave->rx_buffer);
+ if(slave->return_level==return_all && id!=DYN_BROADCAST_ID)
+ dyn_slave_send_status_packet(slave,DYN_NO_ERROR,0,data);
+ break;
+ case DYN_ACTION: if(slave->reg_address!=0xFFFF)
+ {
+ error=slave->on_write(slave->reg_address,slave->reg_length,slave->reg_buffer);
+ slave->reg_address=0xFFFF;
+ }
+ else
+ if(slave->return_level==return_all && id!=DYN_BROADCAST_ID)
+ dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
+ break;
+ case DYN_RESET:
+ break;
+ case DYN_SYNC_READ: dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
+ break;
+ case DYN_SYNC_WRITE: if(dyn_sync_write_id_present(slave->rx_buffer,slave->address,&address,&length,data))// the device is addressed
+ error=slave->on_write(address,length,data);
+ break;
+ case DYN_BULK_READ: prev_id=dyn_bulk_read_id_present(slave->rx_buffer,slave->address,&address,&length);
+ if(prev_id!=0xFF)
+ {
+ if(prev_id==0x00)// first device to answer
+ {
+ error=slave->on_read(address,length,data);
+ if(error==DYN_NO_ERROR)
+ dyn_slave_send_status_packet(slave,DYN_NO_ERROR,length,data);
+ else
+ dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
+ }
+ else// wait for the previous device in the sequence to send its data
+ {
+ slave->sync_bulk_address=address;
+ slave->sync_bulk_length=length;
+ slave->sync_bulk_prev_id=prev_id;
+ slave->bulk_read_pending=0x01;
+ }
+ }
+ break;
+ case DYN_BULK_WRITE: dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
+ break;
+ default:
+ break;
+ }
+ }
+ else
+ {
+ // send a checksum error answer
+ if(dyn_get_id(slave->rx_buffer)!=DYN_BROADCAST_ID)
+ dyn_slave_send_status_packet(slave,DYN_CHECKSUM_ERROR,0,0x00);
+ }
+ }
+ else
+ {
+ if(slave->bulk_read_pending)
+ {
+ if(id==slave->sync_bulk_prev_id)
+ {
+ slave->bulk_read_pending=0x00;
+ error=slave->on_read(slave->sync_bulk_address,slave->sync_bulk_length,data);
+ if(error==DYN_NO_ERROR)
+ dyn_slave_send_status_packet(slave,DYN_NO_ERROR,slave->sync_bulk_length,data);
+ else
+ dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
+ }
+ }
+ else// the packet is addressed to another device, so relay it
+ {
+ if(slave->on_relay(slave->version,slave->rx_buffer,slave->tx_buffer))
+ {
+ // set the tx mode, if necessary
+ slave->set_tx_mode();
+ // start transmission by DMA
+ comm_send_dma(slave->comm_dev,slave->tx_buffer,dyn_get_length(slave->tx_buffer)+4);
+ }
+ }
+ }
+}
+
+void dyn_v2_slave_loop(TDynamixelSlave *slave)
+{
+ static unsigned char data[MAX_DYN_SLAVE_TX_BUFFER_LEN],error,prev_id,id;
+ static unsigned short int length,address;
+
+ id=dyn2_get_id(slave->rx_buffer);
+ 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
+ {
+ // process the packet
+ switch(dyn2_get_instruction(slave->rx_buffer))
+ {
+ case DYN_PING: slave->on_ping();
+ if(id!=DYN_BROADCAST_ID)
+ dyn_slave_send_status_packet(slave,DYN_NO_ERROR,0,data);
+ break;
+ case DYN_READ: error=slave->on_read(dyn2_get_read_address(slave->rx_buffer),dyn2_get_read_length(slave->rx_buffer),data);
+ if(slave->return_level!=no_return && id!=DYN_BROADCAST_ID)
+ {
+ if(error==DYN_NO_ERROR)
+ dyn_slave_send_status_packet(slave,DYN_NO_ERROR,dyn2_get_read_length(slave->rx_buffer),data);
+ else
+ dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
+ }
+ break;
+ case DYN_WRITE: length=dyn2_get_write_data(slave->rx_buffer,data);
+ error=slave->on_write(dyn2_get_write_address(slave->rx_buffer),length,data);
+ if(slave->return_level==return_all && id!=DYN_BROADCAST_ID)
+ {
+ if(error==DYN_NO_ERROR)
+ dyn_slave_send_status_packet(slave,DYN_NO_ERROR,0,data);
+ else
+ dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
+ }
+ break;
+ case DYN_REG_WRITE: slave->reg_length=dyn2_get_reg_write_data(slave->rx_buffer,slave->reg_buffer);
+ slave->reg_address=dyn2_get_reg_write_address(slave->rx_buffer);
+ if(slave->return_level==return_all && id!=DYN_BROADCAST_ID)
+ dyn_slave_send_status_packet(slave,DYN_NO_ERROR,0,data);
+ break;
+ case DYN_ACTION: if(slave->reg_address!=0xFFFF)
+ {
+ error=slave->on_write(slave->reg_address,slave->reg_length,slave->reg_buffer);
+ slave->reg_address=0xFFFF;
+ }
+ else
+ if(slave->return_level==return_all && id!=DYN_BROADCAST_ID)
+ dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
+ break;
+ case DYN_RESET:
+ break;
+ case DYN_SYNC_READ: prev_id=dyn2_sync_read_id_present(slave->rx_buffer,slave->address,&address,&length);
+ if(prev_id!=0xFF)
+ {
+ if(prev_id==0x00)// first device to answer
+ {
+ error=slave->on_read(address,length,data);
+ if(error==DYN_NO_ERROR)
+ dyn_slave_send_status_packet(slave,DYN_NO_ERROR,length,data);
+ else
+ dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
+ }
+ else// wait for the previous device in the sequence to send its data
+ {
+ slave->sync_bulk_address=address;
+ slave->sync_bulk_length=length;
+ slave->sync_bulk_prev_id=prev_id;
+ slave->sync_read_pending=0x01;
+ }
+ }
+ break;
+ case DYN_SYNC_WRITE: if(dyn2_sync_write_id_present(slave->rx_buffer,slave->address,&address,&length,data))// the device is addressed
+ error=slave->on_write(address,length,data);
+ break;
+ case DYN_BULK_READ: prev_id=dyn2_bulk_read_id_present(slave->rx_buffer,slave->address,&address,&length);
+ if(prev_id!=0xFF)
+ {
+ if(prev_id==0x00)// first device to answer
+ {
+ error=slave->on_read(address,length,data);
+ if(error==DYN_NO_ERROR)
+ dyn_slave_send_status_packet(slave,DYN_NO_ERROR,length,data);
+ else
+ dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
+ }
+ else// wait for the previous device in the sequence to send its data
+ {
+ slave->sync_bulk_address=address;
+ slave->sync_bulk_length=length;
+ slave->sync_bulk_prev_id=prev_id;
+ slave->bulk_read_pending=0x01;
+ }
+ }
+ break;
+ 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;
+ }
+ }
+ else
+ {
+ // send a checksum error answer
+ if(dyn_get_id(slave->rx_buffer)!=DYN_BROADCAST_ID)
+ dyn_slave_send_status_packet(slave,DYN_CHECKSUM_ERROR,0,0x00);
+ }
+ }
+ else
+ {
+ if(slave->bulk_read_pending)
+ {
+ if(id==slave->sync_bulk_prev_id)
+ {
+ slave->bulk_read_pending=0x00;
+ error=slave->on_read(slave->sync_bulk_address,slave->sync_bulk_length,data);
+ if(error==DYN_NO_ERROR)
+ dyn_slave_send_status_packet(slave,DYN_NO_ERROR,slave->sync_bulk_length,data);
+ else
+ dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
+ }
+ }
+ else if(slave->sync_read_pending)
+ {
+ if(id==slave->sync_bulk_prev_id)
+ {
+ slave->sync_read_pending=0x00;
+ error=slave->on_read(slave->sync_bulk_address,slave->sync_bulk_length,data);
+ if(error==DYN_NO_ERROR)
+ dyn_slave_send_status_packet(slave,DYN_NO_ERROR,slave->sync_bulk_length,data);
+ else
+ dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
+ }
+ }
+ else// the packet is addressed to another device, so relay it
+ {
+ if(slave->on_relay(slave->version,slave->rx_buffer,slave->tx_buffer))
+ {
+ // set the tx mode, if necessary
+ slave->set_tx_mode();
+ // start transmission by DMA
+ comm_send_dma(slave->comm_dev,slave->tx_buffer,dyn_get_length(slave->tx_buffer)+8);
+ }
+ }
+ }
}
/* public functions */
-void dyn_slave_init(TDynamixelSlave *slave,TComm *dev,unsigned char address)
+void dyn_slave_init(TDynamixelSlave *slave,TComm *dev,unsigned char address,TDynVersion version)
{
/* assign communication functions */
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;
+ slave->version=version;
dev->data=slave;
/* initialize the internal callbacks */
slave->set_tx_mode=dummy_dyn_slave_set_tx_mode;
@@ -125,6 +442,7 @@ void dyn_slave_init(TDynamixelSlave *slave,TComm *dev,unsigned char address)
slave->on_read=dummy_dyn_slave_on_read;
slave->on_write=dummy_dyn_slave_on_write;
slave->on_reset=dummy_dyn_slave_on_reset;
+ slave->on_relay=dummy_dyn_slave_on_relay;
/* initialize internal variables */
slave->address=address;
slave->return_delay=0x00;
@@ -174,115 +492,21 @@ inline return_level_t dyn_slave_get_return_level(TDynamixelSlave *slave)
return slave->return_level;
}
-void dyn_slave_loop(TDynamixelSlave *slave)
+inline TDynVersion dyn_slave_get_version(TDynamixelSlave *slave)
{
- unsigned char data[MAX_DYN_SLAVE_TX_BUFFER_LEN],error,length,address,prev_id,id;
+ return slave->version;
+}
+void dyn_slave_loop(TDynamixelSlave *slave)
+{
if(slave->packet_ready)// check if a new instruction packet has been received
{
slave->packet_ready=0x00;
// check address
- id=dyn_get_id(slave->rx_buffer);
- 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(dyn_check_checksum(slave->rx_buffer)==0xFF)// the incomming packet is okay
- {
- // process the packet
- switch(dyn_get_instruction(slave->rx_buffer))
- {
- case DYN_PING: slave->on_ping();
- if(id!=DYN_BROADCAST_ID)
- dyn_slave_send_status_packet(slave,DYN_NO_ERROR,0,data);
- break;
- case DYN_READ: error=slave->on_read(dyn_get_read_address(slave->rx_buffer),dyn_get_read_length(slave->rx_buffer),data);
- if(slave->return_level!=no_return && id!=DYN_BROADCAST_ID)
- {
- if(error==DYN_NO_ERROR)
- dyn_slave_send_status_packet(slave,DYN_NO_ERROR,dyn_get_read_length(slave->rx_buffer),data);
- else
- dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
- }
- break;
- case DYN_WRITE: length=dyn_get_write_data(slave->rx_buffer,data);
- error=slave->on_write(dyn_get_write_address(slave->rx_buffer),length,data);
- if(slave->return_level==return_all && id!=DYN_BROADCAST_ID)
- {
- if(error==DYN_NO_ERROR)
- dyn_slave_send_status_packet(slave,DYN_NO_ERROR,0,data);
- else
- dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
- }
- break;
- case DYN_REG_WRITE: slave->reg_length=dyn_get_reg_write_data(slave->rx_buffer,slave->reg_buffer);
- slave->reg_address=dyn_get_reg_write_address(slave->rx_buffer);
- if(slave->return_level==return_all && id!=DYN_BROADCAST_ID)
- dyn_slave_send_status_packet(slave,DYN_NO_ERROR,0,data);
- break;
- case DYN_ACTION: if(slave->reg_address!=0xFFFF)
- {
- error=slave->on_write(slave->reg_address,slave->reg_length,slave->reg_buffer);
- slave->reg_address=0xFFFF;
- }
- else
- if(slave->return_level==return_all && id!=DYN_BROADCAST_ID)
- dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
- break;
- case DYN_RESET:
- break;
- case DYN_SYNC_READ: dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
- break;
- case DYN_SYNC_WRITE: if(dyn_sync_write_id_present(slave->rx_buffer,slave->address,&address,&length,data))// the device is addressed
- error=slave->on_write(address,length,data);
- break;
- case DYN_BULK_READ: prev_id=dyn_bulk_read_id_present(slave->rx_buffer,slave->address,&address,&length);
- if(prev_id!=0xFF)
- {
- if(prev_id==0x00)// first device to answer
- {
- error=slave->on_read(address,length,data);
- if(error==DYN_NO_ERROR)
- dyn_slave_send_status_packet(slave,DYN_NO_ERROR,length,data);
- else
- dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
- }
- else// wait for the previous device in the sequence to send its data
- {
- slave->sync_bulk_address=address;
- slave->sync_bulk_length=length;
- slave->sync_bulk_prev_id=prev_id;
- slave->bulk_read_pending=0x01;
- }
- }
- break;
- case DYN_BULK_WRITE: dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
- break;
- default:
- break;
- }
- }
- else
- {
- // send a checksum error answer
- if(dyn_get_id(slave->rx_buffer)!=DYN_BROADCAST_ID)
- dyn_slave_send_status_packet(slave,DYN_CHECKSUM_ERROR,0,0x00);
- }
- }
+ if(slave->version==DYN_VER1)
+ dyn_v1_slave_loop(slave);
else
- {
- if(slave->bulk_read_pending)
- {
- if(id==slave->sync_bulk_prev_id)
- {
- slave->bulk_read_pending=0x00;
- error=slave->on_read(slave->sync_bulk_address,slave->sync_bulk_length,data);
- if(error==DYN_NO_ERROR)
- dyn_slave_send_status_packet(slave,DYN_NO_ERROR,slave->sync_bulk_length,data);
- else
- dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data);
- }
- }
- }
+ dyn_v2_slave_loop(slave);
}
else
{