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

Added functions to receive all the answers from bulk and sync read commands.

parent 6db8f4f5
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
include/dynamixelserver.h
+ 10
− 0
View file @ 57cc9583
......@@ -106,11 +106,21 @@ class CDynamixelServer
*
*/
virtual unsigned char receive_status_packet_v1(unsigned char **data,unsigned char *len);
/**
* \brief
*
*/
virtual void receive_sync_bulk_status_packets_v1(std::vector<unsigned char>& servo_ids, std::vector< std::vector<unsigned char> >& data);
/**
* \brief
*
*/
virtual unsigned char receive_status_packet_v2(unsigned char **data,unsigned short int *len);
/**
* \brief
*
*/
virtual void receive_sync_bulk_status_packets_v2(std::vector<unsigned char>& servo_ids, std::vector< std::vector<unsigned char> >& data);
/**
* \brief
*
......
src/dynamixelserver.cpp
+ 205
− 142
View file @ 57cc9583
......@@ -2,6 +2,7 @@
#include "dynamixelserver.h"
#include "eventexceptions.h"
#include <sstream>
#include <iostream>
const unsigned short crc_table[256] = {
0x0000, 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011,
......@@ -194,7 +195,7 @@ unsigned char CDynamixelServer::receive_status_packet_v1(unsigned char **data,un
do{
if((num=this->comm_dev->get_num_data())==0)
{
this->event_server->wait_all(events,20);
this->event_server->wait_all(events,100);
num=this->comm_dev->get_num_data();
}
if((read+num)>1024)
......@@ -262,6 +263,92 @@ unsigned char CDynamixelServer::receive_status_packet_v1(unsigned char **data,un
}
}
void CDynamixelServer::receive_sync_bulk_status_packets_v1(std::vector<unsigned char>& servo_ids, std::vector< std::vector<unsigned char> >& data)
{
std::list<std::string> events;
unsigned char data_int[1024];
int num=0,read=0,length=-1,new_data,total=0;
bool need_data=true;
if(this->comm_dev!=NULL)
{
try{
data.resize(servo_ids.size());
events.push_back(this->comm_dev->get_rx_event_id());
// read up to the length field
for(unsigned int i=0;i<servo_ids.size();)
{
if(need_data)
{
this->event_server->wait_all(events,100);
new_data=this->comm_dev->get_num_data();
this->comm_dev->read(&data_int[total],new_data);
total+=new_data;
num+=new_data;
need_data=false;
}
if(length==-1)
{
if(num>=4)
{
if(data_int[read+2]!=servo_ids[i])
throw CDynamixelServerException(_HERE_,"Invalid status packet order in Sync/Bulk read");
length=data_int[read+3]+4;
}
else
need_data=true;
}
if(length!=-1 && num>=length)
{
if(this->compute_checksum_v1(&data_int[read],length)!=0x00)
{
data[i].clear();
read+=length;
num-=length;
length=-1;
i++;
continue;
}
try{
this->handle_error(data_int[read+4]);
}catch(CException &e){
data[i].clear();
read+=length;
num-=length;
length=-1;
i++;
continue;
}
if(length>6)
{
data[i].resize(length-6);
for(unsigned j=0;j<(unsigned int)(length-6);j++)
data[i][j]=data_int[read+5+j];
}
else
data[i].clear();
read+=length;
num-=length;
length=-1;
i++;
}
else
need_data=true;
}
}catch(CEventTimeoutException &e){
throw e;
}catch(CException &e){
throw e;
}
}
else
{
/* handle exceptions */
throw CDynamixelServerException(_HERE_,"The communication device is not properly configured");
}
}
unsigned char CDynamixelServer::receive_status_packet_v2(unsigned char **data,unsigned short int *len)
{
std::list<std::string> events;
......@@ -277,7 +364,7 @@ unsigned char CDynamixelServer::receive_status_packet_v2(unsigned char **data,un
do{
if((num=this->comm_dev->get_num_data())==0)
{
this->event_server->wait_all(events,20);
this->event_server->wait_all(events,100);
num=this->comm_dev->get_num_data();
}
if((read+num)>1024)
......@@ -298,7 +385,7 @@ unsigned char CDynamixelServer::receive_status_packet_v2(unsigned char **data,un
{
if((num=this->comm_dev->get_num_data())==0)
{
this->event_server->wait_all(events,20);
this->event_server->wait_all(events,100);
num=this->comm_dev->get_num_data();
}
if((read-start+num)>length)
......@@ -345,6 +432,95 @@ unsigned char CDynamixelServer::receive_status_packet_v2(unsigned char **data,un
}
}
void CDynamixelServer::receive_sync_bulk_status_packets_v2(std::vector<unsigned char> &servo_ids,std::vector< std::vector<unsigned char> > &data)
{
std::list<std::string> events;
unsigned char data_int[1024];
unsigned short int crc;
int num=0,read=0,length=-1,new_data,total=0;
bool need_data=true;
if(this->comm_dev!=NULL)
{
try{
data.resize(servo_ids.size());
events.push_back(this->comm_dev->get_rx_event_id());
// read up to the length field
for(unsigned int i=0;i<servo_ids.size();)
{
if(need_data)
{
this->event_server->wait_all(events,100);
new_data=this->comm_dev->get_num_data();
this->comm_dev->read(&data_int[total],new_data);
total+=new_data;
num+=new_data;
need_data=false;
}
if(length==-1)
{
if(num>=7)
{
if(data_int[read+2]!=servo_ids[i])
throw CDynamixelServerException(_HERE_,"Invalid status packet order in Sync/Bulk read");
length=data_int[read+5]+data_int[read+6]*256+7;
}
else
need_data=true;
}
if(length!=-1 && num>=length)
{
// check the checksum
crc=this->compute_checksum_v2(&data_int[read],length-2);
if((crc%256)!=data_int[read+length-2] || (crc/256)!=data_int[read+length-1])
{
/* handle exceptions */
data[i].clear();
read+=length;
num-=length;
length=-1;
i++;
continue;
}
try{
this->handle_error(data_int[read+8]);
}catch(CException &e){
data[i].clear();
read+=length;
num-=length;
length=-1;
i++;
continue;
}
if(length>11)
{
data[i].resize(length-11);
for(unsigned j=0;j<(unsigned int)(length-11);j++)
data[i][j]=data_int[read+9+j];
}
else
data[i].clear();
read+=length;
num-=length;
length=-1;
i++;
}
else
need_data=true;
}
}catch(CEventTimeoutException &e){
throw e;
}catch(CException &e){
throw e;
}
}
else
{
/* handle exceptions */
throw CDynamixelServerException(_HERE_,"The communication device is not properly configured");
}
}
unsigned char CDynamixelServer::compute_checksum_v1(unsigned char *packet,int len)
{
int i=0;
......@@ -895,6 +1071,7 @@ void CDynamixelServer::write_sync(std::vector<unsigned char>& servo_ids,unsigned
throw CDynamixelServerException(_HERE_,"Too much data blocks or some missing servos");
}
try{
this->dynamixel_access.enter();
if(version==dyn_version1)
{
length=2+(data[0].size()+1)*servo_ids.size();
......@@ -909,9 +1086,7 @@ void CDynamixelServer::write_sync(std::vector<unsigned char>& servo_ids,unsigned
}
try
{
this->dynamixel_access.enter();
this->send_instruction_packet_v1(dyn_sync_write,data_int,length);
this->dynamixel_access.exit();
}
catch(CEventTimeoutException &e)
{
......@@ -941,9 +1116,7 @@ void CDynamixelServer::write_sync(std::vector<unsigned char>& servo_ids,unsigned
}
try
{
this->dynamixel_access.enter();
this->send_instruction_packet_v2(dyn_sync_write,data_int,length);
this->dynamixel_access.exit();
}
catch(CEventTimeoutException &e)
{
......@@ -957,6 +1130,7 @@ void CDynamixelServer::write_sync(std::vector<unsigned char>& servo_ids,unsigned
}
delete[] data_int;
}
this->dynamixel_access.exit();
}catch(CException &e){
if(data_int!=NULL)
{
......@@ -969,9 +1143,8 @@ void CDynamixelServer::write_sync(std::vector<unsigned char>& servo_ids,unsigned
void CDynamixelServer::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)
{
unsigned char *data_int=NULL,*data_out=NULL,error,length_v1;
unsigned short length_v2;
unsigned int i,j=0;
unsigned char *data_int=NULL;
unsigned int i;
int length_int=0;
if(servo_ids.size()==0)
......@@ -980,6 +1153,7 @@ void CDynamixelServer::read_sync(std::vector<unsigned char>& servo_ids,unsigned
throw CDynamixelServerException(_HERE_,"No servos specified");
}
try{
this->dynamixel_access.enter();
if(version==dyn_version2)
{
length_int=4+servo_ids.size();
......@@ -992,38 +1166,9 @@ void CDynamixelServer::read_sync(std::vector<unsigned char>& servo_ids,unsigned
data_int[4+i]=servo_ids[i];
try
{
this->dynamixel_access.enter();
this->send_instruction_packet_v2(dyn_sync_read,data_int,length_int);
/* read all the data from all the servos */
if(data_int!=NULL)
{
delete[] data_int;
data_int=NULL;
}
data.resize(servo_ids.size());
for(i=0;i<servo_ids.size();i++)
{
try{
error=this->receive_status_packet_v2(&data_out,&length_v2);
this->handle_error(error);
if(data_out!=NULL)
{
data[i].resize(length_v2);
for(j=0;j<length_v2;j++)
data[i][j]=data_out[j];
delete[] data_out;
data_out=NULL;
}
}catch(CException &e){
if(data_out!=NULL)
{
delete[] data_out;
data_out=NULL;
}
throw e;
}
}
this->dynamixel_access.exit();
receive_sync_bulk_status_packets_v2(servo_ids,data);
}
catch(CEventTimeoutException &e)
{
......@@ -1035,6 +1180,7 @@ void CDynamixelServer::read_sync(std::vector<unsigned char>& servo_ids,unsigned
this->dynamixel_access.exit();
throw e;
}
delete[] data_int;
}
else
{
......@@ -1046,38 +1192,9 @@ void CDynamixelServer::read_sync(std::vector<unsigned char>& servo_ids,unsigned
data_int[2+i]=servo_ids[i];
try
{
this->dynamixel_access.enter();
this->send_instruction_packet_v1(dyn_sync_read,data_int,length_int);
/* read all the data from all the servos */
if(data_int!=NULL)
{
delete[] data_int;
data_int=NULL;
}
data.resize(servo_ids.size());
for(i=0;i<servo_ids.size();i++)
{
try{
error=this->receive_status_packet_v1(&data_out,&length_v1);
this->handle_error(error);
if(data_out!=NULL)
{
data[i].resize(length_v1);
for(j=0;j<length_v1;j++)
data[i][j]=data_out[j];
delete[] data_out;
data_out=NULL;
}
}catch(CException &e){
if(data_out!=NULL)
{
delete[] data_out;
data_out=NULL;
}
throw e;
}
}
this->dynamixel_access.exit();
receive_sync_bulk_status_packets_v1(servo_ids,data);
}
catch(CEventTimeoutException &e)
{
......@@ -1089,12 +1206,14 @@ void CDynamixelServer::read_sync(std::vector<unsigned char>& servo_ids,unsigned
this->dynamixel_access.exit();
throw e;
}
delete[] data_int;
}
this->dynamixel_access.exit();
}catch(CException &e){
if(data_int!=NULL)
{
delete[] data_int;
data_out=NULL;
data_int=NULL;
}
throw e;
}
......@@ -1122,6 +1241,7 @@ void CDynamixelServer::write_bulk(std::vector<unsigned char>& servo_ids,std::vec
throw CDynamixelServerException(_HERE_,"Too many or not enough data blocks");
}
try{
this->dynamixel_access.enter();
if(version==dyn_version2)
{
length=0;
......@@ -1141,9 +1261,7 @@ void CDynamixelServer::write_bulk(std::vector<unsigned char>& servo_ids,std::vec
}
try
{
this->dynamixel_access.enter();
this->send_instruction_packet_v2(dyn_bulk_write,data_int,length);
this->dynamixel_access.exit();
}
catch(CEventTimeoutException &e)
{
......@@ -1174,9 +1292,7 @@ void CDynamixelServer::write_bulk(std::vector<unsigned char>& servo_ids,std::vec
}
try
{
this->dynamixel_access.enter();
this->send_instruction_packet_v1(dyn_bulk_write,data_int,length);
this->dynamixel_access.exit();
}
catch(CEventTimeoutException &e)
{
......@@ -1190,6 +1306,7 @@ void CDynamixelServer::write_bulk(std::vector<unsigned char>& servo_ids,std::vec
}
delete[] data_int;
}
this->dynamixel_access.exit();
}catch(CException &e){
if(data_int!=NULL)
{
......@@ -1202,9 +1319,8 @@ void CDynamixelServer::write_bulk(std::vector<unsigned char>& servo_ids,std::vec
void CDynamixelServer::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)
{
unsigned char *data_int=NULL,*data_out=NULL,error,length_v1;
unsigned short int length_v2;
unsigned int i,j=0;
unsigned char *data_int=NULL;
unsigned int i;
int length_int=0;
if(servo_ids.size()==0)
......@@ -1223,6 +1339,7 @@ void CDynamixelServer::read_bulk(std::vector<unsigned char>& servo_ids,std::vect
throw CDynamixelServerException(_HERE_,"Too many or not enough read lengths");
}
try{
this->dynamixel_access.enter();
if(version==dyn_version2)
{
length_int=5*servo_ids.size();
......@@ -1237,38 +1354,9 @@ void CDynamixelServer::read_bulk(std::vector<unsigned char>& servo_ids,std::vect
}
try
{
this->dynamixel_access.enter();
this->send_instruction_packet_v2(dyn_bulk_read,data_int,length_int);
/* read all the data from all the servos */
if(data_int!=NULL)
{
delete[] data_int;
data_int=NULL;
}
data.resize(servo_ids.size());
for(i=0;i<servo_ids.size();i++)
{
error=this->receive_status_packet_v2(&data_out,&length_v2);
try{
this->handle_error(error);
if(data_out!=NULL)
{
data[i].resize(length_v2);
for(j=0;j<length_v2;j++)
data[i][j]=data_out[j];
delete[] data_out;
data_out=NULL;
}
}catch(CException &e){
if(data_out!=NULL)
{
delete[] data_out;
data_out=NULL;
}
throw e;
}
}
this->dynamixel_access.exit();
receive_sync_bulk_status_packets_v2(servo_ids,data);
}
catch(CEventTimeoutException &e)
{
......@@ -1280,51 +1368,24 @@ void CDynamixelServer::read_bulk(std::vector<unsigned char>& servo_ids,std::vect
this->dynamixel_access.exit();
throw e;
}
delete[] data_int;
}
else
{
length_int=3*servo_ids.size();
length_int=3*servo_ids.size()+1;
data_int=new unsigned char[length_int];
data_int[0]=0x00;
for(i=0;i<servo_ids.size();i++)
{
data_int[i*3]=servo_ids[i];
data_int[i*3+1]=(unsigned char)start_addr[i];
data_int[i*3+2]=(unsigned char)length[i];
data_int[i*3+1]=(unsigned char)length[i];
data_int[i*3+2]=servo_ids[i];
data_int[i*3+3]=(unsigned char)start_addr[i];
}
try
{
this->dynamixel_access.enter();
this->send_instruction_packet_v1(dyn_bulk_read,data_int,length_int);
/* read all the data from all the servos */
if(data_int!=NULL)
{
delete[] data_int;
data_int=NULL;
}
data.resize(servo_ids.size());
for(i=0;i<servo_ids.size();i++)
{
error=this->receive_status_packet_v1(&data_out,&length_v1);
try{
this->handle_error(error);
if(data_out!=NULL)
{
data[i].resize(length_v1);
for(j=0;j<length_v1;j++)
data[i][j]=data_out[j];
delete[] data_out;
data_out=NULL;
}
}catch(CException &e){
if(data_out!=NULL)
{
delete[] data_out;
data_out=NULL;
}
throw e;
}
}
this->dynamixel_access.exit();
receive_sync_bulk_status_packets_v1(servo_ids,data);
}
catch(CEventTimeoutException &e)
{
......@@ -1336,7 +1397,9 @@ void CDynamixelServer::read_bulk(std::vector<unsigned char>& servo_ids,std::vect
this->dynamixel_access.exit();
throw e;
}
delete[] data_int;
}
this->dynamixel_access.exit();
}catch(CException &e){
if(data_int!=NULL)
{
......
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