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

The Dynamixel motor group class is working with degrees, degrees/s and degrees/s2.

Added the open_loop confioguration to the TMotorConfig Structure
parent 4b1bdb00
No related branches found
No related tags found
No related merge requests found
......@@ -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();
......
......@@ -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;
......
......@@ -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;
}
}
......@@ -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);
......
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