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

Removed the acceleration from the move functions because it can not be... 

Removed the acceleration from the move functions because it can not be directly controlled by the dynamixel servos.
parent 86bea5c9
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/CMakeLists.txt
+ 4
− 2
View file @ 5bdf7a7f
......@@ -11,15 +11,17 @@ FIND_PACKAGE(comm REQUIRED)
FIND_PACKAGE(dynamixel REQUIRED)
FIND_PACKAGE(trajectory REQUIRED)
# edit the following line to add the necessary include directories
INCLUDE_DIRECTORIES(. ${iriutils_INCLUDE_DIR} ${comm_INCLUDE_DIR} ${dynamixel_INCLUDE_DIR})
INCLUDE_DIRECTORIES(. ${iriutils_INCLUDE_DIR} ${comm_INCLUDE_DIR} ${dynamixel_INCLUDE_DIR} ${trajectory_INCLUDE_DIR})
SET_SOURCE_FILES_PROPERTIES(${XSD_SOURCES} PROPERTIES GENERATED 1)
ADD_LIBRARY(dynamixel_motor_cont SHARED ${sources} ${XSD_SOURCES})
#edit the following line to add the necessary system libraries (if any)
TARGET_LINK_LIBRARIES(dynamixel_motor_cont ${iriutils_LIBRARY} ${comm_LIBRARY} ${dynamixel_LIBRARY} ${XSD_LIBRARY})
TARGET_LINK_LIBRARIES(dynamixel_motor_cont ${iriutils_LIBRARY} ${comm_LIBRARY} ${dynamixel_LIBRARY} ${XSD_LIBRARY} ${trajectory_LIBRARY})
ADD_DEPENDENCIES(dynamixel_motor_cont xsd_files_gen)
......
src/dynamixel_motor_group.cpp
+ 0
− 559
View file @ 5bdf7a7f
......@@ -31,12 +31,6 @@ CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id)
this->dyn_server=CDynamixelServer::instance();
this->clear();
this->mode=angle_ctrl;
/* initialize motion sequence attributes */
this->sequence_state=mtn_empty;
this->sequence_current_step=-1;
this->sequence_error_msg="Motion sequence ended successfully";
this->init_events();
this->init_threads();
}
}
......@@ -55,10 +49,6 @@ CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id,unsigned bus_id
this->dyn_server=CDynamixelServer::instance();
this->clear();
this->mode=torque_ctrl;
/* initialize motion sequence attributes */
this->sequence_state=mtn_empty;
this->sequence_current_step=-1;
this->sequence_error_msg="Motion sequence ended successfully";
if(ids.size()==0)
{
/* handle exceptions */
......@@ -69,8 +59,6 @@ CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id,unsigned bus_id
for(i=0;i<ids.size();i++)
this->init_motor(ids[i],version);
this->set_control_mode(angle_ctrl);
this->init_events();
this->init_threads();
}catch(CException &e){
/* handle exceptions */
this->clear();
......@@ -104,8 +92,6 @@ CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id,std::string &bu
for(i=0;i<ids.size();i++)
this->init_motor(ids[i],version);
this->set_control_mode(angle_ctrl);
this->init_events();
this->init_threads();
}catch(CException &e){
/* handle exceptions */
this->clear();
......@@ -662,92 +648,6 @@ void CDynamixelMotorGroup::get_pid_control(unsigned int index,TDynamixel_pid &co
}
}
void *CDynamixelMotorGroup::sequence_thread(void *param)
{
CDynamixelMotorGroup *mtn_seq=(CDynamixelMotorGroup *)param;
std::list<std::string> events;
int event_id=0,to=-1;
bool end=false;
// initialize the motion events
events.push_back(mtn_seq->finish_thread_event_id);
events.push_back(mtn_seq->start_sequence_event_id);
events.push_back(mtn_seq->stop_sequence_event_id);
events.push_back(mtn_seq->resume_sequence_event_id);
while(!end)
{
try{
event_id=mtn_seq->event_server->wait_first(events,to);
mtn_seq->motion_access.enter();
switch(event_id)
{
case -1: mtn_seq->sequence_current_step++;
to=mtn_seq->load_motion(mtn_seq->sequence_current_step);
break;
case 0: /* finish the thread */
end=true;
break;
case 1: /* start sequence */
to=mtn_seq->load_motion(mtn_seq->sequence_current_step);
break;
case 2: /* stop sequence */
mtn_seq->stop();
mtn_seq->sequence_error_msg="Motion sequence stopped by user";
mtn_seq->sequence_current_step=0;
to=-1;
if(!mtn_seq->event_server->event_is_set(mtn_seq->sequence_complete_event_id))
mtn_seq->event_server->set_event(mtn_seq->sequence_complete_event_id);
break;
case 3: /* resume sequence */
mtn_seq->sequence_current_step++;
to=mtn_seq->load_motion(mtn_seq->sequence_current_step);
break;
}
mtn_seq->motion_access.exit();
}catch(CEventTimeoutException &e){
if(!mtn_seq->event_server->event_is_set(mtn_seq->pause_sequence_event_id))
{
/* load and execute the next step */
mtn_seq->motion_access.enter();
mtn_seq->sequence_current_step++;
try{
to=mtn_seq->load_motion(mtn_seq->sequence_current_step);
}catch(CException &e){
to=-1;
mtn_seq->stop();
mtn_seq->sequence_state=mtn_loaded;
mtn_seq->sequence_error_msg=e.what();
// signal the user
if(!mtn_seq->event_server->event_is_set(mtn_seq->sequence_error_event_id))
mtn_seq->event_server->set_event(mtn_seq->sequence_error_event_id);
if(!mtn_seq->event_server->event_is_set(mtn_seq->sequence_complete_event_id))
mtn_seq->event_server->set_event(mtn_seq->sequence_complete_event_id);
}
mtn_seq->motion_access.exit();
}
else
{
mtn_seq->event_server->reset_event(mtn_seq->pause_sequence_event_id);
to=-1;
}
}catch(CException &e){
to=-1;
mtn_seq->motion_access.exit();
mtn_seq->stop();
mtn_seq->sequence_state=mtn_loaded;
mtn_seq->sequence_error_msg=e.what();
// signal the user
if(!mtn_seq->event_server->event_is_set(mtn_seq->sequence_error_event_id))
mtn_seq->event_server->set_event(mtn_seq->sequence_error_event_id);
if(!mtn_seq->event_server->event_is_set(mtn_seq->sequence_complete_event_id))
mtn_seq->event_server->set_event(mtn_seq->sequence_complete_event_id);
}
}
pthread_exit(NULL);
}
void CDynamixelMotorGroup::init_motor(unsigned int id,dyn_version_t version)
{
TDynamixel_info info;
......@@ -777,35 +677,6 @@ void CDynamixelMotorGroup::init_motor(unsigned int id,dyn_version_t version)
this->servo_id.push_back(id);
}
void CDynamixelMotorGroup::init_threads(void)
{
this->thread_server=CThreadServer::instance();
this->sequence_thread_id=this->group_id + "sequence_thread";
this->thread_server->create_thread(this->sequence_thread_id);
this->thread_server->attach_thread(this->sequence_thread_id,this->sequence_thread,this);
this->thread_server->start_thread(this->sequence_thread_id);
}
void CDynamixelMotorGroup::init_events(void)
{
this->event_server=CEventServer::instance();
this->finish_thread_event_id=this->group_id + "_finish_thread_event";
this->event_server->create_event(this->finish_thread_event_id);
this->sequence_complete_event_id=this->group_id + "_sequence_complete_event";
this->event_server->create_event(this->sequence_complete_event_id);
this->event_server->set_event(this->sequence_complete_event_id);
this->sequence_error_event_id=this->group_id + "_sequence_error_event";
this->event_server->create_event(this->sequence_error_event_id);
this->start_sequence_event_id=this->group_id + "_start_sequence_event";
this->event_server->create_event(this->start_sequence_event_id);
this->stop_sequence_event_id=this->group_id + "_stop_sequence_event";
this->event_server->create_event(this->stop_sequence_event_id);
this->pause_sequence_event_id=this->group_id + "_pause_sequence_event";
this->event_server->create_event(this->pause_sequence_event_id);
this->resume_sequence_event_id=this->group_id + "_resume_sequence_event";
this->event_server->create_event(this->resume_sequence_event_id);
}
void CDynamixelMotorGroup::clear(void)
{
unsigned int i=0;
......@@ -827,11 +698,6 @@ void CDynamixelMotorGroup::clear(void)
this->info.clear();
this->alarms.clear();
this->servo_id.clear();
/* clear any motion attribute */
this->sequence_state=mtn_empty;
this->sequence_current_step=-1;
this->sequence_error_msg="Motion sequence ended successfully";
this->seq.clear();
}
void CDynamixelMotorGroup::set_control_mode(control_mode mode)
......@@ -872,51 +738,6 @@ void CDynamixelMotorGroup::set_control_mode(control_mode mode)
}
}
int CDynamixelMotorGroup::load_motion(unsigned int step_id)
{
static std::vector<int> time(this->get_num_motors());
std::vector<double> current_position;
unsigned int i=0;
double distance;
int max=0;
if(step_id<0)
{
/* handle errors */
throw CDynamixelMotionSequenceException(_HERE_,"Invalid step index");
}
else if(step_id>=this->seq.size())
{
this->sequence_state=mtn_loaded;
this->sequence_error_msg="Motion sequence completed successfully";
/* signal the completion of the sequence */
if(!this->event_server->event_is_set(this->sequence_complete_event_id))
this->event_server->set_event(this->sequence_complete_event_id);
return -1;
}
else
{
this->get_current_angle(current_position);
for(i=0;i<this->get_num_motors();i++)
{
if(this->seq[step_id].absolute_motion)// absolute motion
distance=fabs(current_position[i]-this->seq[step_id].position[i]);
else// relative motion
distance=fabs(this->seq[step_id].position[i]);
time[i]=distance/this->seq[step_id].velocity[i]*1000;// time in ms
}
if(this->seq[step_id].absolute_motion)
this->move_absolute_angle(this->seq[step_id].position,this->seq[step_id].velocity);
else
this->move_relative_angle(this->seq[step_id].position,this->seq[step_id].velocity);
max=time[0];
for(i=1;i<this->get_num_motors();i++)
if(time[i]>max)
max=time[i];
return max+this->seq[step_id].delay;
}
}
void CDynamixelMotorGroup::load_config(TDynamixelGroup_config &config)
{
std::vector<TDynamixel_compliance> compliance;
......@@ -1105,105 +926,6 @@ void CDynamixelMotorGroup::save_config(std::string &filename)
}
void CDynamixelMotorGroup::load_sequence(std::string &filename)
{
dyn_sequence_t::step_iterator iterator;
TDynamixelMotionStep step;
struct stat buffer;
if(stat(filename.c_str(),&buffer)==0)
{
// try to open the specified file
try{
// stop any current motion
this->motion_access.enter();
if(this->sequence_state==mtn_started || this->sequence_state==mtn_paused)
{
this->motion_access.exit();
this->stop_sequence();
this->motion_access.enter();
this->sequence_state=mtn_loaded;
}
// load the new sequence
std::auto_ptr<dyn_sequence_t> seq(dyn_motion_seq(filename.c_str(),xml_schema::flags::dont_validate));
if(seq->num_motors()!=this->get_num_motors())
{
this->motion_access.exit();
/* handle exceptions */
throw CDynamixelMotionSequenceException(_HERE_,"The number of motors in the loaded sequence does not coincide with the number of motors in the associated motor group");
}
else
{
// erase the previous sequence
this->seq.clear();
for(iterator=seq->step().begin();iterator!=seq->step().end();iterator++)
{
if(iterator->mode()=="absolute")
step.absolute_motion=true;
else
step.absolute_motion=false;
step.delay=iterator->delay();
step.position.clear();
step.position=iterator->position();
step.velocity.clear();
step.velocity=iterator->velocity();
this->seq.push_back(step);
}
this->sequence_state=mtn_loaded;
this->sequence_current_step=0;
}
this->motion_access.exit();
}catch (const xml_schema::exception& e){
this->sequence_state=mtn_empty;
this->motion_access.exit();
/* handle exceptions */
std::ostringstream os;
os << e;
throw CDynamixelMotionSequenceException(_HERE_,os.str());
}
}
else
CDynamixelMotionSequenceException(_HERE_,"The motion sequence file does not exist");
}
void CDynamixelMotorGroup::save_sequence(std::string &filename)
{
xml_schema::namespace_infomap map;
unsigned int i=0,j=0;
std::string mode;
try{
// save the base class parameters
std::ofstream output_file(filename.c_str(),std::ios_base::out);
dyn_sequence_t sequence_file(this->get_num_motors());
dyn_sequence_t::step_sequence& motion_sequence(sequence_file.step());
for(i=0;i<this->seq.size();i++)
{
if(this->seq[i].absolute_motion)
mode="absolute";
else
mode="relative";
step_t step(mode,this->seq[i].delay);
step_t::position_sequence& position(step.position());
step_t::velocity_sequence& velocity(step.velocity());
for(j=0;j<this->get_num_motors();j++)
{
position.push_back(this->seq[i].position[j]);
velocity.push_back(this->seq[i].velocity[j]);
}
motion_sequence.push_back(step);
}
map[""].name="";
map[""].schema="motion_sequence.xsd";
dyn_motion_seq(output_file,sequence_file,map);
}catch (const xml_schema::exception& e){
std::ostringstream os;
os << e;
/* handle exceptions */
throw CDynamixelMotorException(_HERE_,os.str());
}
}
#endif
void CDynamixelMotorGroup::set_compliance_control(std::vector<TDynamixel_compliance> &config)
......@@ -1687,296 +1409,15 @@ void CDynamixelMotorGroup::set_punch(std::vector<unsigned short int> &punches)
}
}
TDynamixelMotionStates CDynamixelMotorGroup::get_sequence_current_state(void)
{
return this->sequence_state;
}
control_mode CDynamixelMotorGroup::get_control_mode(void)
{
return this->mode;
}
void CDynamixelMotorGroup::add_sequence_step(std::vector<double> &pos,std::vector<double> &vel, double delay, bool abs_motion)
{
TDynamixelMotionStep step;
if(pos.size()!=this->get_num_motors())
{
/* handle exceptions */
throw CDynamixelMotionSequenceException(_HERE_,"The position vector has a wrong length");
}
if(vel.size()!=this->get_num_motors())
{
/* handle exceptions */
throw CDynamixelMotionSequenceException(_HERE_,"The velocity vector has a wrong length");
}
if(delay<0.0)
{
/* handle exceptions */
throw CDynamixelMotionSequenceException(_HERE_,"Negative delay time");
}
this->motion_access.enter();
step.position=pos;
step.velocity=vel;
step.delay=delay;
step.absolute_motion=abs_motion;
this->seq.push_back(step);
if(this->sequence_state!=mtn_loaded)
{
this->sequence_state=mtn_loaded;
this->sequence_current_step=0;
}
this->motion_access.exit();
}
void CDynamixelMotorGroup::insert_sequence_step(unsigned int index,std::vector<double> &pos,std::vector<double> &vel, double delay, bool abs_motion)
{
std::vector<TDynamixelMotionStep> sequence;
TDynamixelMotionStep step;
unsigned int i;
if(index < 0 || index > this->seq.size())
{
/* handle exceptions */
throw CDynamixelMotionSequenceException(_HERE_,"The index is out of range");
}
if(pos.size()!=this->get_num_motors())
{
/* handle exceptions */
throw CDynamixelMotionSequenceException(_HERE_,"The position vector has a wrong length");
}
if(vel.size()!=this->get_num_motors())
{
/* handle exceptions */
throw CDynamixelMotionSequenceException(_HERE_,"The velocity vector has a wrong length");
}
if(delay<0.0)
{
/* handle exceptions */
throw CDynamixelMotionSequenceException(_HERE_,"Negative delay time");
}
this->motion_access.enter();
step.position=pos;
step.velocity=vel;
step.delay=delay;
step.absolute_motion=abs_motion;
sequence=this->seq;
this->seq.clear();
for(i=0;i<sequence.size();i++)
{
if(i==index)
this->seq.push_back(step);
this->seq.push_back(sequence[i]);
}
if(this->sequence_state!=mtn_loaded)
{
this->sequence_state=mtn_loaded;
this->sequence_current_step=0;
}
this->motion_access.exit();
}
void CDynamixelMotorGroup::delete_sequence_step(unsigned int index)
{
std::vector<TDynamixelMotionStep> sequence;
unsigned int i;
if(index < 0 || index > this->seq.size())
{
/* handle exceptions */
throw CDynamixelMotionSequenceException(_HERE_,"The index is out of range");
}
this->motion_access.enter();
sequence=this->seq;
this->seq.clear();
for(i=0;i<sequence.size();i++)
{
if(i!=index)
this->seq.push_back(sequence[i]);
}
if(this->seq.size()==0)
this->sequence_state=mtn_empty;
this->motion_access.exit();
}
void CDynamixelMotorGroup::clear_sequence(void)
{
this->stop_sequence();
this->motion_access.enter();
this->seq.clear();
this->sequence_state=mtn_empty;
this->motion_access.exit();
}
void CDynamixelMotorGroup::start_sequence(void)
{
this->motion_access.enter();
switch(this->sequence_state)
{
case mtn_empty: /* handle exceptions */
//this->motion_access.exit();
//throw CMotionSequenceException(_HERE_,"There is no motion sequence loaded");
break;
case mtn_loaded: // enable the motors
this->enable();
this->sequence_current_step=0;
if(this->event_server->event_is_set(this->sequence_complete_event_id))
this->event_server->reset_event(this->sequence_complete_event_id);
if(this->event_server->event_is_set(this->sequence_error_event_id))
this->event_server->reset_event(this->sequence_error_event_id);
if(!this->event_server->event_is_set(this->start_sequence_event_id))
this->event_server->set_event(this->start_sequence_event_id);
this->sequence_state=mtn_started;
break;
case mtn_started: /* do nothing */
break;
case mtn_paused: /* handle exceptions */
//this->motion_access.exit();
//throw CMotionSequenceException(_HERE_,"A sequence is currently pause, stop it before starting a new one.");
break;
}
this->motion_access.exit();
}
void CDynamixelMotorGroup::pause_sequence(void)
{
this->motion_access.enter();
switch(this->sequence_state)
{
case mtn_empty: /* handle exceptions */
//this->motion_access.exit();
//throw CMotionSequenceException(_HERE_,"There is no motion sequence loaded");
break;
case mtn_loaded: /* handle exceptions */
//this->motion_access.exit();
//throw CMotionSequenceException(_HERE_,"There is no motion sequence currently being executed");
break;
case mtn_started: if(!this->event_server->event_is_set(this->pause_sequence_event_id))
this->event_server->set_event(this->pause_sequence_event_id);
this->sequence_state=mtn_paused;
break;
case mtn_paused: /* do nothing */
break;
}
this->motion_access.exit();
}
void CDynamixelMotorGroup::resume_sequence(void)
{
this->motion_access.enter();
switch(this->sequence_state)
{
case mtn_empty: /* handle exceptions */
//this->motion_access.exit();
//throw CMotionSequenceException(_HERE_,"There is no motion sequence loaded");
break;
case mtn_loaded: /* handle exceptions */
//this->motion_access.exit();
//throw CMotionSequenceException(_HERE_,"There is no motion sequence currently being executed");
break;
case mtn_started: /* do nothing */
break;
case mtn_paused: if(!this->event_server->event_is_set(this->resume_sequence_event_id))
this->event_server->set_event(this->resume_sequence_event_id);
this->sequence_state=mtn_started;
break;
}
this->motion_access.exit();
}
void CDynamixelMotorGroup::stop_sequence(void)
{
this->motion_access.enter();
switch(this->sequence_state)
{
case mtn_empty: /* handle exceptions */
//this->motion_access.exit();
//throw CMotionSequenceException(_HERE_,"There is no motion sequence loaded");
break;
case mtn_loaded: /* handle exceptions */
//this->motion_access.exit();
//throw CMotionSequenceException(_HERE_,"There is no motion sequence currently being executed");
break;
case mtn_started: if(!this->event_server->event_is_set(this->stop_sequence_event_id))
this->event_server->set_event(this->stop_sequence_event_id);
this->sequence_state=mtn_loaded;
break;
case mtn_paused: if(!this->event_server->event_is_set(this->stop_sequence_event_id))
this->event_server->set_event(this->stop_sequence_event_id);
this->sequence_state=mtn_loaded;
break;
}
this->motion_access.exit();
}
std::string CDynamixelMotorGroup::get_sequence_complete_event_id(void)
{
return this->sequence_complete_event_id;
}
std::string CDynamixelMotorGroup::get_sequence_error_event_id(void)
{
return this->sequence_error_event_id;
}
std::string CDynamixelMotorGroup::get_sequence_error_message(void)
{
return this->sequence_error_msg;
}
int CDynamixelMotorGroup::get_sequence_current_step(void)
{
return this->sequence_current_step;
}
double CDynamixelMotorGroup::get_sequence_completed_percentage(void)
{
double ratio;
if(this->sequence_state!=mtn_empty)
ratio=((double)this->sequence_current_step*100.0)/(this->seq.size());
else
{
/* handle errors */
throw CDynamixelMotionSequenceException(_HERE_,"There is no motion sequence loaded");
}
return ratio;
}
CDynamixelMotorGroup::~CDynamixelMotorGroup()
{
/* stop the motor */
try{
this->stop();
/* stop any active sequence */
if(!this->event_server->event_is_set(this->sequence_complete_event_id))
this->stop_sequence();
this->disable();
}catch(CException &e){
/* do nothing */
}
/* stop the internal trhead */
this->event_server->set_event(this->finish_thread_event_id);
this->thread_server->end_thread(this->sequence_thread_id);
this->thread_server->detach_thread(this->sequence_thread_id);
this->thread_server->delete_thread(this->sequence_thread_id);
this->sequence_thread_id="";
this->event_server->set_event(this->finish_thread_event_id);
this->event_server->delete_event(this->finish_thread_event_id);
this->finish_thread_event_id="";
this->event_server->delete_event(this->start_sequence_event_id);
this->start_sequence_event_id="";
this->event_server->delete_event(this->stop_sequence_event_id);
this->stop_sequence_event_id="";
this->event_server->delete_event(this->pause_sequence_event_id);
this->pause_sequence_event_id="";
this->event_server->delete_event(this->resume_sequence_event_id);
this->resume_sequence_event_id="";
this->event_server->delete_event(this->sequence_complete_event_id);
this->sequence_complete_event_id="";
this->event_server->delete_event(this->sequence_error_event_id);
this->sequence_error_event_id="";
this->clear();
}
src/dynamixel_motor_group.h
+ 0
− 192
View file @ 5bdf7a7f
......@@ -5,8 +5,6 @@
#include "mutex.h"
#include <vector>
typedef enum {mtn_empty,mtn_loaded,mtn_started,mtn_paused} TDynamixelMotionStates;
typedef struct
{
std::string bus_id;
......@@ -18,14 +16,6 @@ typedef struct
std::vector<TDynamixel_compliance> compliance_control;
}TDynamixelGroup_config;
typedef struct
{
std::vector<double> position;
std::vector<double> velocity;
bool absolute_motion;
double delay;
}TDynamixelMotionStep;
class CDynamixelMotorGroup
{
private:
......@@ -69,58 +59,6 @@ class CDynamixelMotorGroup
*
*/
control_mode mode;
/* thread attributes */
/**
* \brief
*
*/
CThreadServer *thread_server;
/**
* \brief
*
*/
std::string sequence_thread_id;
/* event attributes */
/**
* \brief
*
*/
CEventServer *event_server;
/**
* \brief
*
*/
std::string finish_thread_event_id;
/**
* \brief
*
*/
std::string sequence_complete_event_id;
/**
* \brief
*
*/
std::string sequence_error_event_id;
/**
* \brief
*
*/
std::string start_sequence_event_id;
/**
* \brief
*
*/
std::string stop_sequence_event_id;
/**
* \brief
*
*/
std::string pause_sequence_event_id;
/**
* \brief
*
*/
std::string resume_sequence_event_id;
/**
* \brief
*
......@@ -137,32 +75,6 @@ class CDynamixelMotorGroup
*
*/
CDynamixelServer *dyn_server;
/* Motion sequence attributes */
/**
* \brief
*
*/
TDynamixelMotionStates sequence_state;
/**
* \brief
*
*/
std::vector<TDynamixelMotionStep> seq;
/**
* \brief
*
*/
int sequence_current_step;
/**
* \brief
*
*/
std::string sequence_error_msg;
/**
* \brief
*
*/
CMutex motion_access;
/**
* \brief
*
......@@ -258,16 +170,6 @@ class CDynamixelMotorGroup
*
*/
void init_motor(unsigned int id,dyn_version_t version=dyn_version1);
/**
* \brief
*
*/
void init_threads(void);
/**
* \brief
*
*/
void init_events(void);
/**
* \brief
*
......@@ -278,17 +180,6 @@ class CDynamixelMotorGroup
*
*/
void set_control_mode(control_mode mode);
/* motion sequence protected functions */
/**
* \brief
*
*/
static void *sequence_thread(void *param);
/**
* \brief
*
*/
int load_motion(unsigned int step_id);
public:
/**
* \brief
......@@ -413,89 +304,6 @@ class CDynamixelMotorGroup
*
*/
control_mode get_control_mode(void);
/* motion sequence public functions */
/**
* \brief
*
*/
TDynamixelMotionStates get_sequence_current_state(void);
#ifdef _HAVE_XSD
/**
* \brief
*
*/
void load_sequence(std::string &filename);
/**
* \brief
*
*/
void save_sequence(std::string &filename);
#endif
/**
* \brief
*
*/
void add_sequence_step(std::vector<double> &pos,std::vector<double> &vel,double delay=0.0,bool abs_motion=true);
/**
* \brief
*
*/
void insert_sequence_step(unsigned int index,std::vector<double> &pos,std::vector<double> &vel,double delay=0.0,bool abs_motion=true);
/**
* \brief
*
*/
void delete_sequence_step(unsigned int index);
/**
* \brief
*
*/
void clear_sequence(void);
/**
* \brief
*
*/
void start_sequence(void);
/**
* \brief
*
*/
void pause_sequence(void);
/**
* \brief
*
*/
void resume_sequence(void);
/**
* \brief
*
*/
void stop_sequence(void);
/**
* \brief
*
*/
std::string get_sequence_complete_event_id(void);
/**
* \brief
*
*/
std::string get_sequence_error_event_id(void);
/**
* \brief
*
*/
std::string get_sequence_error_message(void);
/**
* \brief
*
*/
int get_sequence_current_step(void);
/**
* \brief
*
*/
double get_sequence_completed_percentage(void);
/**
* \brief
*
......
src/examples/test_dynamixel_motion_seq.cpp
+ 8
− 4
View file @ 5bdf7a7f
......@@ -4,6 +4,7 @@
#include "exceptions.h"
#include "dynamixel_motor_group.h"
#include "dynamixel_motor.h"
#include "trajectory.h"
#include <iostream>
#include <math.h>
......@@ -19,17 +20,20 @@ int main(int argc, char *argv[])
CEventServer *event_server=CEventServer::instance();
std::vector<double> angles,speeds;
CDynamixelMotorGroup group(group_name);
CTrajectory<CDynamixelMotorGroup> dyn_traj("dyn_traj",&group,&CDynamixelMotorGroup::stop,&CDynamixelMotorGroup::get_current_angle,
&CDynamixelMotorGroup::move_absolute_angle,&CDynamixelMotorGroup::move_relative_angle,
&CDynamixelMotorGroup::get_num_motors);
try{
if(dyn_server->get_num_buses()>0)
{
#ifdef _HAVE_XSD
group.load_config(config_file);
group.load_sequence(sequence_filename);
group.start_sequence();
while(!event_server->event_is_set(group.get_sequence_complete_event_id()))
dyn_traj.load_trajectory(sequence_filename);
dyn_traj.start_trajectory();
while(!event_server->event_is_set(dyn_traj.get_trajectory_complete_event_id()))
{
std::cout << group.get_sequence_completed_percentage() << "% of sequence completed" << std::endl;
std::cout << dyn_traj.get_trajectory_completed_percentage() << "% of trajectory completed" << std::endl;
usleep(200000);
}
#endif
......
src/examples/test_dynamixel_motor.cpp
+ 8
− 8
View file @ 5bdf7a7f
......@@ -70,21 +70,21 @@ int main(int argc, char *argv[])
sleep(1);
cont->turn_led_off();
return 1;
// cont->move_absolute_angle(-90,300);
// cont->move_absolute_angle(-90,300,0);
// sleep(2);
// cont->move_absolute_angle(90,100);
// cont->move_absolute_angle(90,100,0);
// sleep(2);
// cont->move_relative_angle(-5,100);
// cont->move_relative_angle(-5,100,0);
// sleep(1);
// cont->move_relative_angle(-5,100);
// cont->move_relative_angle(-5,100,0);
// sleep(1);
// cont->move_relative_angle(-5,100);
// cont->move_relative_angle(-5,100,0);
// sleep(1);
// cont->move_relative_angle(-5,100);
// cont->move_relative_angle(-5,100,0);
// sleep(1);
// cont->move_relative_angle(-5,100);
// cont->move_relative_angle(-5,100,0);
// sleep(1);
// cont->move_relative_angle(-5,100);
// cont->move_relative_angle(-5,100,0);
// sleep(1);
// cont->move_torque(50);
// sleep(5);
......
src/xml/CMakeLists.txt
+ 1
− 1
View file @ 5bdf7a7f
......@@ -13,7 +13,7 @@ ENDIF(EXISTS "/usr/include/xsd/cxx")
IF(XSD_FOUND)
SET(XSD_LIBRARY ${XSD_LIBRARY} PARENT_SCOPE)
SET(XSD_FILES dynamixel_motor_cfg_file.xsd dynamixel_motor_group_cfg_file.xsd dyn_motion_seq_file.xsd)
SET(XSD_FILES dynamixel_motor_cfg_file.xsd dynamixel_motor_group_cfg_file.xsd)
IF(XSD_FILES)
SET(XSD_PATH ${CMAKE_CURRENT_SOURCE_DIR})
......
src/xml/dyn_group_config.xml
+ 3
− 3
View file @ 5bdf7a7f
......@@ -3,14 +3,14 @@
<dyn_motor_group_config xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
xsi:noNamespaceSchemaLocation="dyn_motor_group_cfg_file.xsd">
<bus_id>A400gaIt</bus_id>
<bus_id>A400gavq</bus_id>
<baudrate>1000000</baudrate>
<dyn_motor_config>
<id>1</id>
<id>2</id>
<config_file>dyn_config.xml</config_file>
</dyn_motor_config>
<dyn_motor_config>
<id>2</id>
<id>3</id>
<config_file>dyn_config.xml</config_file>
</dyn_motor_config>
</dyn_motor_group_config>
src/xml/sequence1.xml
+ 44
− 28
View file @ 5bdf7a7f
<?xml version="1.0"?>
<dyn_motion_seq xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
xsi:noNamespaceSchemaLocation="dyn_motion_seq_file.xsd">
<trajectory xmlns:xsi="http://www.w3.org/10.01/XMLSchema-instance"
xsi:noNamespaceSchemaLocation="motion_traj_file.xsd">
<num_motors>2</num_motors>
<step>
<position>0.0</position>
<position>0.0</position>
<velocity>100.0</velocity>
<velocity>100.0</velocity>
<position>10.0</position>
<position>30.0</position>
<velocity>10.0</velocity>
<velocity>10.0</velocity>
<mode>absolute</mode>
<delay>0</delay>
</step>
<step>
<position>-10.0</position>
<position>-30.0</position>
<velocity>10.0</velocity>
<velocity>10.0</velocity>
<mode>absolute</mode>
<delay>0</delay>
</step>
<step>
<position>10.0</position>
<position>30.0</position>
<position>30.0</position>
<velocity>100.0</velocity>
<velocity>100.0</velocity>
<velocity>10.0</velocity>
<velocity>10.0</velocity>
<mode>absolute</mode>
<delay>0</delay>
</step>
<step>
<position>-10.0</position>
<position>-30.0</position>
<position>-30.0</position>
<velocity>100.0</velocity>
<velocity>100.0</velocity>
<velocity>10.0</velocity>
<velocity>10.0</velocity>
<mode>absolute</mode>
<delay>0</delay>
</step>
<step>
<position>20.0</position>
<position>20.0</position>
<velocity>20.0</velocity>
<velocity>20.0</velocity>
<position>60.0</position>
<velocity>10.0</velocity>
<velocity>10.0</velocity>
<mode>relative</mode>
<delay>0</delay>
</step>
<step>
<position>20.0</position>
<position>20.0</position>
<velocity>20.0</velocity>
<velocity>20.0</velocity>
<position>-20.0</position>
<position>-60.0</position>
<velocity>10.0</velocity>
<velocity>10.0</velocity>
<mode>relative</mode>
<delay>0</delay>
</step>
<step>
<position>20.0</position>
<position>20.0</position>
<velocity>20.0</velocity>
<velocity>20.0</velocity>
<mode>relative</mode>
<position>10.0</position>
<position>30.0</position>
<velocity>10.0</velocity>
<velocity>10.0</velocity>
<mode>absolute</mode>
<delay>0</delay>
</step>
<step>
<position>-10.0</position>
<position>-30.0</position>
<position>-30.0</position>
<velocity>100.0</velocity>
<velocity>100.0</velocity>
<velocity>10.0</velocity>
<velocity>10.0</velocity>
<mode>absolute</mode>
<delay>0</delay>
</step>
<step>
<position>0.0</position>
<position>0.0</position>
<velocity>10.0</velocity>
<velocity>10.0</velocity>
<mode>absolute</mode>
<delay>0</delay>
</step>
</dyn_motion_seq>
</trajectory>
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment