diff --git a/src/dynamixelserver.cpp b/src/dynamixelserver.cpp
index dc250af1d653d8a89ac4ec00ff9028e0711bee40..1fe70ced63a435cb29ad2bd2cdf11abab72d5c47 100644
--- a/src/dynamixelserver.cpp
+++ b/src/dynamixelserver.cpp
@@ -73,6 +73,9 @@ void CDynamixelServer::send_instruction_packet_v1(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=6+len;
packet=new unsigned char[length];
@@ -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)
{
- 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 int i,j=0;
int length_int=0;
@@ -1005,14 +1008,9 @@ void CDynamixelServer::read_sync(std::vector<unsigned char>& servo_ids,unsigned
/* handle exceptions */
throw CDynamixelServerException(_HERE_,"No servos specified");
}
- if(version==dyn_version1)
- {
- /* handle exception */
- throw CDynamixelServerException(_HERE_,"Operation not supported in version 1");
- }
- else
- {
- try{
+ try{
+ if(version==dyn_version2)
+ {
length_int=4+servo_ids.size();
data_int=new unsigned char[length_int];
data_int[0]=start_addr%256;
@@ -1051,14 +1049,53 @@ void CDynamixelServer::read_sync(std::vector<unsigned char>& servo_ids,unsigned
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)
{
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
/* handle exceptions */
throw CDynamixelServerException(_HERE_,"Too many or not enough data blocks");
}
- if(version==dyn_version1)
- {
- /* handle exception */
- throw CDynamixelServerException(_HERE_,"Operation not supported in version 1");
- }
- else
- {
- try{
+ try{
+ if(version==dyn_version2)
+ {
length=0;
for(i=0;i<servo_ids.size();i++)
length+=5+data[i].size();
@@ -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);
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=NULL;
+ data_int[offset]=servo_ids[i];
+ 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)
{
- 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 int i,j=0;
int length_int=0;
@@ -1141,14 +1191,9 @@ void CDynamixelServer::read_bulk(std::vector<unsigned char>& servo_ids,std::vect
/* handle exceptions */
throw CDynamixelServerException(_HERE_,"Too many or not enough read lengths");
}
- if(version==dyn_version1)
- {
- /* handle exception */
- throw CDynamixelServerException(_HERE_,"Operation not supported in version 1");
- }
- else
- {
- try{
+ try{
+ if(version==dyn_version2)
+ {
length_int=5*servo_ids.size();
data_int=new unsigned char[length_int];
for(i=0;i<servo_ids.size();i++)
@@ -1189,14 +1234,55 @@ void CDynamixelServer::read_bulk(std::vector<unsigned char>& servo_ids,std::vect
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)
{
delete[] data_int;
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;
}
}
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index d882fdcd05bcada8f742964d66b900afc9937316..666109205992fd850d7d764034bc95c210c91cd9 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -27,3 +27,9 @@ ADD_EXECUTABLE(test_dynamixel_slave test_dynamixel_slave.cpp)
# edit the following line to add the necessary libraries
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)
diff --git a/src/examples/test_dynamixel_bulk_read.cpp b/src/examples/test_dynamixel_bulk_read.cpp
index 8e457cf2f86b0aa64e70006542594be12be6ee96..f79de20fbaf8b7d1dc0f6e01ce2e79a9ee643c26 100755
--- a/src/examples/test_dynamixel_bulk_read.cpp
+++ b/src/examples/test_dynamixel_bulk_read.cpp
@@ -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_error_event_id());
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);
if(event_id==0)
{
- devices=dyn_server->get_device_ids(dyn_version2);
+ devices=dyn_server->get_device_ids(dyn_version1);
num_servos=devices.size();
start_addr.resize(num_servos);
length.resize(num_servos);
@@ -36,7 +36,7 @@ int main(int argc, char *argv[])
start_addr[i]=0x1B;
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++)
{
std::cout << "PID parameters for the servo " << (int)servo_ids[i] << std::endl;
diff --git a/src/examples/test_dynamixel_server.cpp b/src/examples/test_dynamixel_server.cpp
index 8e3ed0f5a3e0e65793a7c54e51fc0b3910afcb24..b8567e82a5856ba34ecf67a07ed2e3b828afa7b3 100644
--- a/src/examples/test_dynamixel_server.cpp
+++ b/src/examples/test_dynamixel_server.cpp
@@ -41,6 +41,7 @@ int main(int argc, char *argv[])
}
else
std::cout << "Error while scanning the bus: " << dyn_server->get_scan_error() << std::endl;
+ return 0;
try{
dyn_server->config_bus(0,1000000);
dyn_server->start_scan();
diff --git a/src/examples/test_dynamixel_sync.cpp b/src/examples/test_dynamixel_sync.cpp
index f4ab9f6f087cf86bdcc075edb3ea938ebd2b5188..3bbf67d5c5df4aa59a4acf4a71e61c69b0f705d5 100644
--- a/src/examples/test_dynamixel_sync.cpp
+++ b/src/examples/test_dynamixel_sync.cpp
@@ -8,7 +8,7 @@ int main(int argc, char *argv[])
CEventServer *event_server=CEventServer::instance();
std::vector< std::vector<unsigned char> > data;
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;
std::list<std::string> events;
std::vector<int> devices;
@@ -31,21 +31,25 @@ int main(int argc, char *argv[])
for(i=0;i<num_servos;i++)
{
servo_ids[i]=devices[i];
- data[i].resize(4);
- data[i][0]=pos%256;
- data[i][1]=pos/256;
- data[i][2]=0x00;
- data[i][3]=0x02;
+ data[i].resize(6);
+ data[i][0]=0x00;
+ data[i][1]=0x01;
+ data[i][2]=pos%256;
+ data[i][3]=pos/256;
+ data[i][4]=0xFF;
+ data[i][5]=0x03;
}
dyn_server->write_sync(servo_ids,star_addrs,data,dyn_version1);
sleep(2);
pos=1023;
for(i=0;i<num_servos;i++)
{
- data[i][0]=pos%256;
- data[i][1]=pos/256;
- data[i][2]=0x00;
- data[i][3]=0x01;
+ data[i][0]=0x00;
+ data[i][1]=0x01;
+ data[i][2]=pos%256;
+ data[i][3]=pos/256;
+ data[i][4]=0xFF;
+ data[i][5]=0x03;
}
dyn_server->write_sync(servo_ids,star_addrs,data,dyn_version1);
sleep(5);
diff --git a/src/examples/test_new_firmware.cpp b/src/examples/test_new_firmware.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b5ae6f459baa8a8ea1f73adf555f07514eb19715
--- /dev/null
+++ b/src/examples/test_new_firmware.cpp
@@ -0,0 +1,61 @@
+#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;
+ }
+ }
+}
+
+
+
+
+
+
+