Skip to content
Snippets Groups Projects
Commit 9aec648c authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Untested partial changes to support the version 2 protocol.

parent dd00f1ca
No related branches found
No related tags found
No related merge requests found
......@@ -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 */
......
......@@ -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 */
......
......@@ -3,6 +3,8 @@
#define MAX_DATA_LENGTH 255
#define MAX_HEADER_LENGTH 6
// errors
#define DYN_SUCCESS 0
#define DYN_BAD_FORMAT 1
......
......@@ -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);
......
......@@ -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);
......
#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;
......
#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;
......
......@@ -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;
......
......@@ -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
{
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment