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

Added the implementation of a dynamixel slave module.

parent b4c3dc6e
No related branches found
No related tags found
No related merge requests found
......@@ -10,10 +10,12 @@ INCLUDE_DIRECTORIES(.)
# edit the following line to find the necessary packages
FIND_PACKAGE(iriutils REQUIRED)
FIND_PACKAGE(comm REQUIRED)
find_package(Boost REQUIRED)
# edit the following line to add the necessary include directories
INCLUDE_DIRECTORIES(${iriutils_INCLUDE_DIR})
INCLUDE_DIRECTORIES(${comm_INCLUDE_DIR})
INCLUDE_DIRECTORIES(${Boost_INCLUDE_DIRS})
SET_SOURCE_FILES_PROPERTIES(${XSD_SOURCES} PROPERTIES GENERATED 1)
......@@ -23,6 +25,7 @@ ADD_LIBRARY(dynamixel SHARED ${sources} ${XSD_SOURCES})
TARGET_LINK_LIBRARIES(dynamixel ${iriutils_LIBRARY})
TARGET_LINK_LIBRARIES(dynamixel ${comm_LIBRARY})
TARGET_LINK_LIBRARIES(dynamixel ${XSD_LIBRARY})
TARGET_LINK_LIBRARIES(dynamixel ${Boost_LIBRARIES})
ADD_DEPENDENCIES(dynamixel xsd_files_gen)
......
......@@ -9,21 +9,21 @@ typedef enum {dyn_ping=0x01,
dyn_reg_write=0x04,
dyn_action=0x05,
dyn_factory_reset=0x06,
dyn_reboot=0x08,
dyn_reset=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_NO_ERROR=0x00,
DYN_INST_ERROR=0x40,
DYN_OVERLOAD_ERROR=0x20,
DYN_CHECKSUM_ERROR=0x10,
DYN_RANGE_ERROR=0x08,
DYN_OVERTEMP_ERROR=0x04,
DYN_ANGLE_ERROR=0x02,
DYN_VOLTAGE_ERROR=0x01} TDynError;
typedef enum{dyn_no_error=0x00,
dyn_inst_error=0x40,
dyn_overload_error=0x20,
dyn_checksum_error=0x10,
dyn_range_error=0x08,
dyn_overtemp_error=0x04,
dyn_angle_error=0x02,
dyn_voltage_error=0x01} dyn_error_t;
typedef enum {dyn_reset_all=0xFF,dyn_reset_keep_id=0x01,dyn_reset_keep_id_baud=0x02} dyn_reset_mode_t;
......
This diff is collapsed.
......@@ -10,6 +10,29 @@
#include <stdlib.h>
#include <string>
#include <vector>
#include <boost/function.hpp>
typedef boost::function<void (void)> on_ping_fnct;
typedef boost::function<unsigned char (unsigned short int,unsigned short int,unsigned char *)> on_rw_fnct;
typedef struct
{
unsigned char id;
on_ping_fnct on_ping;
on_rw_fnct on_read;
on_rw_fnct on_write;
unsigned short int reg_address;
unsigned short int reg_length;
unsigned char *reg_data;
unsigned char bulk_prev_id;
unsigned short int bulk_address;
unsigned short int bulk_length;
bool bulk_read_pending;
unsigned char sync_prev_id;
unsigned short int sync_address;
unsigned short int sync_length;
bool sync_read_pending;
}TSlave;
/**
* \brief
......@@ -43,6 +66,11 @@ class CDynamixelSlave
*
*/
std::string process_packets_thread_id;
/**
* \brief
*
*/
std::string dynamixel_loop_thread_id;
/**
* \brief
*
......@@ -63,6 +91,16 @@ class CDynamixelSlave
*
*/
dyn_version_t dyn_ver;
/**
* \brief
*
*/
std::vector <TSlave> slaves;
/**
* \brief
*
*/
std::vector<unsigned char *> packets;
protected:
/**
* \brief mutual exclusion mechanism to access the usb
......@@ -83,152 +121,182 @@ class CDynamixelSlave
* \brief
*
*/
void handle_error(unsigned char error);
static void *dynamixel_loop_thread(void *params);
/**
* \brief
*
*/
void start(void);
void handle_error(unsigned char error);
/**
* \brief
*
*/
void stop(void);
public:
void send_status_packet(unsigned char id,unsigned char error, unsigned short int length, unsigned char *data);
/**
* \brief
* \brief
*
*/
CDynamixelSlave(const std::string& cont_id,dyn_version_t dyn_ver=dyn_version1);
*/
void start(void);
/**
* \brief
*
*/
std::string get_new_packet_available_event_id(void);
*
*/
void stop(void);
/**
* \brief
*
*/
virtual void set_baudrate(int baudrate)=0;
void dynamixel_loop_v1(void);
/**
* \brief
*
*/
virtual int get_baudrate(void)=0;
void dynamixel_loop_v2(void);
/**
* \brief
*
*/
void set_return_delay(unsigned int time_us);
unsigned char get_target_id(unsigned char *data);
/**
* \brief
*
*/
unsigned int get_return_delay(void);
dyn_inst_t get_instruction_type(unsigned char *data);
/**
* \brief
*
*/
void set_return_level(return_level_t level);
unsigned short int get_packet_length(unsigned char *data);
/**
* \brief
*
*/
return_level_t get_return_level(void);
unsigned short int get_checksum(unsigned char *data);
// instruction specific functions
/* read instruction */
/**
* \brief
*
*/
bool new_packet_available(void);
unsigned short int get_read_length(unsigned char *data);
/**
* \brief
*
*/
unsigned char get_target_id(void);
unsigned short int get_read_address(unsigned char *data);
/* write instruction */
/**
* \brief
*
*/
dyn_inst_t get_instruction_type(void);
// instruction specific functions
/* read instruction */
unsigned short int get_write_address(unsigned char *data);
/**
* \brief
*
*/
unsigned short int get_read_length(void);
unsigned short int get_write_length(unsigned char *data);
/**
* \brief
*
*/
unsigned short int get_read_address(void);
/* write instruction */
unsigned short int get_write_data(unsigned char *data, unsigned char **write_data);
/* registered write instruction */
/**
* \brief
*
*/
unsigned short int get_write_address(void);
unsigned short int get_reg_write_address(unsigned char *data);
/**
* \brief
*
*/
unsigned short int get_write_length(void);
unsigned short int get_reg_write_length(unsigned char *data);
/**
* \brief
*
*/
void get_write_data(std::vector<unsigned char> &data);
/* registered write instruction */
unsigned short int get_reg_write_data(unsigned char *data,unsigned char **write_data);
/* sync write instruction */
/**
* \brief
*
*/
unsigned short int get_reg_write_address(void);
*/
bool sync_write_id_present(unsigned char *data,unsigned char id,unsigned short int *address,unsigned short int *length,unsigned char **write_data);
/* sync read instruction */
/**
* \brief
*
*/
unsigned char sync_read_id_present(unsigned char *data,unsigned char id,unsigned short int *address,unsigned short int *length);
/* bulk read instruction */
/**
* \brief
*
*/
unsigned char bulk_read_id_present(unsigned char *data,unsigned char id,unsigned short int *address,unsigned short int *length);
/* bulk write instruction */
/**
* \brief
*
*/
bool bulk_write_id_present(unsigned char *data,unsigned char id,unsigned short int *address,unsigned short int *length,unsigned char **write_data);
// status return
/**
* \brief
*
*/
void send_status_packet(unsigned char id,dyn_error_t error,unsigned short int length,unsigned char *data);
public:
/**
* \brief
*
*/
unsigned short int get_reg_write_length(void);
CDynamixelSlave(const std::string& cont_id,dyn_version_t dyn_ver=dyn_version1);
/**
* \brief
*
*/
virtual void set_baudrate(int baudrate)=0;
/**
* \brief
*
*/
virtual int get_baudrate(void)=0;
/**
* \brief
*
*/
void get_reg_write_data(std::vector<unsigned char> &data);
/* sync write instruction */
void add_slave(unsigned char id, on_ping_fnct on_ping, on_rw_fnct on_read, on_rw_fnct on_write);
/**
* \brief
*
*/
bool sync_write_id_present(unsigned char id,unsigned short int *address,unsigned short int *length,std::vector<unsigned char> &data);
/* sync read instruction */
void delete_slave(unsigned char id);
/**
* \brief
*
*/
bool sync_read_id_present(unsigned char id,unsigned short int *address,unsigned short int *length);
/* bulk read instruction */
unsigned int get_num_slaves(void);
/**
* \brief
*
*/
bool bulk_read_id_present(unsigned char id,std::vector<unsigned short int> &address,std::vector<unsigned short int> &length);
/* bulk write instruction */
unsigned int get_return_delay(void);
/**
* \brief
*
*/
bool bulk_write_id_present(unsigned char id,std::vector<unsigned short int> &address,std::vector<unsigned short int> &length,std::vector<unsigned char> &data);
// status return
void set_return_delay(unsigned int delay_us);
/**
* \brief
*
*/
void send_status_packet(unsigned char id,TDynError error,unsigned short int length,std::vector<unsigned char> &data);
return_level_t get_return_level(void);
/**
* \brief
*
*/
void next_packet(void);
void get_return_level(return_level_t level);
/**
* \brief
*
......
......@@ -5,6 +5,7 @@
const std::string dynamixel_error_message="Dynamixel error - ";
const std::string dynamixel_server_error_message="Dynamixel server error - ";
const std::string dynamixel_slave_error_message="Dynamixel slave error - ";
const std::string dynamixel_sync_error="Dynamixel synchronization event";
CDynamixelException::CDynamixelException(const std::string& where,const std::string &error_msg,int dev_id):CException(where,dynamixel_error_message)
......@@ -44,3 +45,8 @@ CDynamixelServerException::CDynamixelServerException(const std::string& where,co
this->error_msg+=error_msg;
}
CDynamixelSlaveException::CDynamixelSlaveException(const std::string& where,const std::string &error_msg):CException(where,dynamixel_slave_error_message)
{
this->error_msg+=error_msg;
}
......@@ -132,4 +132,25 @@ class CDynamixelServerException : public CException
CDynamixelServerException(const std::string& where,const std::string &error_msg);
};
/**
* \brief Dynamixel slave exception
*
*/
class CDynamixelSlaveException : public CException
{
public:
/**
* \brief Class constructor
*
* \param where a null terminated string with the information about the name
* of the function, the source code filename and the line where
* the exception was generated. This string must be generated
* by the _HERE_ macro.
*
* \param error_msg a null terminated string with the details of the error.
*/
CDynamixelSlaveException(const std::string& where,const std::string &error_msg);
};
#endif
......@@ -2,6 +2,8 @@
#include "dynamixelserver_ftdi.h"
#include <iostream>
std::string serial="A400gaIt";
int main(int argc, char *argv[])
{
CDynamixelServerFTDI *dyn_server=CDynamixelServerFTDI::instance();
......@@ -20,7 +22,7 @@ int main(int argc, char *argv[])
try{
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->config_bus(serial,1000000);
dyn_server->start_scan();
event_id=event_server->wait_first(events);
if(event_id==0)
......
#include "eventexceptions.h"
#include "dynamixel_slave_serial.h"
#include "dynamixel_slave_ftdi.h"
#include <iostream>
#include <boost/bind.hpp>
std::string serial="A400gavm";
void on_ping(void)
{
std::cout << "device pinged" << std::endl;
}
unsigned char on_read(unsigned short int address, unsigned short int length, unsigned char *data)
{
std::cout << "read operation at address " << address << " with length " << length << std::endl;
return 0x00;
}
unsigned char on_write(unsigned short int address, unsigned short int length, unsigned char *data)
{
std::cout << "wrirte operation at address " << address << " with length " << length << std::endl;
return 0x00;
}
int main(int argc, char *argv[])
{
int num_buses=0;
try{
CDynamixelSlaveSerial slave("slave","/dev/ttyUSB1");
slave.set_baudrate(1000000);
sleep(10);
std::cout << "exit" << std::endl;
CDynamixelSlaveFTDI slave("slave");
num_buses=slave.get_num_buses();
std::cout << "Num. buses: " << num_buses << std::endl;
if(num_buses>0)
{
slave.config_bus(serial,1000000);
slave.add_slave(0x10,boost::bind(&on_ping),boost::bind(&on_read,_1,_2,_3),boost::bind(&on_write,_1,_2,_3));
sleep(10000);
std::cout << "exit" << std::endl;
}
}catch(CException &e){
std::cout << e.what() << std::endl;
}
......
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