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

Added the bulk_read, bulk_write and sync_read functions for the version 1 protocol.

Added a module to test the new firmware for AX12 servos.
parent 10e4164d
No related branches found
No related tags found
No related merge requests found
...@@ -73,6 +73,9 @@ void CDynamixelServer::send_instruction_packet_v1(dyn_inst_t inst,unsigned char ...@@ -73,6 +73,9 @@ void CDynamixelServer::send_instruction_packet_v1(dyn_inst_t inst,unsigned char
if(len!=0) if(len!=0)
throw CDynamixelServerException(_HERE_,"Invalid data length"); throw CDynamixelServerException(_HERE_,"Invalid data length");
break; break;
case dyn_bulk_read:
case dyn_bulk_write:
case dyn_sync_read:
case dyn_sync_write: id=0xFE; case dyn_sync_write: id=0xFE;
length=6+len; length=6+len;
packet=new unsigned char[length]; packet=new unsigned char[length];
...@@ -995,7 +998,7 @@ void CDynamixelServer::write_sync(std::vector<unsigned char>& servo_ids,unsigned ...@@ -995,7 +998,7 @@ 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) 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; unsigned char *data_int=NULL,*data_out=NULL,error,length_v1;
unsigned short length_v2; unsigned short length_v2;
unsigned int i,j=0; unsigned int i,j=0;
int length_int=0; int length_int=0;
...@@ -1005,14 +1008,9 @@ void CDynamixelServer::read_sync(std::vector<unsigned char>& servo_ids,unsigned ...@@ -1005,14 +1008,9 @@ void CDynamixelServer::read_sync(std::vector<unsigned char>& servo_ids,unsigned
/* handle exceptions */ /* handle exceptions */
throw CDynamixelServerException(_HERE_,"No servos specified"); throw CDynamixelServerException(_HERE_,"No servos specified");
} }
if(version==dyn_version1) try{
{ if(version==dyn_version2)
/* handle exception */ {
throw CDynamixelServerException(_HERE_,"Operation not supported in version 1");
}
else
{
try{
length_int=4+servo_ids.size(); length_int=4+servo_ids.size();
data_int=new unsigned char[length_int]; data_int=new unsigned char[length_int];
data_int[0]=start_addr%256; data_int[0]=start_addr%256;
...@@ -1051,14 +1049,53 @@ void CDynamixelServer::read_sync(std::vector<unsigned char>& servo_ids,unsigned ...@@ -1051,14 +1049,53 @@ void CDynamixelServer::read_sync(std::vector<unsigned char>& servo_ids,unsigned
throw e; throw e;
} }
} }
}catch(CException &e){ }
else
{
length_int=2+servo_ids.size();
data_int=new unsigned char[length_int];
data_int[0]=(unsigned char)start_addr;
data_int[1]=(unsigned char)length;
for(i=0;i<servo_ids.size();i++)
data_int[2+i]=servo_ids[i];
this->send_instruction_packet_v1(dyn_sync_read,data_int,length_int);
/* read all the data from all the servos */
if(data_int!=NULL) if(data_int!=NULL)
{ {
delete[] data_int; delete[] data_int;
data_out=NULL; data_int=NULL;
} }
throw e; 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;
}
}
}
}catch(CException &e){
if(data_int!=NULL)
{
delete[] data_int;
data_out=NULL;
} }
throw e;
} }
} }
...@@ -1083,14 +1120,9 @@ void CDynamixelServer::write_bulk(std::vector<unsigned char>& servo_ids,std::vec ...@@ -1083,14 +1120,9 @@ void CDynamixelServer::write_bulk(std::vector<unsigned char>& servo_ids,std::vec
/* handle exceptions */ /* handle exceptions */
throw CDynamixelServerException(_HERE_,"Too many or not enough data blocks"); throw CDynamixelServerException(_HERE_,"Too many or not enough data blocks");
} }
if(version==dyn_version1) try{
{ if(version==dyn_version2)
/* handle exception */ {
throw CDynamixelServerException(_HERE_,"Operation not supported in version 1");
}
else
{
try{
length=0; length=0;
for(i=0;i<servo_ids.size();i++) for(i=0;i<servo_ids.size();i++)
length+=5+data[i].size(); length+=5+data[i].size();
...@@ -1108,20 +1140,38 @@ void CDynamixelServer::write_bulk(std::vector<unsigned char>& servo_ids,std::vec ...@@ -1108,20 +1140,38 @@ void CDynamixelServer::write_bulk(std::vector<unsigned char>& servo_ids,std::vec
} }
this->send_instruction_packet_v2(dyn_bulk_write,data_int,length); this->send_instruction_packet_v2(dyn_bulk_write,data_int,length);
delete[] data_int; delete[] data_int;
}catch(CException &e){ }
if(data_int!=NULL) else
{
length=0;
for(i=0;i<servo_ids.size();i++)
length+=3+data[i].size();
data_int=new unsigned char[length];
for(i=0;i<servo_ids.size();i++)
{ {
delete[] data_int; data_int[offset]=servo_ids[i];
data_int=NULL; data_int[offset+1]=(unsigned char)start_addr[i];
data_int[offset+2]=(unsigned char)data[i].size();
for(j=0;j<data[i].size();j++)
data_int[offset+3+j]=data[i][j];
offset+=3+data[i].size();
} }
throw e; this->send_instruction_packet_v1(dyn_bulk_write,data_int,length);
delete[] data_int;
}
}catch(CException &e){
if(data_int!=NULL)
{
delete[] data_int;
data_int=NULL;
} }
throw e;
} }
} }
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) 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; unsigned char *data_int=NULL,*data_out=NULL,error,length_v1;
unsigned short int length_v2; unsigned short int length_v2;
unsigned int i,j=0; unsigned int i,j=0;
int length_int=0; int length_int=0;
...@@ -1141,14 +1191,9 @@ void CDynamixelServer::read_bulk(std::vector<unsigned char>& servo_ids,std::vect ...@@ -1141,14 +1191,9 @@ void CDynamixelServer::read_bulk(std::vector<unsigned char>& servo_ids,std::vect
/* handle exceptions */ /* handle exceptions */
throw CDynamixelServerException(_HERE_,"Too many or not enough read lengths"); throw CDynamixelServerException(_HERE_,"Too many or not enough read lengths");
} }
if(version==dyn_version1) try{
{ if(version==dyn_version2)
/* handle exception */ {
throw CDynamixelServerException(_HERE_,"Operation not supported in version 1");
}
else
{
try{
length_int=5*servo_ids.size(); length_int=5*servo_ids.size();
data_int=new unsigned char[length_int]; data_int=new unsigned char[length_int];
for(i=0;i<servo_ids.size();i++) for(i=0;i<servo_ids.size();i++)
...@@ -1189,14 +1234,55 @@ void CDynamixelServer::read_bulk(std::vector<unsigned char>& servo_ids,std::vect ...@@ -1189,14 +1234,55 @@ void CDynamixelServer::read_bulk(std::vector<unsigned char>& servo_ids,std::vect
throw e; throw e;
} }
} }
}catch(CException &e){ }
else
{
length_int=3*servo_ids.size();
data_int=new unsigned char[length_int];
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];
}
this->send_instruction_packet_v1(dyn_bulk_read,data_int,length_int);
/* read all the data from all the servos */
if(data_int!=NULL) if(data_int!=NULL)
{ {
delete[] data_int; delete[] data_int;
data_int=NULL; data_int=NULL;
} }
throw e; 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;
}
}
} }
}catch(CException &e){
if(data_int!=NULL)
{
delete[] data_int;
data_int=NULL;
}
throw e;
} }
} }
...@@ -27,3 +27,9 @@ ADD_EXECUTABLE(test_dynamixel_slave test_dynamixel_slave.cpp) ...@@ -27,3 +27,9 @@ ADD_EXECUTABLE(test_dynamixel_slave test_dynamixel_slave.cpp)
# edit the following line to add the necessary libraries # edit the following line to add the necessary libraries
TARGET_LINK_LIBRARIES(test_dynamixel_slave dynamixel) TARGET_LINK_LIBRARIES(test_dynamixel_slave dynamixel)
# edit the following line to add the source code for the example and the name of the executable
ADD_EXECUTABLE(test_new_firmware test_new_firmware.cpp)
# edit the following line to add the necessary libraries
TARGET_LINK_LIBRARIES(test_new_firmware dynamixel)
...@@ -21,11 +21,11 @@ int main(int argc, char *argv[]) ...@@ -21,11 +21,11 @@ int main(int argc, char *argv[])
events.push_back(dyn_server->get_scan_done_event_id()); events.push_back(dyn_server->get_scan_done_event_id());
events.push_back(dyn_server->get_scan_error_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(dyn_version2); dyn_server->start_scan(dyn_version1);
event_id=event_server->wait_first(events); event_id=event_server->wait_first(events);
if(event_id==0) if(event_id==0)
{ {
devices=dyn_server->get_device_ids(dyn_version2); devices=dyn_server->get_device_ids(dyn_version1);
num_servos=devices.size(); num_servos=devices.size();
start_addr.resize(num_servos); start_addr.resize(num_servos);
length.resize(num_servos); length.resize(num_servos);
...@@ -36,7 +36,7 @@ int main(int argc, char *argv[]) ...@@ -36,7 +36,7 @@ int main(int argc, char *argv[])
start_addr[i]=0x1B; start_addr[i]=0x1B;
length[i]=3; length[i]=3;
} }
dyn_server->read_bulk(servo_ids,start_addr,length,data,dyn_version2); dyn_server->read_bulk(servo_ids,start_addr,length,data,dyn_version1);
for(i=0;i<num_servos;i++) for(i=0;i<num_servos;i++)
{ {
std::cout << "PID parameters for the servo " << (int)servo_ids[i] << std::endl; std::cout << "PID parameters for the servo " << (int)servo_ids[i] << std::endl;
......
...@@ -41,6 +41,7 @@ int main(int argc, char *argv[]) ...@@ -41,6 +41,7 @@ int main(int argc, char *argv[])
} }
else else
std::cout << "Error while scanning the bus: " << dyn_server->get_scan_error() << std::endl; std::cout << "Error while scanning the bus: " << dyn_server->get_scan_error() << std::endl;
return 0;
try{ try{
dyn_server->config_bus(0,1000000); dyn_server->config_bus(0,1000000);
dyn_server->start_scan(); dyn_server->start_scan();
......
...@@ -8,7 +8,7 @@ int main(int argc, char *argv[]) ...@@ -8,7 +8,7 @@ int main(int argc, char *argv[])
CEventServer *event_server=CEventServer::instance(); CEventServer *event_server=CEventServer::instance();
std::vector< std::vector<unsigned char> > data; std::vector< std::vector<unsigned char> > data;
std::vector<unsigned char> servo_ids; std::vector<unsigned char> servo_ids;
unsigned short int star_addrs=0x1E; unsigned short int star_addrs=0x1C;
int i,num_servos,pos=0,event_id; int i,num_servos,pos=0,event_id;
std::list<std::string> events; std::list<std::string> events;
std::vector<int> devices; std::vector<int> devices;
...@@ -31,21 +31,25 @@ int main(int argc, char *argv[]) ...@@ -31,21 +31,25 @@ int main(int argc, char *argv[])
for(i=0;i<num_servos;i++) for(i=0;i<num_servos;i++)
{ {
servo_ids[i]=devices[i]; servo_ids[i]=devices[i];
data[i].resize(4); data[i].resize(6);
data[i][0]=pos%256; data[i][0]=0x00;
data[i][1]=pos/256; data[i][1]=0x01;
data[i][2]=0x00; data[i][2]=pos%256;
data[i][3]=0x02; data[i][3]=pos/256;
data[i][4]=0xFF;
data[i][5]=0x03;
} }
dyn_server->write_sync(servo_ids,star_addrs,data,dyn_version1); dyn_server->write_sync(servo_ids,star_addrs,data,dyn_version1);
sleep(2); sleep(2);
pos=1023; pos=1023;
for(i=0;i<num_servos;i++) for(i=0;i<num_servos;i++)
{ {
data[i][0]=pos%256; data[i][0]=0x00;
data[i][1]=pos/256; data[i][1]=0x01;
data[i][2]=0x00; data[i][2]=pos%256;
data[i][3]=0x01; data[i][3]=pos/256;
data[i][4]=0xFF;
data[i][5]=0x03;
} }
dyn_server->write_sync(servo_ids,star_addrs,data,dyn_version1); dyn_server->write_sync(servo_ids,star_addrs,data,dyn_version1);
sleep(5); sleep(5);
......
#include "eventexceptions.h"
#include "dynamixelserver_ftdi.h"
#include "eventserver.h"
#include <iostream>
int main(int argc, char *argv[])
{
CDynamixelServerFTDI *dyn_server=CDynamixelServerFTDI::instance();
CEventServer *event_server=CEventServer::instance();
int i,j,num_buses,event_id,num_dev,baudrate;
std::list<std::string> events;
std::vector<int> devices;
unsigned short int model;
unsigned char data[53];
CDynamixel *servo;
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->start_scan();
event_id=event_server->wait_first(events);
if(event_id==0)
{
num_dev=dyn_server->get_num_devices();
std::cout << "Num. devices: " << std::dec << num_dev << std::endl;
baudrate=dyn_server->get_baudrate();
std::cout << "Baudrate: " << std::dec << baudrate << " bps" << std::endl;
devices=dyn_server->get_device_ids();
for(i=0;i<num_dev;i++)
{
servo=dyn_server->get_device(devices[i]);
servo->read_word_register(0x00,&model);
std::cout << "servo " << std::dec << devices[i] << " model " << (int)model << std::endl;
servo->read_registers(0x00,data,53);
for(j=0;j<53;j++)
{
std::cout << " Reg " << std::hex << j << " value: " << std::hex << (int)data[j] << std::endl;
}
dyn_server->free_device(devices[i]);
delete servo;
}
}
else
std::cout << "Error while scanning the bus: " << dyn_server->get_scan_error() << 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