diff --git a/src/examples/test_dynamixel_bulk_read.cpp b/src/examples/test_dynamixel_bulk_read.cpp
index 212dff4d4b20587e67eca0e0d93f6b26bec0da8e..9ad7c34e852f6afbde89861b076771d3fac43b73 100755
--- a/src/examples/test_dynamixel_bulk_read.cpp
+++ b/src/examples/test_dynamixel_bulk_read.cpp
@@ -7,42 +7,45 @@ int main(int argc, char *argv[])
{
CDynamixelServer *dyn_server=CDynamixelServer::instance();
CEventServer *event_server=CEventServer::instance();
- int i,j,num_servos;
- std::vector<int> devices;
+ std::vector< std::vector<unsigned char> > data;
+ std::vector<unsigned short int> start_addr;
+ std::vector<unsigned short int> length;
+ std::vector<unsigned char> servo_ids;
std::list<std::string> events;
- unsigned char start_addrs=0x00;
- unsigned char data[49];
- CDynamixel *device;
+ int i,num_servos,event_id;
+ std::vector<int> devices;
try{
if(dyn_server->get_num_buses()>0)
{
-
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_server->wait_first(events);
- devices=dyn_server->get_device_ids();
- num_servos=devices.size();
- for(i=0;i<num_servos;i++)
+ dyn_server->start_scan(dyn_version2);
+ event_id=event_server->wait_first(events);
+ if(event_id==0)
{
- device=dyn_server->get_device(devices[i]);
- device->read_registers(start_addrs,data,49);
- std::cout << "Servo " << std::dec << devices[i] << " registers:" << std::endl;
- for(j=0;j<49;j++)
- std::cout << "Register 0x" << std::hex << j << ": 0x" << (int)data[j] << std::endl;
- data[0]=0x00;
- data[1]=0x00;
- data[2]=0xFF;
- data[3]=0x03;
- device->write_registers(0x06,data,4);
- device->read_registers(start_addrs,data,49);
- std::cout << "Servo " << std::dec << devices[i] << " registers:" << std::endl;
- for(j=0;j<49;j++)
- std::cout << "Register 0x" << std::hex << j << ": 0x" << (int)data[j] << std::endl;
- dyn_server->free_device(devices[i]);
- }
- }
+ devices=dyn_server->get_device_ids(dyn_version2);
+ num_servos=devices.size();
+ start_addr.resize(num_servos);
+ length.resize(num_servos);
+ servo_ids.resize(num_servos);
+ for(i=0;i<num_servos;i++)
+ {
+ servo_ids[i]=devices[i];
+ start_addr[i]=0x1B;
+ length[i]=3;
+ }
+ dyn_server->read_bulk(servo_ids,start_addr,length,data,dyn_version2);
+ for(i=0;i<num_servos;i++)
+ {
+ std::cout << "PID parameters for the servo " << (int)servo_ids[i] << std::endl;
+ std::cout << "\tP: " << (int)data[i][2] << std::endl;
+ std::cout << "\tI: " << (int)data[i][1] << std::endl;
+ std::cout << "\tD: " << (int)data[i][0] << std::endl;
+ }
+ }
+ }
}catch(CException &e){
std::cout << e.what() << std::endl;
}
diff --git a/src/examples/test_dynamixel_server.cpp b/src/examples/test_dynamixel_server.cpp
index e9be11137aeab6232b8296a4684bea444c46aaec..e02eaa6f2eb35f793d0103701e93c567e724ac66 100644
--- a/src/examples/test_dynamixel_server.cpp
+++ b/src/examples/test_dynamixel_server.cpp
@@ -6,16 +6,12 @@ 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,j;
+ int num_buses=0,baudrate=0,event_id;
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;
+ CDynamixel *dyn_device;
+ unsigned int i,num_dev;
num_buses=dyn_server->get_num_buses();
std::cout << "Num. buses: " << num_buses << std::endl;
@@ -24,7 +20,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(0,1000000);
dyn_server->start_scan();
event_id=event_server->wait_first(events);
if(event_id==0)
@@ -36,49 +32,22 @@ int main(int argc, char *argv[])
devices=dyn_server->get_device_ids();
for(i=0;i<num_dev;i++)
{
- dyn_motor=dyn_server->get_device(devices[i]);
- 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);
- std::cout << "Device " << std::dec << i << ": id -> " << devices[i] << " model: " << std::hex << model << " margin: " << (int)margin << std::endl;
+ dyn_device=dyn_server->get_device(devices[i]);
+ dyn_device->read_word_register(0x00,&model);
dyn_server->free_device(devices[i]);
+ delete dyn_device;
}
- // 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);
@@ -92,50 +61,10 @@ int main(int argc, char *argv[])
devices=dyn_server->get_device_ids(dyn_version2);
for(i=0;i<num_dev;i++)
{
- dyn_motor=dyn_server->get_device(devices[i],dyn_version2);
- 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);
- std::cout << "Device " << std::dec << i << ": id -> " << devices[i] << " model: " << std::hex << model << " margin: " << (int)margin << std::endl;
+ dyn_device=dyn_server->get_device(devices[i],dyn_version2);
+ dyn_device->read_word_register(0x00,&model);
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;
+ delete dyn_device;
}
}
else
diff --git a/src/examples/test_dynamixel_sync.cpp b/src/examples/test_dynamixel_sync.cpp
index 2ed4335f6a456ba9439ae6fcf8b18910229d77b1..778d44eddab27056b28ae8f05399292be5b78eb8 100644
--- a/src/examples/test_dynamixel_sync.cpp
+++ b/src/examples/test_dynamixel_sync.cpp
@@ -6,80 +6,59 @@ int main(int argc, char *argv[])
{
CDynamixelServer *dyn_server=CDynamixelServer::instance();
CEventServer *event_server=CEventServer::instance();
- int i,num_servos,num_instruc,dir,pos;
- unsigned char const goal_pos=0x1E;
- unsigned char star_addrs;
- std::list<std::string> events;
std::vector< std::vector<unsigned char> > data;
- std::vector<unsigned char> devices(3);
-
+ std::vector<unsigned char> servo_ids;
+ unsigned short int star_addrs=0x1E;
+ int i,num_servos,pos=0,event_id;
+ std::list<std::string> events;
+ std::vector<int> devices;
+
try{
if(dyn_server->get_num_buses()>0)
{
-
- //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());
dyn_server->config_bus(0,1000000);
- //dyn_server->start_scan();
- //event_server->wait_first(events);
- //devices=dyn_server->get_device_ids();
- data.resize(devices.size());
- num_servos=data.size();
- star_addrs=goal_pos;
- pos=512;
- dir=1;
- devices[0]=0x0F;
- devices[1]=0x01;
- devices[2]=0x0C;
- for(i=0;i<num_servos;i++)
+ dyn_server->start_scan(dyn_version1);
+ event_id=event_server->wait_first(events);
+ if(event_id==0)
{
- /* Number of instructions in bytes:
- - goal_pos: 2 bytes
- - goal_speed: 2 bytes */
- num_instruc=4;
- data[i].resize(num_instruc);
- }
- for(i=0;i<num_servos;i++)
- {
-
- data[i][0]=pos%256;
- data[i][1]=(int)(pos/256);
- data[i][2]=0x00;
- data[i][3]=0x02;
- }
- dyn_server->write_sync(devices,star_addrs,data);
- usleep(2000000);
- std::cout << "move" << std::endl;
- for(;;)
- {
- if(pos>=1000)
- dir=-1;
- else if(pos<=24)
- dir=1;
-
-
- for(i=0;i<num_servos;i++)
- {
-
- data[i][0]=pos%256;
- data[i][1]=(int)(pos/256);
- data[i][2]=0xFF;
- data[i][3]=0x03;
- }
- std::cout << pos << std::endl;
- pos=pos+dir*2;
-
- dyn_server->write_sync(devices,star_addrs,data);
- usleep(10000);std::cout << pos << std::endl;
+ num_servos=dyn_server->get_num_devices(dyn_version1);
+ std::cout << "Num servos: " << num_servos << std::endl;
+ devices=dyn_server->get_device_ids(dyn_version1);
+ data.resize(num_servos);
+ servo_ids.resize(num_servos);
+ 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;
+ }
+ 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;
+ }
+ dyn_server->write_sync(servo_ids,star_addrs,data,dyn_version1);
+ sleep(5);
}
- }
+ }
}catch(CException &e){
- std::cout << "Movement aborted" << std::endl;
+ std::cout << e.what() << std::endl;
}
}
-
-
-
-
-
-
-
+
+
+
+
+
+
+