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

Modified the examples to better demonstrate the operation of the driver.

parent a0397b10
No related branches found
No related tags found
No related merge requests found
......@@ -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;
}
......
......@@ -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
......
......@@ -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;
}
}
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