diff --git a/src/dynamixelserver.cpp b/src/dynamixelserver.cpp
index 94839ebcec0f67ea9e0ff4669a618a0fc194b02e..f7ba1a01b2a45e3eb9f130171720e2427f4d3d16 100644
--- a/src/dynamixelserver.cpp
+++ b/src/dynamixelserver.cpp
@@ -86,7 +86,7 @@ CDynamixelServer *CDynamixelServer::instance(void)
void CDynamixelServer::send_instruction_packet_v1(dyn_inst_t inst,unsigned char *data,unsigned char len,unsigned char id)
{
- unsigned char *packet;
+ unsigned char *packet=NULL;
int i,length;
switch(inst)
@@ -99,7 +99,7 @@ void CDynamixelServer::send_instruction_packet_v1(dyn_inst_t inst,unsigned char
break;
case dyn_sync_write: id=0xFE;
length=6+len;
- data=new unsigned char[length];
+ packet=new unsigned char[length];
break;
default: throw CDynamixelServerException(_HERE_,"Instruction not supported");
break;
@@ -120,13 +120,19 @@ void CDynamixelServer::send_instruction_packet_v1(dyn_inst_t inst,unsigned char
if(this->comm_dev->write(packet,length)!=length)
{
/* handle exceptions */
+ if(packet!=NULL)
+ delete[] packet;
this->dynamixel_access.exit();
throw CDynamixelServerException(_HERE_,"Unexpected error while writing to the communication device");
}
+ if(packet!=NULL)
+ delete[] packet;
this->dynamixel_access.exit();
}
else
{
+ if(packet!=NULL)
+ delete[] packet;
/* handle exceptions */
throw CDynamixelServerException(_HERE_,"The communication device is not properly configured");
}
@@ -134,7 +140,7 @@ void CDynamixelServer::send_instruction_packet_v1(dyn_inst_t inst,unsigned char
void CDynamixelServer::send_instruction_packet_v2(dyn_inst_t inst,unsigned char *data,unsigned short int len,unsigned char id)
{
- unsigned char *packet;
+ unsigned char *packet=NULL;
int i,length,crc;
switch(inst)
@@ -145,9 +151,12 @@ void CDynamixelServer::send_instruction_packet_v2(dyn_inst_t inst,unsigned char
if(len!=0)
throw CDynamixelServerException(_HERE_,"Invalid data length");
break;
+ case dyn_bulk_read:
+ case dyn_bulk_write:
+ case dyn_sync_read:
case dyn_sync_write: id=0xFE;
length=10+len;
- data=new unsigned char[length];
+ packet=new unsigned char[length];
break;
default: throw CDynamixelServerException(_HERE_,"Instruction not supported");
break;
@@ -173,13 +182,19 @@ void CDynamixelServer::send_instruction_packet_v2(dyn_inst_t inst,unsigned char
if(this->comm_dev->write(packet,length)!=length)
{
/* handle exceptions */
+ if(packet!=NULL)
+ delete[] packet;
this->dynamixel_access.exit();
throw CDynamixelServerException(_HERE_,"Unexpected error while writing to the communication device");
}
+ if(packet!=NULL)
+ delete[] packet;
this->dynamixel_access.exit();
}
else
{
+ if(packet!=NULL)
+ delete[] packet;
/* handle exceptions */
throw CDynamixelServerException(_HERE_,"The communication device is not properly configured");
}
@@ -1066,7 +1081,7 @@ void CDynamixelServer::close(void)
void CDynamixelServer::write_sync(std::vector<unsigned char>& servo_ids,unsigned short int start_addr, std::vector< std::vector<unsigned char> >& data,dyn_version_t version)
{
- unsigned char *data_int;
+ unsigned char *data_int=NULL;
unsigned int i,j=0;
int length=0;
@@ -1085,41 +1100,52 @@ void CDynamixelServer::write_sync(std::vector<unsigned char>& servo_ids,unsigned
/* handle exception */
throw CDynamixelServerException(_HERE_,"Too much data blocks or some missing servos");
}
- if(version==dyn_version1)
- {
- length=2+(data[0].size()+1)*servo_ids.size();
- data_int=new unsigned char[length];
- data_int[0]=(unsigned char)start_addr;
- data_int[1]=data[0].size();
- for(i=0;i<servo_ids.size();i++)
+ try{
+ if(version==dyn_version1)
{
- data_int[2+(data[i].size()+1)*i]= servo_ids[i];
- for(j=0;j<data[i].size();j++)
- data_int[2+(data[i].size()+1)*i+1+j]=data[i][j];
+ length=2+(data[0].size()+1)*servo_ids.size();
+ data_int=new unsigned char[length];
+ data_int[0]=(unsigned char)start_addr;
+ data_int[1]=data[0].size();
+ for(i=0;i<servo_ids.size();i++)
+ {
+ data_int[2+(data[i].size()+1)*i]= servo_ids[i];
+ for(j=0;j<data[i].size();j++)
+ data_int[2+(data[i].size()+1)*i+1+j]=data[i][j];
+ }
+ this->send_instruction_packet_v1(dyn_sync_write,data_int,length);
+ delete[] data_int;
}
- this->send_instruction_packet_v1(dyn_sync_write,data_int,length);
- }
- else
- {
- length=4+(data[0].size()+1)*servo_ids.size();
- data_int=new unsigned char[length];
- data_int[0]=start_addr%256;
- data_int[1]=start_addr/256;
- data_int[2]=data[0].size()%256;
- data_int[3]=data[0].size()/256;
- for(i=0;i<servo_ids.size();i++)
+ else
+ {
+ length=4+(data[0].size()+1)*servo_ids.size();
+ data_int=new unsigned char[length];
+ data_int[0]=start_addr%256;
+ data_int[1]=start_addr/256;
+ data_int[2]=data[0].size()%256;
+ data_int[3]=data[0].size()/256;
+ for(i=0;i<servo_ids.size();i++)
+ {
+ data_int[4+(data[i].size()+1)*i]= servo_ids[i];
+ for(j=0;j<data[i].size();j++)
+ data_int[4+(data[i].size()+1)*i+1+j]=data[i][j];
+ }
+ this->send_instruction_packet_v2(dyn_sync_write,data_int,length);
+ delete[] data_int;
+ }
+ }catch(CException &e){
+ if(data_int!=NULL)
{
- data_int[4+(data[i].size()+1)*i]= servo_ids[i];
- for(j=0;j<data[i].size();j++)
- data_int[4+(data[i].size()+1)*i+1+j]=data[i][j];
+ delete[] data_int;
+ data_int=NULL;
}
- this->send_instruction_packet_v2(dyn_sync_write,data_int,length);
+ throw e;
}
}
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,*data_out,error;
+ unsigned char *data_int=NULL,*data_out=NULL,error;
unsigned short length_v2;
unsigned int i,j=0;
int length_int=0;
@@ -1136,41 +1162,60 @@ void CDynamixelServer::read_sync(std::vector<unsigned char>& servo_ids,unsigned
}
else
{
- length_int=4+servo_ids.size();
- data_int=new unsigned char[length_int];
- data_int[0]=start_addr%256;
- data_int[1]=start_addr/256;
- data_int[2]=length%256;
- data_int[3]=length/256;
- for(i=0;i<servo_ids.size();i++)
- data_int[4+i]=servo_ids[i];
- this->send_instruction_packet_v2(dyn_sync_read,data_int,length_int);
- /* read all the data from all the servos */
- 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;
- }
- }catch(CException &e){
- if(data_out!=NULL)
- delete[] data_out;
+ try{
+ length_int=4+servo_ids.size();
+ data_int=new unsigned char[length_int];
+ data_int[0]=start_addr%256;
+ data_int[1]=start_addr/256;
+ data_int[2]=length%256;
+ data_int[3]=length/256;
+ for(i=0;i<servo_ids.size();i++)
+ data_int[4+i]=servo_ids[i];
+ 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;
+ }
+ }
+ }catch(CException &e){
+ if(data_int!=NULL)
+ {
+ delete[] data_int;
+ data_out=NULL;
}
+ throw e;
}
}
}
void CDynamixelServer::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)
{
- unsigned char *data_int;
- unsigned int i;
+ unsigned char *data_int=NULL;
+ unsigned int i,j;
int length=0,offset=0;
if(servo_ids.size()==0)
@@ -1195,26 +1240,38 @@ void CDynamixelServer::write_bulk(std::vector<unsigned char>& servo_ids,std::vec
}
else
{
- length=0;
- for(i=0;i<servo_ids.size();i++)
- length+=5+data[i].size();
- data_int=new unsigned char[length];
- for(i=0;i<servo_ids.size();i++)
- {
- data_int[offset]=servo_ids[i];
- data_int[offset+1]=start_addr[i]%256;
- data_int[offset+2]=start_addr[i]/256;
- data_int[offset+3]=data[i].size()%256;
- data_int[offset+4]=data[i].size()/256;
- offset+=5+data[i].size();
+ try{
+ length=0;
+ for(i=0;i<servo_ids.size();i++)
+ length+=5+data[i].size();
+ data_int=new unsigned char[length];
+ for(i=0;i<servo_ids.size();i++)
+ {
+ data_int[offset]=servo_ids[i];
+ data_int[offset+1]=start_addr[i]%256;
+ data_int[offset+2]=start_addr[i]/256;
+ data_int[offset+3]=data[i].size()%256;
+ data_int[offset+4]=data[i].size()/256;
+ for(j=0;j<data[i].size();j++)
+ data_int[offset+5+j]=data[i][j];
+ offset+=5+data[i].size();
+ }
+ this->send_instruction_packet_v2(dyn_bulk_write,data_int,length);
+ delete[] data_int;
+ }catch(CException &e){
+ if(data_int!=NULL)
+ {
+ delete[] data_int;
+ data_int=NULL;
+ }
+ throw e;
}
- this->send_instruction_packet_v2(dyn_sync_read,data_int,length);
}
}
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,*data_out,error;
+ unsigned char *data_int=NULL,*data_out=NULL,error;
unsigned short int length_v2;
unsigned int i,j=0;
int length_int=0;
@@ -1241,35 +1298,54 @@ void CDynamixelServer::read_bulk(std::vector<unsigned char>& servo_ids,std::vect
}
else
{
- length_int=5*servo_ids.size();
- data_int=new unsigned char[length_int];
- for(i=0;i<servo_ids.size();i++)
- {
- data_int[i*5]=servo_ids[i];
- data_int[i*5+1]=start_addr[i]%256;
- data_int[i*5+2]=start_addr[i]/256;
- data_int[i*5+3]=length[i]%256;
- data_int[i*5+4]=length[i]/256;
- }
- this->send_instruction_packet_v2(dyn_sync_read,data_int,length_int);
- /* read all the data from all the servos */
- 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;
+ try{
+ length_int=5*servo_ids.size();
+ data_int=new unsigned char[length_int];
+ for(i=0;i<servo_ids.size();i++)
+ {
+ data_int[i*5]=servo_ids[i];
+ data_int[i*5+1]=start_addr[i]%256;
+ data_int[i*5+2]=start_addr[i]/256;
+ data_int[i*5+3]=length[i]%256;
+ data_int[i*5+4]=length[i]/256;
+ }
+ 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;
}
- }catch(CException &e){
- if(data_out!=NULL)
- delete[] data_out;
}
+ }catch(CException &e){
+ if(data_int!=NULL)
+ {
+ delete[] data_int;
+ data_int=NULL;
+ }
+ throw e;
}
}
}
diff --git a/src/examples/test_dynamixel_server.cpp b/src/examples/test_dynamixel_server.cpp
index 12e416f7a70903d22e5c1c3cc84a66a7399342d2..e9be11137aeab6232b8296a4684bea444c46aaec 100644
--- a/src/examples/test_dynamixel_server.cpp
+++ b/src/examples/test_dynamixel_server.cpp
@@ -6,21 +6,25 @@ 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,event_id;
+ int num_buses=0,num_dev=0,baudrate=0,i=0,event_id,j;
std::list<std::string> events;
std::vector<float> position;
std::vector<int> devices;
unsigned short int model;
unsigned char margin;
CDynamixel *dyn_motor;
+ std::vector<unsigned char> servo_ids;
+ std::vector< std::vector<unsigned char> > data;
+ std::vector<unsigned short int> address,length;
num_buses=dyn_server->get_num_buses();
std::cout << "Num. buses: " << num_buses << std::endl;
if(num_buses>0)
{
+ 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(0,1000000);
dyn_server->start_scan();
event_id=event_server->wait_first(events);
if(event_id==0)
@@ -43,17 +47,38 @@ int main(int argc, char *argv[])
std::cout << "Device " << std::dec << i << ": id -> " << devices[i] << " model: " << std::hex << model << " margin: " << (int)margin << std::endl;
dyn_server->free_device(devices[i]);
}
+ // sync write operation
+ std::cout << "sync write" << std::endl;
+ data.resize(num_dev);
+ servo_ids.resize(num_dev);
+ for(i=0;i<num_dev;i++)
+ {
+ servo_ids[i]=devices[i];
+ data[i].clear();
+ data[i].push_back(0xFF);
+ data[i].push_back(0x03);
+ }
+ dyn_server->write_sync(servo_ids,0x1E,data,dyn_version1);
+ sleep(2);
+ for(i=0;i<num_dev;i++)
+ {
+ servo_ids[i]=devices[i];
+ data[i].clear();
+ data[i].push_back(0x00);
+ data[i].push_back(0x00);
+ }
+ dyn_server->write_sync(servo_ids,0x1E,data,dyn_version1);
}
else
- std::cout << "Error while scanning the bus: " << dyn_server->get_scan_error() << std::endl;
- try{
+ 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();
event_server->wait_first(events,2000);
}catch(CException &e){
dyn_server->stop_scan();
std::cout << "Scanning canceled !!!" << std::endl;
- }
+ }*/
/* scan for version 2 devices */
dyn_server->config_bus(0,1000000);
dyn_server->start_scan(dyn_version2);
@@ -71,15 +96,52 @@ int main(int argc, char *argv[])
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);
+ //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);
}
+ /* sync read operation */
+ servo_ids.resize(num_dev);
+ for(i=0;i<num_dev;i++)
+ servo_ids[i]=devices[i];
+ std::cout << "sync read" << std::endl;
+ dyn_server->read_sync(servo_ids,0x1B,3,data,dyn_version2);
+ for(i=0;i<data.size();i++)
+ {
+ std::cout << "Servo " << (int)servo_ids[i] << " PID" << std::endl;
+ for(j=0;j<data[i].size();j++)
+ std::cout << std::hex << "\t" << (int)data[i][j] << std::endl;
+ }
+ /* bulk write */
+ address.resize(num_dev);
+ length.resize(num_dev);
+ for(i=0;i<num_dev;i++)
+ {
+ address[i]=0x1B;
+ data[i][0]=0xFF;
+ data[i][1]=0xFF;
+ data[i][2]=0xFD;
+ length[i]=3;
+ }
+ std::cout << "bulk write" << std::endl;
+ dyn_server->write_bulk(servo_ids,address,data,dyn_version2);
+ std::cout << "bulk read" << std::endl;
+ data.clear();
+ dyn_server->read_bulk(servo_ids,address,length,data,dyn_version2);
+ for(i=0;i<data.size();i++)
+ {
+ std::cout << "Servo " << (int)servo_ids[i] << " PID" << std::endl;
+ for(j=0;j<data[i].size();j++)
+ std::cout << std::hex << "\t" << (int)data[i][j] << std::endl;
+ }
}
else
std::cout << "Error while scanning the bus: " << dyn_server->get_scan_error() << std::endl;
+ }catch(CException &e){
+ std::cout << e.what() << std::endl;
+ }
}
}