-
Sergi Hernandez authoredSergi Hernandez authored
dynamixel_master.c 11.72 KiB
#include "dynamixel_master.h"
/* private functions */
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 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;
}
/* 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->return_level=return_all;
master->set_rx_mode();
}
void dyn_master_set_rx_timeout(TDynamixelMaster *master,unsigned short int timeout_ms)
{
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)
{
unsigned char i;
*num=0;
for(i=0;i<254;i++)
{
if(dyn_master_ping(master,i)==DYN_SUCCESS)// the device exists
{
ids[*num]=i;
(*num)++;
}
}
}
unsigned char dyn_master_ping(TDynamixelMaster *master,unsigned char id)
{
unsigned char error;
// 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 *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;
}