diff --git a/src/examples/test_dynamixel_motor_group_open_loop.cpp b/src/examples/test_dynamixel_motor_group_open_loop.cpp new file mode 100755 index 0000000000000000000000000000000000000000..64a451c3a7eca14bcff4eeed7527b21a2918beec --- /dev/null +++ b/src/examples/test_dynamixel_motor_group_open_loop.cpp @@ -0,0 +1,64 @@ +#include "dynamixelexceptions.h" +#include "dynamixelserver.h" +#include "eventserver.h" +#include "exceptions.h" +#include "dynamixel_motor_group.h" +#include "dynamixel_motor.h" +#include <iostream> +#include <math.h> + +std::string cont1_name="AX-12+_1"; +std::string cont2_name="AX-12+_2"; +std::string cont3_name="AX-12+_3"; +std::string cont4_name="AX-12+_4"; +std::string group_name="GROUP1"; + +int main(int argc, char *argv[]) +{ + CDynamixelServer *dyn_server=CDynamixelServer::instance(); + std::vector<float> pos(2),vel(2),acc(2); + CDynamixelMotor *cont1,*cont2,*cont3,*cont4; + CDynamixelMotorGroup *group; + int dir=1,i=0; + + try{ + if(dyn_server->get_num_buses()>0) + { + cont1=new CDynamixelMotor(cont1_name,0,1000000,21); + cont2=new CDynamixelMotor(cont2_name,0,1000000,11); + 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); + + group=new CDynamixelMotorGroup(group_name); + group->add_motor_control(cont1); + group->add_motor_control(cont2); + group->use_open_loop_control(); + pos[0]=512; + pos[1]=512; + group->move(pos,vel,acc); + sleep(5); + for(;;) + { + std::cout << pos[0] << std::endl; + if(pos[0]>=700) + dir=-1; + else if(pos[0]<=300) + dir=1; + for(i=0;i<pos.size();i++) + { + pos[i]+=5*dir; + } + group->move(pos,vel,acc); + usleep(20000); + } + } + }catch(CException &e){ + std::cout << e.what() << std::endl; + } +} + +