From d2198d37c82835aaea88b6efcedd1a0733320e17 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Sergi=20Hern=C3=A0ndez=20Juan?= <shernand@iri.upc.edu> Date: Tue, 7 Feb 2012 11:53:37 +0000 Subject: [PATCH] The Dynamixel motor group class is working with degrees, degrees/s and degrees/s2. Added the open_loop confioguration to the TMotorConfig Structure --- src/dynamixel_motor.cpp | 1 + src/dynamixel_motor_group.cpp | 16 ++-- src/examples/test_dynamixel_motor_group.cpp | 79 +++++++++---------- .../test_dynamixel_motor_group_open_loop.cpp | 12 +-- 4 files changed, 54 insertions(+), 54 deletions(-) diff --git a/src/dynamixel_motor.cpp b/src/dynamixel_motor.cpp index f2cb92e..102d3bf 100644 --- a/src/dynamixel_motor.cpp +++ b/src/dynamixel_motor.cpp @@ -53,6 +53,7 @@ CDynamixelMotor::CDynamixelMotor(std::string& cont_id,unsigned char bus_id,int b config.position.resize(1); config.position[0].min=min[0]; config.position[0].max=max[0]; + config.open_loop=false; this->config(&config); if(min[0]==0.0 && max[0]==0.0) this->enable_torque_control(); diff --git a/src/dynamixel_motor_group.cpp b/src/dynamixel_motor_group.cpp index 6493d42..6c97713 100644 --- a/src/dynamixel_motor_group.cpp +++ b/src/dynamixel_motor_group.cpp @@ -27,11 +27,11 @@ CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id):CMotorGroup(gr void CDynamixelMotorGroup::move(std::vector<float> &position,std::vector<float> &velocity,std::vector<float> &acceleration) { + std::vector<float> pos_count,vel_count,accel_count; std::vector< std::vector<unsigned char> > data; - unsigned char star_addrs; + std::vector<short int> count_value; unsigned int i=0; - if(position.size()!=this->servo_id.size()) { /* handle errors */ @@ -50,7 +50,6 @@ void CDynamixelMotorGroup::move(std::vector<float> &position,std::vector<float> else { try{ - star_addrs=goal_pos; data.resize(servo_id.size()); for(i=0;i<servo_id.size();i++) { @@ -59,15 +58,18 @@ void CDynamixelMotorGroup::move(std::vector<float> &position,std::vector<float> - goal_speed: 2 bytes */ data[i].resize(4); } + // convert the input data + this->angles_to_controller(position,pos_count); + this->speeds_to_controller(velocity,vel_count); + this->accels_to_controller(acceleration,accel_count); for(i=0;i<servo_id.size();i++) { - - data[i][0]=((int)position[i])%256; - data[i][1]=position[i]/256; + data[i][0]=((int)(pos_count[i])%256); + data[i][1]=(int)(pos_count[i]/256); data[i][2]=0x00; data[i][3]=0x00; } - dyn_server->write_sync(servo_id,star_addrs,data); + dyn_server->write_sync(servo_id,goal_pos,data); }catch(CException &e){ /* handle exceptions */ throw; diff --git a/src/examples/test_dynamixel_motor_group.cpp b/src/examples/test_dynamixel_motor_group.cpp index 5fd26cc..88e8b49 100644 --- a/src/examples/test_dynamixel_motor_group.cpp +++ b/src/examples/test_dynamixel_motor_group.cpp @@ -17,65 +17,60 @@ std::string config_file="../src/xml/base_dyn_config.xml"; int main(int argc, char *argv[]) { - CDynamixelServer *dyn_server=CDynamixelServer::instance(); -// std::vector<float> pos(4),vel(4),acc(4); - std::vector<float> pos(2),vel(2),acc(2); - CDynamixelMotor *cont1,*cont2,*cont3,*cont4; - CDynamixelMotorGroup *group; - int dir=1,i=0; - std::string name; - - try{ + CDynamixelServer *dyn_server=CDynamixelServer::instance(); + CEventServer *event_server=CEventServer::instance(); + std::vector<float> pos(2),vel(2),acc(2); + std::vector<bool> enable(2,true); + std::list<std::string> events; + CDynamixelMotor *cont1,*cont2; + CMotorGroup *group; + unsigned int i=0; + std::string name; + + try{ if(dyn_server->get_num_buses()>0) { cont1=new CDynamixelMotor(cont1_name,0,1000000,21); cont2=new CDynamixelMotor(cont2_name,0,1000000,11); -// cont3=new CDynamixelMotor(cont3_name,0,1000000,3); -// cont4=new CDynamixelMotor(cont4_name,0,1000000,5); cont1->set_compliance_slope(254,254); cont1->set_punch(4); cont1->set_compliance_margin(1,1); cont2->set_compliance_slope(254,254); cont2->set_punch(16); cont2->set_compliance_margin(1,1); -/* cont3->set_compliance_slope(254,254); - cont3->set_punch(16); - cont3->set_compliance_margin(1,1); - cont4->set_compliance_slope(254,254); - cont4->set_punch(32); - cont4->set_compliance_margin(1,1);*/ - - group=new CDynamixelMotorGroup(group_name); + + group=new CMotorGroup(group_name); name=group->add_motor_control(cont1); group->config_motor_control(name,config_file); name=group->add_motor_control(cont2); - group->config_motor_control(name,config_file); -// group->add_motor_control(cont3); -// group->add_motor_control(cont4); - pos[0]=512; - pos[1]=512; -// pos[2]=512; -// pos[3]=512; + + group->get_position(pos); + group->enable(enable); + group->use_closed_loop_control(); + events.push_back(group->get_position_reached_event_id()); + pos[0]=150.0; + pos[1]=150.0; + vel[0]=100.0; + vel[1]=100.0; + acc[0]=vel[0]*vel[0]/(0.2*50); + acc[1]=vel[1]*vel[1]/(0.2*50); + event_server->wait_all(events); group->move(pos,vel,acc); - usleep(1000000); - for(;;) + for(i=0;1<10;i++) { - if(pos[0]>=700) - dir=-1; - else if(pos[0]<=300) - dir=1; - for(i=0;i<pos.size();i++) - { - pos[i]+=5*dir; - } + pos[0]=100; + pos[1]=100; + event_server->wait_all(events); group->move(pos,vel,acc); - usleep(20000); - - } + pos[0]=200; + pos[1]=200; + event_server->wait_all(events); + group->move(pos,vel,acc); + } } }catch(CException &e){ - std::cout << e.what() << std::endl; + std::cout << e.what() << std::endl; } } - - + + diff --git a/src/examples/test_dynamixel_motor_group_open_loop.cpp b/src/examples/test_dynamixel_motor_group_open_loop.cpp index 3e8a09f..99fc1f7 100755 --- a/src/examples/test_dynamixel_motor_group_open_loop.cpp +++ b/src/examples/test_dynamixel_motor_group_open_loop.cpp @@ -15,6 +15,7 @@ int main(int argc, char *argv[]) { CDynamixelServer *dyn_server=CDynamixelServer::instance(); std::vector<float> pos(2),vel(2),acc(2); + std::vector<bool> enable(2,true); CDynamixelMotor *cont1,*cont2; CDynamixelMotorGroup *group; unsigned int i=0; @@ -36,19 +37,20 @@ int main(int argc, char *argv[]) group->add_motor_control(cont1); group->add_motor_control(cont2); group->use_open_loop_control(); - pos[0]=512; - pos[1]=512; + pos[0]=150.0; + pos[1]=150.0; group->move(pos,vel,acc); sleep(2); for(;;) { - if(pos[0]>=700) + std::cout << pos[0] << std::endl; + if(pos[0]>=290) dir=-1; - else if(pos[0]<=300) + else if(pos[0]<=10) dir=1; for(i=0;i<pos.size();i++) { - pos[i]+=5*dir; + pos[i]+=1.5*dir; } group->move(pos,vel,acc); usleep(20000); -- GitLab