diff --git a/src/dynamixel_motor.cpp b/src/dynamixel_motor.cpp
index 102d3bf177c24e5bb1d6e0876cdc8b53b19eacdc..f36d867556784de6989695c320eaf7372a1782dc 100644
--- a/src/dynamixel_motor.cpp
+++ b/src/dynamixel_motor.cpp
@@ -339,31 +339,34 @@ void CDynamixelMotor::cont_load(std::vector<float>& position, std::vector<float>
}
else
{
- try{
- if(!this->torque_control)
- {
- this->dynamixel_dev->write_word_register(goal_speed,(unsigned short int)velocity[0]);
- this->is_motion_relative(relative);
- if(relative[0])
+ if(this->get_motor_group_id()=="")
+ {
+ try{
+ if(!this->torque_control)
{
- this->dynamixel_dev->registered_word_write(goal_pos,(unsigned short int)(this->current_position+position[0]));
- this->current_position+=position[0];
+ this->dynamixel_dev->write_word_register(goal_speed,(unsigned short int)velocity[0]);
+ this->is_motion_relative(relative);
+ if(relative[0])
+ {
+ this->dynamixel_dev->registered_word_write(goal_pos,(unsigned short int)(this->current_position+position[0]));
+ this->current_position+=position[0];
+ }
+ else
+ {
+ this->dynamixel_dev->registered_word_write(goal_pos,(unsigned short int)position[0]);
+ this->current_position=position[0];
+ }
}
else
{
- this->dynamixel_dev->registered_word_write(goal_pos,(unsigned short int)position[0]);
- this->current_position=position[0];
+ /* handle exceptions */
}
+ }catch(CDynamixelAlarmException &e){
+ /* handle dynamixel exception */
+ if(e.get_alarm()&this->alarms)
+ this->reset_motor();
+ throw;
}
- else
- {
- /* handle exceptions */
- }
- }catch(CDynamixelAlarmException &e){
- /* handle dynamixel exception */
- if(e.get_alarm()&this->alarms)
- this->reset_motor();
- throw;
}
}
}
@@ -385,7 +388,8 @@ void CDynamixelMotor::cont_load(std::vector<float>& velocity, std::vector<float>
void CDynamixelMotor::cont_move(void)
{
- this->dyn_server->action();
+ if(this->get_motor_group_id()=="")
+ this->dyn_server->action();
}
void CDynamixelMotor::cont_stop(void)
diff --git a/src/dynamixel_motor_group.cpp b/src/dynamixel_motor_group.cpp
index 6c9771386b205a48c0d006c7f69a34aa35c74027..0bdf3a014954861fdb95bf95a8090dd258c6e482 100644
--- a/src/dynamixel_motor_group.cpp
+++ b/src/dynamixel_motor_group.cpp
@@ -17,11 +17,6 @@ CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id):CMotorGroup(gr
else
{
this->dyn_server=CDynamixelServer::instance();
-// this->servo_id.resize(4);
-// this->servo_id[0]=0x02;
-// this->servo_id[1]=0x04;
-// this->servo_id[2]=0x03;
-// this->servo_id[3]=0x05;
}
}
@@ -50,6 +45,8 @@ void CDynamixelMotorGroup::move(std::vector<float> &position,std::vector<float>
else
{
try{
+ // call the base class function
+ CMotorGroup::move(position,velocity,acceleration);
data.resize(servo_id.size());
for(i=0;i<servo_id.size();i++)
{
@@ -66,8 +63,8 @@ void CDynamixelMotorGroup::move(std::vector<float> &position,std::vector<float>
{
data[i][0]=((int)(pos_count[i])%256);
data[i][1]=(int)(pos_count[i]/256);
- data[i][2]=0x00;
- data[i][3]=0x00;
+ data[i][2]=((int)(vel_count[i])%256);
+ data[i][3]=(int)(vel_count[i]/256);
}
dyn_server->write_sync(servo_id,goal_pos,data);
}catch(CException &e){
diff --git a/src/examples/test_dynamixel_motor_group.cpp b/src/examples/test_dynamixel_motor_group.cpp
index 88e8b4921e99419fed5e821331e073d13790e356..265b77b22b23b76344e6e9962365e1bee0e72079 100644
--- a/src/examples/test_dynamixel_motor_group.cpp
+++ b/src/examples/test_dynamixel_motor_group.cpp
@@ -23,7 +23,7 @@ int main(int argc, char *argv[])
std::vector<bool> enable(2,true);
std::list<std::string> events;
CDynamixelMotor *cont1,*cont2;
- CMotorGroup *group;
+ CDynamixelMotorGroup *group;
unsigned int i=0;
std::string name;
@@ -39,7 +39,7 @@ int main(int argc, char *argv[])
cont2->set_punch(16);
cont2->set_compliance_margin(1,1);
- group=new CMotorGroup(group_name);
+ group=new CDynamixelMotorGroup(group_name);
name=group->add_motor_control(cont1);
group->config_motor_control(name,config_file);
name=group->add_motor_control(cont2);
@@ -56,7 +56,7 @@ int main(int argc, char *argv[])
acc[1]=vel[1]*vel[1]/(0.2*50);
event_server->wait_all(events);
group->move(pos,vel,acc);
- for(i=0;1<10;i++)
+ for(i=0;i<10;i++)
{
pos[0]=100;
pos[1]=100;
diff --git a/src/examples/test_dynamixel_motor_group_open_loop.cpp b/src/examples/test_dynamixel_motor_group_open_loop.cpp
index 99fc1f716c0afe5c08e797f46dc68c37479a035e..59d4266495d78b130fa1505239ab0c5c83e51705 100755
--- a/src/examples/test_dynamixel_motor_group_open_loop.cpp
+++ b/src/examples/test_dynamixel_motor_group_open_loop.cpp
@@ -14,11 +14,13 @@ std::string group_name="GROUP1";
int main(int argc, char *argv[])
{
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;
CDynamixelMotorGroup *group;
- unsigned int i=0;
+ unsigned int i=0,j=0;
int dir=1;
try{
@@ -36,14 +38,14 @@ int main(int argc, char *argv[])
group=new CDynamixelMotorGroup(group_name);
group->add_motor_control(cont1);
group->add_motor_control(cont2);
+ group->enable(enable);
group->use_open_loop_control();
pos[0]=150.0;
pos[1]=150.0;
group->move(pos,vel,acc);
sleep(2);
- for(;;)
+ for(j=0;j<1000;j++)
{
- std::cout << pos[0] << std::endl;
if(pos[0]>=290)
dir=-1;
else if(pos[0]<=10)
@@ -55,6 +57,28 @@ int main(int argc, char *argv[])
group->move(pos,vel,acc);
usleep(20000);
}
+ 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);
+ for(i=0;i<10;i++)
+ {
+ pos[0]=100;
+ pos[1]=100;
+ event_server->wait_all(events);
+ group->move(pos,vel,acc);
+ 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;
diff --git a/src/examples/test_sequence_open_loop.cpp b/src/examples/test_sequence_open_loop.cpp
index 1acfaa42804c21958e194a270672793dbca827de..aa0a907a742e6894b6d1d51b8475b606dfa5cd07 100755
--- a/src/examples/test_sequence_open_loop.cpp
+++ b/src/examples/test_sequence_open_loop.cpp
@@ -2,6 +2,7 @@
#include "eventserver.h"
#include "exceptions.h"
#include "dynamixel_motor.h"
+#include "dynamixel_motor_group.h"
#include "motor_group.h"
#include <math.h>
@@ -19,7 +20,7 @@ int main(int argc, char * argv[])
try{
CEventServer *event_server=CEventServer::instance();
std::list<std::string> events;
- CMotorGroup group(group_name);
+ CDynamixelMotorGroup group(group_name);
CMotionSequence sequence(seq_name);
CDynamixelMotor cont1(cont_name1,0,1000000,11);
CDynamixelMotor cont2(cont_name2,0,1000000,21);
diff --git a/src/xml/test_motion.xml b/src/xml/test_motion.xml
index 86a6a178e084d3c26663c1685db66b025218c47e..95e982aaab8d3eb5647b331cfae45c487da8e83a 100755
--- a/src/xml/test_motion.xml
+++ b/src/xml/test_motion.xml
@@ -2,16 +2,14 @@
<motion_seq xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
xsi:noNamespaceSchemaLocation="motion_sequence_file.xsd">
- <num_motors>3</num_motors>
+ <num_motors>2</num_motors>
<num_steps>1</num_steps>
<control>position</control>
<motion>absolute</motion>
- <loop>closed</loop>
+ <loop>open</loop>
<step>
<position>150.0</position>
<position>150.0</position>
- <position>150.0</position>
- <velocity>100.0</velocity>
<velocity>100.0</velocity>
<velocity>100.0</velocity>
<delay>0</delay>
diff --git a/src/xml/test_motion2.xml b/src/xml/test_motion2.xml
index b80295d991bbf69f3762ed532911a891d4f3c53f..0b474af49661caca00f551e629600034ffd2f2ee 100755
--- a/src/xml/test_motion2.xml
+++ b/src/xml/test_motion2.xml
@@ -2,16 +2,14 @@
<motion_seq xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
xsi:noNamespaceSchemaLocation="motion_sequence_file.xsd">
- <num_motors>3</num_motors>
+ <num_motors>2</num_motors>
<num_steps>1</num_steps>
<control>position</control>
<motion>absolute</motion>
- <loop>closed</loop>
+ <loop>open</loop>
<step>
<position>150.0</position>
<position>150.0</position>
- <position>150.0</position>
- <velocity>150.0</velocity>
<velocity>150.0</velocity>
<velocity>150.0</velocity>
<delay>0</delay>