diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 15f5b02b3f9a37f6fc2b1a4135b500d77ad7dae7..95f8ba91984e0b15d0aab74a43850b534d9bc116 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -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)
diff --git a/src/dynamixel_common.h b/src/dynamixel_common.h
index a28da96a7228f0cdbd08e3b62bbb855143fca973..269105c7101294a99fe8b359046de062f7798e7f 100644
--- a/src/dynamixel_common.h
+++ b/src/dynamixel_common.h
@@ -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;
diff --git a/src/dynamixel_slave.cpp b/src/dynamixel_slave.cpp
index d5b12075db9f3d034eb5f44ae9d02f60dab59707..518198e128fa4bdc62de533ab31394262fd01a42 100644
--- a/src/dynamixel_slave.cpp
+++ b/src/dynamixel_slave.cpp
@@ -1,4 +1,6 @@
#include "dynamixel_slave.h"
+#include "dynamixelserver.h"
+#include "dynamixelexceptions.h"
#include <iostream>
CDynamixelSlave::CDynamixelSlave(const std::string& cont_id,dyn_version_t dyn_ver)
@@ -10,14 +12,17 @@ CDynamixelSlave::CDynamixelSlave(const std::string& cont_id,dyn_version_t dyn_ve
this->event_server->create_event(this->finish_thread_event_id);
this->thread_server=CThreadServer::instance();
- this->process_packets_thread_id=cont_id + "_thread_id";
+ this->process_packets_thread_id=cont_id + "process_packets_thread_id";
this->thread_server->create_thread(this->process_packets_thread_id);
this->thread_server->attach_thread(this->process_packets_thread_id,this->process_packets_thread,this);
+ this->dynamixel_loop_thread_id=cont_id + "dynamixel_loop_thread_id";
+ this->thread_server->create_thread(this->dynamixel_loop_thread_id);
+ this->thread_server->attach_thread(this->dynamixel_loop_thread_id,this->dynamixel_loop_thread,this);
this->return_delay=0;
this->return_level=return_all;
this->dyn_ver=dyn_ver;
-
+
this->comm_dev=NULL;
}
@@ -26,13 +31,13 @@ void *CDynamixelSlave::process_packets_thread(void *params)
CDynamixelSlave *slave=(CDynamixelSlave *)params;
std::list<std::string> events;
int event_index,num,i;
- unsigned char *data;
+ unsigned char *data,*new_data;
bool end=false;
static bool data_phase=false;
static unsigned char packet[256];
static int num_bytes=0,length=0;
-
+
// wait until the comm device becomes valid
while(!end)
{
@@ -52,16 +57,19 @@ void *CDynamixelSlave::process_packets_thread(void *params)
else
{
// process the incomming data
+ slave->comm_access.enter();
num=slave->comm_dev->get_num_data();
- std::cout << "num data: " << num << std::endl;
data=new unsigned char[num];
if(slave->comm_dev->read(data,num)!=num)
+ {
+ slave->comm_access.exit();
std::cout << "Error while reading the communication device" << std::endl;
+ }
else
{
+ slave->comm_access.exit();
for(i=0;i<num;i++)
{
- std::cout << std::hex << (int)data[i] << std::endl;
if(!data_phase)
{
switch(num_bytes)
@@ -117,7 +125,6 @@ void *CDynamixelSlave::process_packets_thread(void *params)
case 6: packet[num_bytes]=data[i];
num_bytes++;
length+=(data[i]<<8);
- num_bytes++;
/* read packet_data */
data_phase=true;
break;
@@ -131,15 +138,46 @@ void *CDynamixelSlave::process_packets_thread(void *params)
length--;
if(length==0)
{
- num_bytes=0;
data_phase=false;
- std::cout << "new packet" << std::endl;
+ new_data=new unsigned char[num_bytes];
+ memcpy(new_data,packet,num_bytes);
+ slave->packets.push_back(new_data);
+ slave->event_server->set_event(slave->new_packet_available_event_id);
+ num_bytes=0;
}
}
}
}
}
delete[] data;
+ data=NULL;
+ }
+
+ pthread_exit(NULL);
+}
+
+void *CDynamixelSlave::dynamixel_loop_thread(void *params)
+{
+ CDynamixelSlave *slave=(CDynamixelSlave *)params;
+ std::list<std::string> events;
+ int event_index;
+ bool end=false;
+
+ events.push_back(slave->finish_thread_event_id);
+ events.push_back(slave->new_packet_available_event_id);
+ while(!end)
+ {
+ event_index=slave->event_server->wait_first(events);
+ if(event_index==0)
+ end=true;
+ else
+ {
+ // process the packets
+ if(slave->dyn_ver==dyn_version1)
+ slave->dynamixel_loop_v1();
+ else
+ slave->dynamixel_loop_v2();
+ }
}
pthread_exit(NULL);
@@ -154,149 +192,682 @@ void CDynamixelSlave::start(void)
{
if(this->thread_server->get_thread_state(this->process_packets_thread_id)==attached)
this->thread_server->start_thread(this->process_packets_thread_id);
+ if(this->thread_server->get_thread_state(this->dynamixel_loop_thread_id)==attached)
+ this->thread_server->start_thread(this->dynamixel_loop_thread_id);
}
void CDynamixelSlave::stop(void)
{
+ this->event_server->set_event(this->finish_thread_event_id);
+ this->event_server->set_event(this->finish_thread_event_id);
+ if(this->thread_server->get_thread_state(this->dynamixel_loop_thread_id)==active || this->thread_server->get_thread_state(this->dynamixel_loop_thread_id)==starting)
+ this->thread_server->end_thread(this->dynamixel_loop_thread_id);
if(this->thread_server->get_thread_state(this->process_packets_thread_id)==active || this->thread_server->get_thread_state(this->process_packets_thread_id)==starting)
- {
- this->event_server->set_event(this->finish_thread_event_id);
this->thread_server->end_thread(this->process_packets_thread_id);
- }
}
-std::string CDynamixelSlave::get_new_packet_available_event_id(void)
+void CDynamixelSlave::dynamixel_loop_v1(void)
{
- return this->new_packet_available_event_id;
+ unsigned short int length,address;
+ unsigned char error,id,prev_id;;
+ unsigned char *data,*write_data,*read_data;
+ unsigned int i=0;
+
+ data=this->packets[0];
+ id=this->get_target_id(data);
+ for(i=0;i<this->slaves.size();i++)
+ {
+ if(id==this->slaves[i].id || id==0xFE)// the packet is addressed to this device or it is a broadcast
+ {
+ // check the packet checksum
+ if(CDynamixelServer::compute_checksum_v1(data,this->get_packet_length(data))==0x00)// the incomming packet is okay
+ {
+ // process the packet
+ switch(this->get_instruction_type(data))
+ {
+ case dyn_ping: this->slaves[i].on_ping();
+ if(id!=0xFE)
+ this->send_status_packet(this->slaves[i].id,dyn_no_error,0,NULL);
+ break;
+ case dyn_read: read_data=new unsigned char[this->get_read_length(data)];
+ error=this->slaves[i].on_read(this->get_read_address(data),this->get_read_length(data),read_data);
+ if(this->return_level!=no_return && id!=0xFE)
+ {
+ if(error==dyn_no_error)
+ this->send_status_packet(this->slaves[i].id,dyn_no_error,this->get_read_length(data),read_data);
+ else
+ this->send_status_packet(this->slaves[i].id,dyn_inst_error,0,NULL);
+ }
+ delete[] read_data;
+ break;
+ case dyn_write: length=this->get_write_data(data,&write_data);
+ error=this->slaves[i].on_write(this->get_write_address(data),length,write_data);
+ if(this->return_level==return_all && id!=0xFE)
+ {
+ if(error==dyn_no_error)
+ this->send_status_packet(this->slaves[i].id,dyn_no_error,0,NULL);
+ else
+ this->send_status_packet(this->slaves[i].id,dyn_inst_error,0,NULL);
+ }
+ break;
+ case dyn_reg_write: this->slaves[i].reg_length=this->get_reg_write_data(data,&this->slaves[i].reg_data);
+ this->slaves[i].reg_address=this->get_reg_write_address(data);
+ if(this->return_level==return_all && id!=0xFE)
+ this->send_status_packet(this->slaves[i].id,dyn_no_error,0,NULL);
+ break;
+ case dyn_action: if(this->slaves[i].reg_address!=-1)
+ {
+ error=this->slaves[i].on_write(this->slaves[i].reg_address,this->slaves[i].reg_length,this->slaves[i].reg_data);
+ this->slaves[i].reg_address=-1;
+ }
+ else
+ if(this->return_level==return_all && id!=0xFE)
+ this->send_status_packet(this->slaves[i].id,dyn_inst_error,0,NULL);
+ break;
+ case dyn_reset:
+ break;
+ case dyn_sync_read: this->send_status_packet(this->slaves[i].id,dyn_inst_error,0,NULL);
+ break;
+ case dyn_sync_write: if(this->sync_write_id_present(data,this->slaves[i].id,&address,&length,&write_data))// the device is addressed
+ error=this->slaves[i].on_write(address,length,write_data);
+ break;
+ case dyn_bulk_read: prev_id=this->bulk_read_id_present(data,this->slaves[i].id,&address,&length);
+ if(prev_id!=0xFF)
+ {
+ if(prev_id==0x00)// first device to answer
+ {
+ read_data=new unsigned char[length];
+ error=this->slaves[i].on_read(address,length,read_data);
+ if(error==dyn_no_error)
+ this->send_status_packet(this->slaves[i].id,dyn_no_error,length,read_data);
+ else
+ this->send_status_packet(this->slaves[i].id,dyn_inst_error,0,NULL);
+ delete[] read_data;
+ }
+ else// wait for the previous device in the sequence to send its data
+ {
+ this->slaves[i].bulk_address=address;
+ this->slaves[i].bulk_length=length;
+ this->slaves[i].bulk_prev_id=prev_id;
+ this->slaves[i].bulk_read_pending=true;
+ }
+ }
+ break;
+ case dyn_bulk_write: this->send_status_packet(this->slaves[i].id,dyn_inst_error,0,NULL);
+ break;
+ default:
+ break;
+ }
+ }
+ else
+ {
+ // send a checksum error answer
+ if(this->get_target_id(data)!=0xFE)
+ this->send_status_packet(this->slaves[i].id,dyn_checksum_error,0,NULL);
+ }
+ }
+ else
+ {
+ if(this->slaves[i].bulk_read_pending)
+ {
+ if(id==this->slaves[i].bulk_prev_id)
+ {
+ this->slaves[i].bulk_read_pending=false;
+ read_data=new unsigned char[this->slaves[i].bulk_length];
+ error=this->slaves[i].on_read(this->slaves[i].bulk_address,this->slaves[i].bulk_length,read_data);
+ if(error==dyn_no_error)
+ this->send_status_packet(this->slaves[i].id,dyn_no_error,this->slaves[i].bulk_length,read_data);
+ else
+ this->send_status_packet(this->slaves[i].id,dyn_inst_error,0,NULL);
+ delete[] read_data;
+ }
+ }
+ }
+ }
+ this->packets.erase(this->packets.begin());
}
-void CDynamixelSlave::set_return_delay(unsigned int time_us)
+void CDynamixelSlave::dynamixel_loop_v2(void)
{
- if(time_us<1000)
- this->return_delay=time_us;
- else
+ unsigned char error,prev_id,id;
+ unsigned short int length,address,checksum,checksum_pkt;
+ unsigned char *data,*write_data,*read_data;
+ unsigned int i=0;
+
+ data=this->packets[0];
+ id=this->get_target_id(data);
+ for(i=0;i<this->slaves.size();i++)
{
- /* handle exceptions */
+ if(id==this->slaves[i].id || id==0xFE)// the packet is addressed to this device or it is a broadcast
+ {
+ // check the packet checksum
+ checksum=CDynamixelServer::compute_checksum_v2(data,this->get_packet_length(data)-2);
+ checksum_pkt=this->get_checksum(data);
+ if(checksum==checksum_pkt)
+ {
+ // process the packet
+ switch(this->get_instruction_type(data))
+ {
+ case dyn_ping: this->slaves[i].on_ping();
+ if(id!=0xFE)
+ this->send_status_packet(this->slaves[i].id,dyn_no_error,0,NULL);
+ break;
+ case dyn_read: read_data=new unsigned char[this->get_read_length(data)];
+ error=this->slaves[i].on_read(this->get_read_address(data),this->get_read_length(data),read_data);
+ if(this->return_level!=no_return && id!=0xFE)
+ {
+ if(error==dyn_no_error)
+ this->send_status_packet(this->slaves[i].id,dyn_no_error,this->get_read_length(data),read_data);
+ else
+ this->send_status_packet(this->slaves[i].id,dyn_inst_error,0,NULL);
+ }
+ delete[] read_data;
+ break;
+ case dyn_write: length=this->get_write_data(data,&write_data);
+ error=this->slaves[i].on_write(this->get_write_address(data),length,write_data);
+ if(this->return_level==return_all && id!=0xFE)
+ {
+ if(error==dyn_no_error)
+ this->send_status_packet(this->slaves[i].id,dyn_no_error,0,write_data);
+ else
+ this->send_status_packet(this->slaves[i].id,dyn_inst_error,0,NULL);
+ }
+ break;
+ case dyn_reg_write: this->slaves[i].reg_length=this->get_reg_write_data(data,&this->slaves[i].reg_data);
+ this->slaves[i].reg_address=this->get_reg_write_address(data);
+ if(this->return_level==return_all && id!=0xFE)
+ this->send_status_packet(this->slaves[i].id,dyn_no_error,0,NULL);
+ break;
+ case dyn_action: if(this->slaves[i].reg_address!=-1)
+ {
+ error=this->slaves[i].on_write(this->slaves[i].reg_address,this->slaves[i].reg_length,this->slaves[i].reg_data);
+ this->slaves[i].reg_address=-1;
+ }
+ else
+ if(this->return_level==return_all && id!=0xFE)
+ this->send_status_packet(this->slaves[i].id,dyn_inst_error,0,NULL);
+ break;
+ case dyn_reset:
+ break;
+ case dyn_sync_read: prev_id=this->sync_read_id_present(data,this->slaves[i].id,&address,&length);
+ if(prev_id!=0xFF)
+ {
+ if(prev_id==0x00)// first device to answer
+ {
+ read_data=new unsigned char[length];
+ error=this->slaves[i].on_read(address,length,read_data);
+ if(error==dyn_no_error)
+ this->send_status_packet(this->slaves[i].id,dyn_no_error,length,read_data);
+ else
+ this->send_status_packet(this->slaves[i].id,dyn_inst_error,0,NULL);
+ delete[] read_data;
+ }
+ else// wait for the previous device in the sequence to send its data
+ {
+ this->slaves[i].sync_address=address;
+ this->slaves[i].sync_length=length;
+ this->slaves[i].sync_prev_id=prev_id;
+ this->slaves[i].sync_read_pending=true;
+ }
+ }
+ break;
+ case dyn_sync_write: if(this->sync_write_id_present(data,this->slaves[i].id,&address,&length,&write_data))// the device is addressed
+ error=this->slaves[i].on_write(address,length,write_data);
+ break;
+ case dyn_bulk_read: prev_id=this->bulk_read_id_present(data,this->slaves[i].id,&address,&length);
+ if(prev_id!=0xFF)
+ {
+ if(prev_id==0x00)// first device to answer
+ {
+ read_data=new unsigned char[length];
+ error=this->slaves[i].on_read(address,length,read_data);
+ if(error==dyn_no_error)
+ this->send_status_packet(this->slaves[i].id,dyn_no_error,length,read_data);
+ else
+ this->send_status_packet(this->slaves[i].id,dyn_inst_error,0,NULL);
+ delete read_data;
+ }
+ else// wait for the previous device in the sequence to send its data
+ {
+ this->slaves[i].bulk_address=address;
+ this->slaves[i].bulk_length=length;
+ this->slaves[i].bulk_prev_id=prev_id;
+ this->slaves[i].bulk_read_pending=true;
+ }
+ }
+ break;
+ case dyn_bulk_write: if(this->bulk_write_id_present(data,this->slaves[i].id,&address,&length,&write_data))
+ error=this->slaves[i].on_write(address,length,write_data);
+ break;
+
+ default:
+ break;
+ }
+ }
+ else
+ {
+ // send a checksum error answer
+ if(this->get_target_id(data)!=0xFE)
+ this->send_status_packet(this->slaves[i].id,dyn_checksum_error,0,NULL);
+ }
+ }
+ else
+ {
+ if(this->slaves[i].bulk_read_pending)
+ {
+ if(id==this->slaves[i].bulk_prev_id)
+ {
+ this->slaves[i].bulk_read_pending=false;
+ read_data=new unsigned char[this->slaves[i].bulk_length];
+ error=this->slaves[i].on_read(this->slaves[i].bulk_address,this->slaves[i].bulk_length,read_data);
+ if(error==dyn_no_error)
+ this->send_status_packet(this->slaves[i].id,dyn_no_error,this->slaves[i].bulk_length,read_data);
+ else
+ this->send_status_packet(this->slaves[i].id,dyn_inst_error,0,NULL);
+ delete read_data;
+ }
+ }
+ else if(this->slaves[i].sync_read_pending)
+ {
+ if(id==this->slaves[i].sync_prev_id)
+ {
+ this->slaves[i].sync_read_pending=false;
+ read_data=new unsigned char[this->slaves[i].sync_length];
+ error=this->slaves[i].on_read(this->slaves[i].sync_address,this->slaves[i].sync_length,read_data);
+ if(error==dyn_no_error)
+ this->send_status_packet(this->slaves[i].id,dyn_no_error,this->slaves[i].sync_length,read_data);
+ else
+ this->send_status_packet(this->slaves[i].id,dyn_inst_error,0,NULL);
+ delete[] read_data;
+ }
+ }
+ }
}
+ this->packets.erase(this->packets.begin());
}
-unsigned int CDynamixelSlave::get_return_delay(void)
+unsigned char CDynamixelSlave::get_target_id(unsigned char *data)
{
- return this->return_delay;
+ if(this->dyn_ver==dyn_version1)
+ return data[2];
+ else
+ return data[4];
}
-void CDynamixelSlave::set_return_level(return_level_t level)
+dyn_inst_t CDynamixelSlave::get_instruction_type(unsigned char *data)
{
- this->return_level=level;
+ if(this->dyn_ver==dyn_version1)
+ return (dyn_inst_t)data[4];
+ else
+ return (dyn_inst_t)data[7];
}
-return_level_t CDynamixelSlave::get_return_level(void)
+unsigned short int CDynamixelSlave::get_packet_length(unsigned char *data)
{
- return this->return_level;
+ if(this->dyn_ver==dyn_version1)
+ return data[3]+4;
+ else
+ return data[5]+data[6]*256+7;
}
-bool CDynamixelSlave::new_packet_available(void)
+unsigned short int CDynamixelSlave::get_checksum(unsigned char *data)
{
- if(this->event_server->event_is_set(this->new_packet_available_event_id))
- return true;
+ unsigned short int length;
+
+ length=this->get_packet_length(data);
+ if(this->dyn_ver==dyn_version1)
+ return data[length-1];
else
- return false;
+ return data[length-2]+data[length-1]*256;
}
-unsigned char CDynamixelSlave::get_target_id(void)
+/* read instruction */
+unsigned short int CDynamixelSlave::get_read_length(unsigned char *data)
{
-
+ if(this->dyn_ver==dyn_version1)
+ return data[6];
+ else
+ return data[10]+data[11]*256;
}
-dyn_inst_t CDynamixelSlave::get_instruction_type(void)
+unsigned short int CDynamixelSlave::get_read_address(unsigned char *data)
{
-
+ if(this->dyn_ver==dyn_version1)
+ return data[5];
+ else
+ return data[8]+data[9]*256;
}
-// instruction specific functions
-/* read instruction */
-unsigned short int CDynamixelSlave::get_read_length(void)
+/* write instruction */
+unsigned short int CDynamixelSlave::get_write_address(unsigned char *data)
{
-
+ if(this->dyn_ver==dyn_version1)
+ return data[5];
+ else
+ return data[8]+data[9]*256;
}
-unsigned short int CDynamixelSlave::get_read_address(void)
+unsigned short int CDynamixelSlave::get_write_length(unsigned char *data)
{
+ if(this->dyn_ver==dyn_version1)
+ return data[3]-3;
+ else
+ return data[5]+data[6]*256-5;
+}
+unsigned short int CDynamixelSlave::get_write_data(unsigned char *data, unsigned char **write_data)
+{
+ if(this->dyn_ver==dyn_version1)
+ {
+ (*write_data)=&data[6];
+ return data[3]-3;
+ }
+ else
+ {
+ (*write_data)=&data[10];
+ return data[5]+data[6]*256-5;
+ }
}
-/* write instruction */
-unsigned short int CDynamixelSlave::get_write_address(void)
+/* registered write instruction */
+unsigned short int CDynamixelSlave::get_reg_write_address(unsigned char *data)
{
+ if(this->dyn_ver==dyn_version1)
+ return data[5];
+ else
+ return data[8]+data[9]*256;
+}
+unsigned short int CDynamixelSlave::get_reg_write_length(unsigned char *data)
+{
+ if(this->dyn_ver==dyn_version1)
+ return data[3]-3;
+ else
+ return data[5]+data[6]*256-5;
}
-unsigned short int CDynamixelSlave::get_write_length(void)
+unsigned short int CDynamixelSlave::get_reg_write_data(unsigned char *data,unsigned char **write_data)
{
+ if(*write_data!=NULL)
+ delete[] *write_data;
+ if(this->dyn_ver==dyn_version1)
+ {
+ *write_data=new unsigned char[data[3]-3];
+ memcpy(*write_data,&data[6],data[3]-3);
+ return data[3]-3;
+ }
+ else
+ {
+ *write_data=new unsigned char[data[5]+data[6]*256-5];
+ memcpy(*write_data,&data[10],data[5]+data[6]*256-5);
+ return data[5]+data[6]*256-5;
+ }
}
-void CDynamixelSlave::get_write_data(std::vector<unsigned char> &data)
+/* sync write instruction */
+bool CDynamixelSlave::sync_write_id_present(unsigned char *data,unsigned char id,unsigned short int *address,unsigned short int *length,unsigned char **write_data)
{
+ unsigned int num_servos=0,i,len;
+ if(this->dyn_ver==dyn_version1)
+ {
+ num_servos=(data[3]-4)/(data[6]+1);
+ for(i=0;i<num_servos;i++)
+ {
+ if(data[7+(data[6]+1)*i]==id)
+ {
+ *address=data[5];
+ *length=data[6];
+ (*write_data)=&data[7+(data[6]+1)*i+1];
+ return true;
+ }
+ }
+ return false;
+ }
+ else
+ {
+ len=data[10]+data[11]*256;
+ num_servos=((data[5]+data[6]*256)-7)/(len+1);
+ for(i=0;i<num_servos;i++)
+ {
+ if(data[12+(len+1)*i]==id)
+ {
+ (*address)=data[8]+data[9]*256;
+ (*length)=data[10]+data[11]*256;
+ (*write_data)=&data[12+(len+1)*i+1];
+ return true;
+ }
+ }
+ return false;
+ }
}
-/* registered write instruction */
-unsigned short int CDynamixelSlave::get_reg_write_address(void)
+/* sync read instruction */
+unsigned char CDynamixelSlave::sync_read_id_present(unsigned char *data,unsigned char id,unsigned short int *address,unsigned short int *length)
{
+ unsigned char num_servos,i;
+ if(this->dyn_ver==dyn_version2)
+ {
+ num_servos=(data[5]+data[6]*256)-7;
+ for(i=0;i<num_servos;i++)
+ if(data[12+i]==id)
+ {
+ (*address)=data[8]+data[9]*256;
+ (*length)=data[10]+data[11]*256;
+ if(i==0)
+ return 0x00;
+ else
+ return data[12+i];
+ }
+ return 0xFF;
+ }
+ else
+ return 0xFF;
}
-unsigned short int CDynamixelSlave::get_reg_write_length(void)
+/* bulk read instruction */
+unsigned char CDynamixelSlave::bulk_read_id_present(unsigned char *data,unsigned char id,unsigned short int *address,unsigned short int *length)
{
+ int num_servos,i;
+ unsigned char prev_id=0xFF;
+ if(this->dyn_ver==dyn_version1)
+ {
+ num_servos=(data[3]-3)/3;
+ for(i=0;i<num_servos;i++)
+ {
+ if(data[6+i*3+1]==id)
+ {
+ (*address)=data[6+i*3+2];
+ (*address)=data[6+i*3+3];
+ if(i==0)
+ return 0x00;
+ else
+ return prev_id;
+ }
+ else
+ prev_id=data[6+i*3+1];
+ }
+ return 0xFF;
+ }
+ else
+ {
+ num_servos=((data[5]+data[6]*256)-3)/5;
+ for(i=0;i<num_servos;i++)
+ {
+ if(data[8+i*5]==id)
+ {
+ (*address)=data[8+i*5+1]+data[9+i*5+1]*256;
+ (*length)=data[10+i*8+1]+data[11+i*5+1]*256;
+ if(i==0)
+ return 0x00;
+ else
+ return prev_id;
+ }
+ else
+ prev_id=data[8+i*5];
+ }
+ return 0xFF;
+ }
}
-void CDynamixelSlave::get_reg_write_data(std::vector<unsigned char> &data)
+/* bulk write instruction */
+bool CDynamixelSlave::bulk_write_id_present(unsigned char *data,unsigned char id,unsigned short int *address,unsigned short int *length,unsigned char **write_data)
{
+ unsigned short int len,offset=0;
+ len=data[5]+data[6]*256-3;
+ while(offset<len)
+ {
+ if(data[8+offset]==id)
+ {
+ (*address)=data[8+offset+1]+data[8+offset+2]*256;
+ (*length)=data[8+offset+3]+data[8+offset+4]*256;
+ (*write_data)=&data[8+offset+5];
+ return true;
+ }
+ else
+ offset+=data[8+offset+3]+data[8+offset+4]*256+5;
+ }
+
+ return false;
}
-/* sync write instruction */
-bool CDynamixelSlave::sync_write_id_present(unsigned char id,unsigned short int *address,unsigned short int *length,std::vector<unsigned char> &data)
+// status return
+void CDynamixelSlave::send_status_packet(unsigned char id,dyn_error_t error,unsigned short int length,unsigned char *data)
{
+ unsigned char *status_data;
+ unsigned short int checksum;
+
+ if(this->return_delay>0)
+ usleep(this->return_delay*2);
+ if(this->dyn_ver==dyn_version1)
+ {
+ this->comm_access.enter();
+ status_data=new unsigned char[6+length];
+ status_data[0]=0xFF;
+ status_data[1]=0xFF;
+ status_data[2]=id;
+ status_data[3]=length+2;
+ status_data[4]=error;
+ memcpy(&status_data[5],data,length);
+ status_data[5+length]=0x00;
+ status_data[5+length]=CDynamixelServer::compute_checksum_v1(status_data,this->get_packet_length(status_data));
+ if(this->comm_dev->write(status_data,this->get_packet_length(status_data))!=this->get_packet_length(status_data))
+ std::cout << "Error while reading the communication device" << std::endl;
+ this->comm_access.exit();
+ }
+ else
+ {
+ this->comm_access.enter();
+ status_data=new unsigned char[11+length];
+ status_data[0]=0xFF;
+ status_data[1]=0xFF;
+ status_data[2]=0xFD;
+ status_data[3]=0x00;
+ status_data[4]=id;
+ status_data[5]=(4+length)%256;
+ status_data[6]=(4+length)/256;
+ status_data[7]=0x55;
+ status_data[8]=error;
+ memcpy(&status_data[9],data,length);
+ status_data[9+length]=0x00;
+ status_data[10+length]=0x00;
+ checksum=CDynamixelServer::compute_checksum_v2(status_data,this->get_packet_length(status_data)-2);
+ status_data[9+length]=checksum%256;
+ status_data[10+length]=checksum/256;
+ if(this->comm_dev->write(status_data,this->get_packet_length(status_data))!=this->get_packet_length(status_data))
+ std::cout << "Error while reading the communication device" << std::endl;
+ this->comm_access.exit();
+ }
}
-/* sync read instruction */
-bool CDynamixelSlave::sync_read_id_present(unsigned char id,unsigned short int *address,unsigned short int *length)
+void CDynamixelSlave::add_slave(unsigned char id, on_ping_fnct on_ping, on_rw_fnct on_read, on_rw_fnct on_write)
{
+ unsigned int i=0;
+ TSlave new_slave;
+ for(i=0;i<this->slaves.size();i++)
+ {
+ if(id==this->slaves[i].id)
+ throw CDynamixelSlaveException(_HERE_,"The desired slave device is already present");
+ }
+ new_slave.id=id;
+ new_slave.on_ping=on_ping;
+ new_slave.on_read=on_read;
+ new_slave.on_write=on_write;
+ new_slave.reg_address=-1;
+ new_slave.reg_length=-1;
+ new_slave.reg_data=NULL;
+ new_slave.bulk_prev_id=-1;
+ new_slave.bulk_address=-1;
+ new_slave.bulk_length=-1;
+ new_slave.bulk_read_pending=-1;
+ new_slave.sync_prev_id=-1;
+ new_slave.sync_address=-1;
+ new_slave.sync_length=-1;
+ new_slave.sync_read_pending=-1;
+ this->slaves.push_back(new_slave);
}
-/* bulk read instruction */
-bool CDynamixelSlave::bulk_read_id_present(unsigned char id,std::vector<unsigned short int> &address,std::vector<unsigned short int> &length)
+void CDynamixelSlave::delete_slave(unsigned char id)
{
+ unsigned int i=0;
+ for(i=0;i<this->slaves.size();i++)
+ {
+ if(id==this->slaves[i].id)
+ {
+ if(this->slaves[i].reg_data!=NULL)
+ {
+ delete[] this->slaves[i].reg_data;
+ this->slaves[i].reg_data=NULL;
+ }
+ this->slaves.erase(this->slaves.begin()+i);
+ return;
+ }
+ }
+ throw CDynamixelSlaveException(_HERE_,"The desired device is not present to be removed");
}
-/* bulk write instruction */
-bool CDynamixelSlave::bulk_write_id_present(unsigned char id,std::vector<unsigned short int> &address,std::vector<unsigned short int> &length,std::vector<unsigned char> &data)
+unsigned int CDynamixelSlave::get_num_slaves(void)
{
-
+ return this->slaves.size();
}
-// status return
-void CDynamixelSlave::send_status_packet(unsigned char id,TDynError error,unsigned short int length,std::vector<unsigned char> &data)
+unsigned int CDynamixelSlave::get_return_delay(void)
{
+ return this->return_delay;
+}
+void CDynamixelSlave::set_return_delay(unsigned int delay_us)
+{
+ this->return_delay=delay_us;
}
-void CDynamixelSlave::next_packet(void)
+return_level_t CDynamixelSlave::get_return_level(void)
{
+ return this->return_level;
+}
+void CDynamixelSlave::get_return_level(return_level_t level)
+{
+ this->return_level=level;
}
+
CDynamixelSlave::~CDynamixelSlave()
{
this->thread_server->detach_thread(this->process_packets_thread_id);
this->thread_server->delete_thread(this->process_packets_thread_id);
this->process_packets_thread_id="";
+ this->thread_server->detach_thread(this->dynamixel_loop_thread_id);
+ this->thread_server->delete_thread(this->dynamixel_loop_thread_id);
+ this->dynamixel_loop_thread_id="";
+
this->event_server->delete_event(this->finish_thread_event_id);
this->finish_thread_event_id="";
this->event_server->delete_event(this->new_packet_available_event_id);
diff --git a/src/dynamixel_slave.h b/src/dynamixel_slave.h
index 042a28c97b915c0a0068b293879814890f52a235..68c580e645b8e45647717230aea0617e40924635 100644
--- a/src/dynamixel_slave.h
+++ b/src/dynamixel_slave.h
@@ -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
*
diff --git a/src/dynamixelexceptions.cpp b/src/dynamixelexceptions.cpp
index d7bebd8ccccd3288568e21866973ddbb231dd5a4..a0832e59a1ec884cb5af30aa4be96f4d2fb1a5bf 100644
--- a/src/dynamixelexceptions.cpp
+++ b/src/dynamixelexceptions.cpp
@@ -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;
+}
+
diff --git a/src/dynamixelexceptions.h b/src/dynamixelexceptions.h
index 90cb4b5740dec469badb0d8b36a47fa2e8d8a4e1..498c6338a62035d079fbae8877c734c8617a798c 100644
--- a/src/dynamixelexceptions.h
+++ b/src/dynamixelexceptions.h
@@ -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
diff --git a/src/examples/test_dynamixel_server.cpp b/src/examples/test_dynamixel_server.cpp
index 8e3ed0f5a3e0e65793a7c54e51fc0b3910afcb24..0ca19b63812ad95456ee6e9f8604b518680557cd 100644
--- a/src/examples/test_dynamixel_server.cpp
+++ b/src/examples/test_dynamixel_server.cpp
@@ -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)
diff --git a/src/examples/test_dynamixel_slave.cpp b/src/examples/test_dynamixel_slave.cpp
index 1f269e340055acf536707fa8cfa4944464b1500a..3360f368be31db2ac1ec1980c95a0f673b2ef0e5 100644
--- a/src/examples/test_dynamixel_slave.cpp
+++ b/src/examples/test_dynamixel_slave.cpp
@@ -1,14 +1,44 @@
#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;
}