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

Added support for the 2.0 revision of the dynamixel protocol.

parent c4d3a705
No related branches found
No related tags found
No related merge requests found
Expand all Hide whitespace changes
Inline Side-by-side
src/dynamixel.cpp
+ 484
− 759
View file @ 9a4732cf
This diff is collapsed.
src/dynamixel.h
+ 62
− 14
View file @ 9a4732cf
......@@ -12,6 +12,23 @@
const int NUM_RETRIES=10;
typedef enum {dyn_version1,dyn_version2} dyn_version_t;
typedef enum {dyn_ping=0x01,
dyn_read=0x02,
dyn_write=0x03,
dyn_reg_write=0x04,
dyn_action=0x05,
dyn_factory_reset=0x06,
dyn_reboot=0x08,
dyn_status=0x55,
dyn_sync_read=0x82,
dyn_sync_write=0x83,
dyn_bulk_read=0x92,
dyn_bulk_write=0x93} dyn_inst_t;
typedef enum {dyn_reset_all=0xFF,dyn_reset_keep_id=0x01,dyn_reset_keep_id_baud=0x02} dyn_reset_mode_t;
/**
* \brief
*
......@@ -55,11 +72,6 @@ class CDynamixel
*
*/
unsigned char baudrate_register;
/**
* \brief Function to compute the checksum of the data packets
*
*/
unsigned char compute_checksum(unsigned char *packet,int len);
/**
* \brief
*
......@@ -70,6 +82,37 @@ class CDynamixel
*
*/
void resync(void);
/**
* \brief
*
*/
dyn_version_t version;
protected:
/**
* \brief
*
*/
void send_instruction_packet_v1(dyn_inst_t inst,unsigned char *data=NULL,unsigned char len=0);
/**
* \brief
*
*/
void send_instruction_packet_v2(dyn_inst_t inst,unsigned char *data=NULL,unsigned short int len=0);
/**
* \brief
*
*/
unsigned char receive_status_packet_v1(unsigned char **data,unsigned char *len);
/**
* \brief
*
*/
unsigned char receive_status_packet_v2(unsigned char **data,unsigned short int *len);
/**
* \brief
*
*/
void handle_error(unsigned char error);
public:
/**
* \brief
......@@ -80,47 +123,52 @@ class CDynamixel
* \brief Function to read a register
*
*/
void read_byte_register(unsigned char address, unsigned char *value);
void read_byte_register(unsigned short int address, unsigned char *value);
/**
* \brief Function to read a register
*
*/
void read_word_register(unsigned char address,unsigned short int *value);
void read_word_register(unsigned short int address,unsigned short int *value);
/**
* \brief Function to write to a register
*
*/
void write_byte_register(unsigned short int address, unsigned char value);
/**
* \brief Function to write to a register
*
*/
void write_byte_register(unsigned char address, unsigned char data);
void write_word_register(unsigned short int address, unsigned short int value);
/**
* \brief Function to write to a register
*
*/
void write_word_register(unsigned char address, unsigned short int data);
void registered_byte_write(unsigned short int address, unsigned char value);
/**
* \brief Function to write to a register
*
*/
void registered_byte_write(unsigned char address, unsigned char data);
void registered_word_write(unsigned short int address, unsigned short int value);
/**
* \brief Function to write to a register
*
*/
void registered_word_write(unsigned char address, unsigned short int data);
void registered_write(unsigned short int address, unsigned char *values,unsigned int length);
/**
* \brief Function to write to a register
*
*/
void write_registers(unsigned char address, unsigned char *data, unsigned int length);
void write_registers(unsigned short int address, unsigned char *values, unsigned int length);
/**
* \brief Function to write to a register
*
*/
void read_registers(unsigned char address, unsigned char *data, unsigned int length);
void read_registers(unsigned short int address, unsigned char *values, unsigned int length);
/**
* \brief
*
*/
void reset(void);
void reset(dyn_reset_mode_t mode=dyn_reset_all);
/**
* \brief
*
......
src/dynamixelserver.cpp
+ 776
− 160
View file @ 9a4732cf
This diff is collapsed.
src/dynamixelserver.h
+ 97
− 11
View file @ 9a4732cf
......@@ -52,7 +52,8 @@ typedef struct
typedef struct
{
unsigned char id;
unsigned char id;
dyn_version_t version;
bool used;
}TDynDevice;
......@@ -89,6 +90,11 @@ class CDynamixelServer
*
*/
std::string scan_done_event_id;
/**
* \brief
*
*/
std::string scan_error_event_id;
/**
* \brief
*
......@@ -123,7 +129,42 @@ class CDynamixelServer
* \brief
*
*/
std::vector<TDynDevice> devices;
std::vector<TDynDevice> devices_v1;
/**
* \brief
*
*/
std::vector<TDynDevice> devices_v2;
/**
* \brief
*
*/
std::string scan_error;
/**
* \brief
*
*/
dyn_version_t scan_version;
/**
* \brief
*
*/
void send_instruction_packet_v1(dyn_inst_t inst,unsigned char *data=NULL,unsigned char len=0,unsigned char id=0xFE);
/**
* \brief
*
*/
void send_instruction_packet_v2(dyn_inst_t inst,unsigned char *data=NULL,unsigned short int len=0,unsigned char id=0xFE);
/**
* \brief
*
*/
unsigned char receive_status_packet_v1(unsigned char **data,unsigned char *len);
/**
* \brief
*
*/
unsigned char receive_status_packet_v2(unsigned char **data,unsigned short int *len);
protected:
/**
* \brief
......@@ -149,7 +190,12 @@ class CDynamixelServer
* \brief
*
*/
unsigned char compute_checksum(unsigned char *packet,int len);
static void byte_stuffing(unsigned char *packet_in,int len_in,unsigned char *packet_out,int *len_out);
/**
* \brief
*
*/
static void byte_destuffing(unsigned char *packet_in,int len_in,unsigned char *packet_out,int *len_out);
/**
* \brief
*
......@@ -160,12 +206,27 @@ class CDynamixelServer
*
*/
void set_bus_id(std::string &bus_id);
/**
* \brief
*
*/
void handle_error(unsigned char error);
public:
/**
* \brief
*
*/
static CDynamixelServer *instance(void);
/**
* \brief
*
*/
static unsigned char compute_checksum_v1(unsigned char *packet,int len);
/**
* \brief
*
*/
static unsigned short int compute_checksum_v2(unsigned char *packet,int len);
/**
* \brief
*
......@@ -200,7 +261,7 @@ class CDynamixelServer
* \brief
*
*/
void start_scan(void);
void start_scan(dyn_version_t version=dyn_version1);
/**
* \brief
*
......@@ -216,6 +277,16 @@ class CDynamixelServer
*
*/
std::string get_scan_done_event_id(void);
/**
* \brief
*
*/
std::string get_scan_error_event_id(void);
/**
* \brief
*
*/
std::string get_scan_error(void);
/**
* \brief
*
......@@ -225,32 +296,32 @@ class CDynamixelServer
* \brief
*
*/
int get_num_devices(void);
int get_num_devices(dyn_version_t version=dyn_version1);
/**
* \brief
*
*/
std::vector<int> get_device_ids(void);
std::vector<int> get_device_ids(dyn_version_t version=dyn_version1);
/**
* \brief
*
*/
CDynamixel *get_device(int dev_id);
CDynamixel *get_device(int dev_id,dyn_version_t version=dyn_version1);
/**
* \brief
*
*/
void free_device(int dev_id);
void free_device(int dev_id,dyn_version_t version=dyn_version1);
/**
* \brief
*
*/
void action(void);
void action(dyn_version_t version=dyn_version1);
/**
* \brief
*
*/
void ping(int dev_id,int time=100);
void ping(int dev_id,int time=100,dyn_version_t version=dyn_version1,unsigned short *model=NULL,unsigned char *fw_ver=NULL);
/**
* \brief
*
......@@ -260,7 +331,22 @@ class CDynamixelServer
* \brief
*
*/
void write_sync(std::vector<unsigned char>& servo_ids,unsigned char start_addr, std::vector< std::vector<unsigned char> >& data);
void write_sync(std::vector<unsigned char>& servo_ids,unsigned short int start_addr, std::vector< std::vector<unsigned char> >& data,dyn_version_t version=dyn_version1);
/**
* \brief
*
*/
void read_sync(std::vector<unsigned char>& servo_ids,unsigned short int start_addr,unsigned short int length, std::vector< std::vector<unsigned char> >& data,dyn_version_t version=dyn_version2);
/**
* \brief
*
*/
void write_bulk(std::vector<unsigned char>& servo_ids,std::vector<unsigned short int> &start_addr, std::vector< std::vector<unsigned char> >& data,dyn_version_t version=dyn_version2);
/**
* \brief
*
*/
void read_bulk(std::vector<unsigned char>& servo_ids,std::vector<unsigned short int> &start_addr,std::vector<unsigned short int> &length, std::vector< std::vector<unsigned char> >& data,dyn_version_t version=dyn_version2);
};
#endif
src/examples/test_dynamixel_server.cpp
+ 55
− 17
View file @ 9a4732cf
......@@ -6,10 +6,12 @@ int main(int argc, char *argv[])
{
CDynamixelServer *dyn_server=CDynamixelServer::instance();
CEventServer *event_server=CEventServer::instance();
int num_buses=0,num_dev=0,baudrate=0,i=0;
int num_buses=0,num_dev=0,baudrate=0,i=0,event_id;
std::list<std::string> events;
std::vector<float> position;
std::vector<int> devices;
unsigned short int model;
unsigned char margin;
CDynamixel *dyn_motor;
num_buses=dyn_server->get_num_buses();
......@@ -17,17 +19,33 @@ int main(int argc, char *argv[])
if(num_buses>0)
{
events.push_back(dyn_server->get_scan_done_event_id());
events.push_back(dyn_server->get_scan_error_event_id());
dyn_server->config_bus(0,1000000);
dyn_server->start_scan();
event_server->wait_first(events);
num_dev=dyn_server->get_num_devices();
std::cout << "Num. devices: " << num_dev << std::endl;
baudrate=dyn_server->get_baudrate();
std::cout << "Baudrate: " << baudrate << " bps" << std::endl;
devices=dyn_server->get_device_ids();
for(i=0;i<num_dev;i++)
std::cout << "Device " << i << ": id -> " << devices[i] << std::endl;
dyn_motor=dyn_server->get_device(devices[0]);
event_id=event_server->wait_first(events);
if(event_id==0)
{
num_dev=dyn_server->get_num_devices();
std::cout << "Num. devices: " << num_dev << std::endl;
baudrate=dyn_server->get_baudrate();
std::cout << "Baudrate: " << baudrate << " bps" << std::endl;
devices=dyn_server->get_device_ids();
for(i=0;i<num_dev;i++)
{
dyn_motor=dyn_server->get_device(devices[i]);
dyn_motor->write_byte_register(0x1A,0x10);
dyn_motor->read_word_register(0x00,&model);
dyn_motor->read_byte_register(0x1A,&margin);
dyn_motor->write_word_register(0x1E,0x03FF);
sleep(2);
dyn_motor->write_word_register(0x1E,0x000);
sleep(2);
std::cout << "Device " << std::dec << i << ": id -> " << devices[i] << " model: " << std::hex << model << " margin: " << (int)margin << std::endl;
dyn_server->free_device(devices[i]);
}
}
else
std::cout << "Error while scanning the bus: " << dyn_server->get_scan_error() << std::endl;
try{
dyn_server->config_bus(0,1000000);
dyn_server->start_scan();
......@@ -36,12 +54,32 @@ int main(int argc, char *argv[])
dyn_server->stop_scan();
std::cout << "Scanning canceled !!!" << std::endl;
}
num_dev=dyn_server->get_num_devices();
std::cout << "Num. devices: " << num_dev << std::endl;
baudrate=dyn_server->get_baudrate();
std::cout << "Baudrate: " << baudrate << " bps" << std::endl;
devices=dyn_server->get_device_ids();
for(i=0;i<num_dev;i++)
std::cout << "Device " << i << ": id -> " << devices[i] << std::endl;
/* scan for version 2 devices */
dyn_server->config_bus(0,1000000);
dyn_server->start_scan(dyn_version2);
event_id=event_server->wait_first(events);
if(event_id==0)
{
num_dev=dyn_server->get_num_devices(dyn_version2);
std::cout << "Num. devices: " << num_dev << std::endl;
baudrate=dyn_server->get_baudrate();
std::cout << "Baudrate: " << baudrate << " bps" << std::endl;
devices=dyn_server->get_device_ids(dyn_version2);
for(i=0;i<num_dev;i++)
{
dyn_motor=dyn_server->get_device(devices[i],dyn_version2);
dyn_motor->write_byte_register(0x1A,0x10);
dyn_motor->read_word_register(0x00,&model);
dyn_motor->read_byte_register(0x1A,&margin);
dyn_motor->write_word_register(0x1E,0x03FF);
sleep(2);
dyn_motor->write_word_register(0x1E,0x000);
sleep(2);
std::cout << "Device " << std::dec << i << ": id -> " << devices[i] << " model: " << std::hex << model << " margin: " << (int)margin << std::endl;
dyn_server->free_device(devices[i],dyn_version2);
}
}
else
std::cout << "Error while scanning the bus: " << dyn_server->get_scan_error() << std::endl;
}
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment