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

Added an initial module for the dynamixel master (ping working).

Solved some minor errors in the dynamixel slave interface.
parent 79771f4e
No related branches found
No related tags found
No related merge requests found
......@@ -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;
......
......@@ -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
#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)
{
}
......
#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;
......
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