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

Implemented the dynamixel master protocol functions.

parent 5fb44d13
No related branches found
No related tags found
No related merge requests found
...@@ -14,6 +14,10 @@ typedef struct{ ...@@ -14,6 +14,10 @@ typedef struct{
unsigned char *data_addr; unsigned char *data_addr;
}TWriteData; }TWriteData;
typedef enum{no_return=0x00,
return_only_read=0x01,
return_all=0x02} return_level_t;
// possible packet types // possible packet types
typedef enum{DYN_PING=0x01, typedef enum{DYN_PING=0x01,
DYN_READ=0x02, DYN_READ=0x02,
......
...@@ -12,6 +12,7 @@ typedef struct ...@@ -12,6 +12,7 @@ typedef struct
TComm *comm_dev; TComm *comm_dev;
unsigned char tx_buffer[MAX_TX_BUFFER_LEN]; unsigned char tx_buffer[MAX_TX_BUFFER_LEN];
unsigned char rx_buffer[MAX_RX_BUFFER_LEN]; unsigned char rx_buffer[MAX_RX_BUFFER_LEN];
return_level_t return_level;
unsigned char packet_ready; unsigned char packet_ready;
unsigned char received_bytes; unsigned char received_bytes;
void (*set_tx_mode)(void); void (*set_tx_mode)(void);
...@@ -24,6 +25,8 @@ typedef struct ...@@ -24,6 +25,8 @@ typedef struct
/* public functions */ /* public functions */
void dyn_master_init(TDynamixelMaster *master,TComm *dev); void dyn_master_init(TDynamixelMaster *master,TComm *dev);
void dyn_master_set_rx_timeout(TDynamixelMaster *master,unsigned short int timeout_ms); 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);
void dyn_master_scan(TDynamixelMaster *master,unsigned char *num,unsigned char *ids); 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_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_byte(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned char *data);
......
...@@ -16,10 +16,6 @@ ...@@ -16,10 +16,6 @@
#define MAX_DYN_SLAVE_REG_BUFFER_LEN 1024 #define MAX_DYN_SLAVE_REG_BUFFER_LEN 1024
#endif #endif
typedef enum{no_return=0x00,
return_only_read=0x01,
return_all=0x02} return_level_t;
typedef struct typedef struct
{ {
TComm *comm_dev; TComm *comm_dev;
......
...@@ -90,7 +90,6 @@ void dummy_dyn_master_set_rx_mode(void) ...@@ -90,7 +90,6 @@ void dummy_dyn_master_set_rx_mode(void)
unsigned char dyn_master_wait_transmission(TDynamixelMaster *master) unsigned char dyn_master_wait_transmission(TDynamixelMaster *master)
{ {
unsigned short int timeout_left=master->rx_timeout_ms*1000;
unsigned char error; unsigned char error;
while((error=comm_is_send_done(master->comm_dev))==COMM_BUSY); while((error=comm_is_send_done(master->comm_dev))==COMM_BUSY);
...@@ -160,6 +159,7 @@ void dyn_master_init(TDynamixelMaster *master,TComm *dev) ...@@ -160,6 +159,7 @@ void dyn_master_init(TDynamixelMaster *master,TComm *dev)
master->rx_timeout_ms=50; master->rx_timeout_ms=50;
master->rx_no_answer=0x00; master->rx_no_answer=0x00;
master->rx_num_packets=0x00; master->rx_num_packets=0x00;
master->return_level=return_all;
master->set_rx_mode(); master->set_rx_mode();
} }
...@@ -169,6 +169,16 @@ void dyn_master_set_rx_timeout(TDynamixelMaster *master,unsigned short int timeo ...@@ -169,6 +169,16 @@ void dyn_master_set_rx_timeout(TDynamixelMaster *master,unsigned short int timeo
master->rx_timeout_ms=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) void dyn_master_scan(TDynamixelMaster *master,unsigned char *num,unsigned char *ids)
{ {
...@@ -204,51 +214,227 @@ unsigned char dyn_master_ping(TDynamixelMaster *master,unsigned char id) ...@@ -204,51 +214,227 @@ 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_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 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 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) 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 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 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 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 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 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 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;
} }
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