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

Removed the static variables from some functions. 

Implemented the thread in a non-static function.
Tested the execution of several CTrajectory modules at the same time.
parent 35c682fa
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/examples/trajectory_test.cpp
+ 10
− 6
View file @ 00e92d3a
......@@ -46,16 +46,20 @@ int main(int argc, char *argv[])
{
CMotorTest motor;
CEventServer *event_server=CEventServer::instance();
CTrajectory<CMotorTest> traj("trajectory",&motor,&CMotorTest::stop,&CMotorTest::get_current_angles,&CMotorTest::move_absolute_angles,
&CMotorTest::move_relative_angles,&CMotorTest::get_num_motors);
CTrajectory<CMotorTest> traj1("trajectory1",&motor,&CMotorTest::stop,&CMotorTest::get_current_angles,&CMotorTest::move_absolute_angles,
&CMotorTest::move_relative_angles,&CMotorTest::get_num_motors);
CTrajectory<CMotorTest> traj2("trajectory2",&motor,&CMotorTest::stop,&CMotorTest::get_current_angles,&CMotorTest::move_absolute_angles,
&CMotorTest::move_relative_angles,&CMotorTest::get_num_motors);
try{
#ifdef _HAVE_XSD
traj.load_trajectory(sequence_filename);
traj.start_trajectory();
while(!event_server->event_is_set(traj.get_trajectory_complete_event_id()))
traj1.load_trajectory(sequence_filename);
traj1.start_trajectory();
traj2.load_trajectory(sequence_filename);
traj2.start_trajectory();
while(!event_server->event_is_set(traj1.get_trajectory_complete_event_id()))
{
std::cout << traj.get_trajectory_completed_percentage() << "% of sequence completed" << std::endl;
std::cout << traj1.get_trajectory_completed_percentage() << "% of sequence completed" << std::endl;
usleep(200000);
}
#endif
......
src/trajectory.cpp
+ 0
− 2
View file @ 00e92d3a
#include "trajectory.h"
src/trajectory.h
+ 172
− 127
View file @ 00e92d3a
......@@ -32,6 +32,11 @@ template <class T>
class CTrajectory
{
private:
/**
* \brief
*
*/
std::string traj_id;
/**
* \brief
*
......@@ -135,6 +140,27 @@ class CTrajectory
*
*/
get_num_motors_fnct_ptr get_num_motors_fnct;
/**
* \brief
*
*/
std::vector<int> time;
/**
* \brief
*
*/
std::vector<double> current_position;
/**
* \brief
*
*/
std::vector<double> last_speeds;
/**
* \brief
*
*/
std::vector<int> dir;
traj_t *int_traj;
protected:
/**
* \brief
......@@ -163,96 +189,102 @@ class CTrajectory
static void *trajectory_thread(void *param)
{
CTrajectory *mtn_seq=(CTrajectory *)param;
mtn_seq->do_trajectory();
pthread_exit(NULL);
};
void do_trajectory(void)
{
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_trajectory_event_id);
events.push_back(mtn_seq->stop_trajectory_event_id);
events.push_back(mtn_seq->resume_trajectory_event_id);
events.push_back(this->finish_thread_event_id);
events.push_back(this->start_trajectory_event_id);
events.push_back(this->stop_trajectory_event_id);
events.push_back(this->resume_trajectory_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->trajectory_current_step++;
to=mtn_seq->load_motion(mtn_seq->trajectory_current_step);
break;
case 0: /* finish the thread */
end=true;
break;
case 1: /* start trajectory */
to=mtn_seq->load_motion(mtn_seq->trajectory_current_step);
break;
case 2: /* stop trajectory */
if(mtn_seq->stop_fnct!=NULL)
(mtn_seq->motor_control->*mtn_seq->stop_fnct)();
else
throw CTrajectoryException(_HERE_,"Stop function not defined");
mtn_seq->trajectory_error_msg="Motion trajectory stopped by user";
mtn_seq->trajectory_current_step=0;
to=-1;
if(!mtn_seq->event_server->event_is_set(mtn_seq->trajectory_complete_event_id))
mtn_seq->event_server->set_event(mtn_seq->trajectory_complete_event_id);
break;
case 3: /* resume trajectory */
mtn_seq->trajectory_current_step++;
to=mtn_seq->load_motion(mtn_seq->trajectory_current_step);
break;
}
mtn_seq->motion_access.exit();
}catch(CEventTimeoutException &e){
if(!mtn_seq->event_server->event_is_set(mtn_seq->pause_trajectory_event_id))
{
/* load and execute the next step */
mtn_seq->motion_access.enter();
mtn_seq->trajectory_current_step++;
try{
to=mtn_seq->load_motion(mtn_seq->trajectory_current_step);
}catch(CException &e){
try{
event_id=this->event_server->wait_first(events,to);
this->motion_access.enter();
switch(event_id)
{
case -1: this->trajectory_current_step++;
to=this->load_motion(this->trajectory_current_step);
break;
case 0: /* finish the thread */
end=true;
break;
case 1: /* start trajectory */
to=this->load_motion(this->trajectory_current_step);
break;
case 2: /* stop trajectory */
if(this->stop_fnct!=NULL)
(this->motor_control->*this->stop_fnct)();
else
throw CTrajectoryException(_HERE_,"Stop function not defined");
this->trajectory_error_msg="Motion trajectory stopped by user";
this->trajectory_current_step=0;
to=-1;
if(!this->event_server->event_is_set(this->trajectory_complete_event_id))
this->event_server->set_event(this->trajectory_complete_event_id);
break;
case 3: /* resume trajectory */
this->trajectory_current_step++;
to=this->load_motion(this->trajectory_current_step);
break;
}
this->motion_access.exit();
}catch(CEventTimeoutException &e){
if(!this->event_server->event_is_set(this->pause_trajectory_event_id))
{
/* load and execute the next step */
this->motion_access.enter();
this->trajectory_current_step++;
try{
to=this->load_motion(this->trajectory_current_step);
}catch(CException &e){
std::cout << e.what() << std::endl;
to=-1;
if(mtn_seq->stop_fnct!=NULL)
(mtn_seq->motor_control->*mtn_seq->stop_fnct)();
else
throw CTrajectoryException(_HERE_,"Stop function not defined");
mtn_seq->trajectory_state=mtn_loaded;
mtn_seq->trajectory_error_msg=e.what();
// signal the user
if(!mtn_seq->event_server->event_is_set(mtn_seq->trajectory_error_event_id))
mtn_seq->event_server->set_event(mtn_seq->trajectory_error_event_id);
if(!mtn_seq->event_server->event_is_set(mtn_seq->trajectory_complete_event_id))
mtn_seq->event_server->set_event(mtn_seq->trajectory_complete_event_id);
}
mtn_seq->motion_access.exit();
}
else
{
mtn_seq->event_server->reset_event(mtn_seq->pause_trajectory_event_id);
to=-1;
}
}catch(CException &e){
to=-1;
if(this->stop_fnct!=NULL)
(this->motor_control->*this->stop_fnct)();
else
throw CTrajectoryException(_HERE_,"Stop function not defined");
this->trajectory_state=mtn_loaded;
this->trajectory_error_msg=e.what();
// signal the user
if(!this->event_server->event_is_set(this->trajectory_error_event_id))
this->event_server->set_event(this->trajectory_error_event_id);
if(!this->event_server->event_is_set(this->trajectory_complete_event_id))
this->event_server->set_event(this->trajectory_complete_event_id);
}
this->motion_access.exit();
}
else
{
this->event_server->reset_event(this->pause_trajectory_event_id);
to=-1;
}
}catch(CException &e){
std::cout << e.what() << std::endl;
to=-1;
mtn_seq->motion_access.exit();
if(mtn_seq->stop_fnct!=NULL)
(mtn_seq->motor_control->*mtn_seq->stop_fnct)();
mtn_seq->trajectory_state=mtn_loaded;
mtn_seq->trajectory_error_msg=e.what();
// signal the user
if(!mtn_seq->event_server->event_is_set(mtn_seq->trajectory_error_event_id))
mtn_seq->event_server->set_event(mtn_seq->trajectory_error_event_id);
if(!mtn_seq->event_server->event_is_set(mtn_seq->trajectory_complete_event_id))
mtn_seq->event_server->set_event(mtn_seq->trajectory_complete_event_id);
}
to=-1;
this->motion_access.exit();
if(this->stop_fnct!=NULL)
(this->motor_control->*this->stop_fnct)();
this->trajectory_state=mtn_loaded;
this->trajectory_error_msg=e.what();
// signal the user
if(!this->event_server->event_is_set(this->trajectory_error_event_id))
this->event_server->set_event(this->trajectory_error_event_id);
if(!this->event_server->event_is_set(this->trajectory_complete_event_id))
this->event_server->set_event(this->trajectory_complete_event_id);
}
}
pthread_exit(NULL);
};
/**
* \brief
......@@ -299,11 +331,6 @@ class CTrajectory
*/
int load_motion(unsigned int step_id)
{
static std::vector<int> time((this->motor_control->*this->get_num_motors_fnct)(),0);
static std::vector<double> current_position((this->motor_control->*this->get_num_motors_fnct)(),0.0);
static std::vector<double> last_speeds((this->motor_control->*this->get_num_motors_fnct)(),0.0);
static std::vector<int> dir((this->motor_control->*this->get_num_motors_fnct)(),1);
std::vector<double> position((this->motor_control->*this->get_num_motors_fnct)());
std::vector<double> velocity((this->motor_control->*this->get_num_motors_fnct)());
std::vector<double> acceleration((this->motor_control->*this->get_num_motors_fnct)());
......@@ -328,7 +355,7 @@ class CTrajectory
else
{
if(this->get_current_angles_fnct!=NULL)
(this->motor_control->*this->get_current_angles_fnct)(current_position);
(this->motor_control->*this->get_current_angles_fnct)(this->current_position);
else
throw CTrajectoryException(_HERE_,"Get_current_angles function not defined");
if(step_id==0)// first segment
......@@ -336,43 +363,43 @@ class CTrajectory
for(i=0;i<(this->motor_control->*this->get_num_motors_fnct)();i++)
{
if(this->seq[step_id].absolute_motion)
delta_angle=this->seq[step_id].position[i]-current_position[i];
delta_angle=this->seq[step_id].position[i]-this->current_position[i];
else
delta_angle=this->seq[step_id].position[i];
if(fabs(delta_angle)>0.0)
if(fabs(delta_angle)>(pow(this->seq[step_id].velocity[i],2)/this->seq[step_id].acceleration[i]))
{
if(this->seq[step_id].position[i]>=0)
dir[i]=1;
this->dir[i]=1;
else
dir[i]=-1;
this->dir[i]=-1;
t_acc=this->seq[step_id].velocity[i]/this->seq[step_id].acceleration[i];
t_seg=2.0*t_acc+(fabs(delta_angle)-pow(this->seq[step_id].velocity[i],2)/(this->seq[step_id].acceleration[i]))/this->seq[step_id].velocity[i];
if(t_seg<2.0*t_acc)
throw CTrajectoryException(_HERE_,"Invalid motion commands");
if(this->zero_speed(step_id,i,&dir[i]))// it is the last command or there is a delay
if(this->zero_speed(step_id,i,&this->dir[i]))// it is the last command or there is a delay
{
position[i]=this->seq[step_id].position[i];
velocity[i]=this->seq[step_id].velocity[i];
acceleration[i]=this->seq[step_id].acceleration[i];
time[i]=t_seg*1000+this->seq[step_id].delay;
last_speeds[i]=0.0;
this->time[i]=t_seg*1000+this->seq[step_id].delay;
this->last_speeds[i]=0.0;
}
else
{
velocity[i]=delta_angle/(t_seg-t_acc/2.0);
acceleration[i]=velocity[i]/t_acc;
position[i]=this->seq[step_id].position[i]+dir[i]*velocity[i]*t_acc/2.0;
time[i]=t_seg*1000;
last_speeds[i]=velocity[i];
position[i]=this->seq[step_id].position[i]+this->dir[i]*velocity[i]*t_acc/2.0;
this->time[i]=t_seg*1000;
this->last_speeds[i]=velocity[i];
}
}
else
{
position[i]=this->seq[step_id].position[i];
velocity[i]=0.0;
velocity[i]=this->seq[step_id].velocity[i];
acceleration[i]=this->seq[step_id].acceleration[i];
time[i]=0.0;
last_speeds[i]=0.0;
this->time[i]=0.0;
this->last_speeds[i]=0.0;
}
}
}
......@@ -381,66 +408,66 @@ class CTrajectory
for(i=0;i<(this->motor_control->*this->get_num_motors_fnct)();i++)
{
if(this->seq[step_id].absolute_motion)
delta_angle=this->seq[step_id].position[i]-current_position[i];//this->seq[step_id-1].position[i];
delta_angle=this->seq[step_id].position[i]-this->current_position[i];//this->seq[step_id-1].position[i];
else
delta_angle=this->seq[step_id].position[i];
if(fabs(delta_angle)>0.0)
if(fabs(delta_angle)>(pow(this->seq[step_id].velocity[i],2)/this->seq[step_id].acceleration[i]))
{
t_acc=this->seq[step_id].velocity[i]/this->seq[step_id].acceleration[i];
t_seg=2.0*t_acc+(fabs(delta_angle)-pow(this->seq[step_id].velocity[i],2)/(this->seq[step_id].acceleration[i]))/this->seq[step_id].velocity[i];
if(t_seg<2.0*t_acc)
throw CTrajectoryException(_HERE_,"Invalid motion commands");
if(last_speeds[i]==0.0)
if(this->last_speeds[i]==0.0)
{
if(this->zero_speed(step_id,i,&dir[i]))// it is the last command or there is a change in direction or there is a delay
if(this->zero_speed(step_id,i,&this->dir[i]))// it is the last command or there is a change in direction or there is a delay
{
position[i]=this->seq[step_id].position[i];
velocity[i]=this->seq[step_id].velocity[i];
acceleration[i]=this->seq[step_id].acceleration[i];
time[i]=t_seg*1000+this->seq[step_id].delay;
last_speeds[i]=0.0;
this->time[i]=t_seg*1000+this->seq[step_id].delay;
this->last_speeds[i]=0.0;
}
else
{
velocity[i]=(fabs(delta_angle))/(t_seg-(t_acc/2.0));
acceleration[i]=velocity[i]/t_acc;
position[i]=this->seq[step_id].position[i]+dir[i]*velocity[i]*t_acc/2.0;
time[i]=t_seg*1000;
last_speeds[i]=velocity[i];
position[i]=this->seq[step_id].position[i]+this->dir[i]*velocity[i]*t_acc/2.0;
this->time[i]=t_seg*1000;
this->last_speeds[i]=velocity[i];
}
}
else
{
if(this->zero_speed(step_id,i,&dir[i]))// it is the last command or there is a change in direction or there is a delay
if(this->zero_speed(step_id,i,&this->dir[i]))// it is the last command or there is a change in direction or there is a delay
{
a=1.0;
b=2.0*this->seq[step_id].acceleration[i]*(t_seg-3.0*t_acc/2.0);
c=(last_speeds[i]*t_acc/2.0-fabs(delta_angle))*2.0*this->seq[step_id].acceleration[i];
c=(this->last_speeds[i]*t_acc/2.0-fabs(delta_angle))*2.0*this->seq[step_id].acceleration[i];
if((b*b-4*a*c)<0)
throw CTrajectoryException(_HERE_,"Invalid motion commands");
velocity[i]=(-b+sqrt(b*b-4.0*a*c))/2.0;
acceleration[i]=fabs(velocity[i]-last_speeds[i])/t_acc;
acceleration[i]=fabs(velocity[i]-this->last_speeds[i])/t_acc;
position[i]=this->seq[step_id].position[i];
time[i]=t_seg*1000+this->seq[step_id].delay;
last_speeds[i]=0.0;
this->time[i]=t_seg*1000+this->seq[step_id].delay;
this->last_speeds[i]=0.0;
}
else
{
velocity[i]=(fabs(delta_angle)-last_speeds[i]*t_acc/2.0)/(t_seg-t_acc/2.0);
acceleration[i]=fabs(velocity[i]-last_speeds[i])/t_acc;
position[i]=this->seq[step_id].position[i]+dir[i]*velocity[i]*t_acc/2.0;
time[i]=t_seg*1000;
last_speeds[i]=velocity[i];
velocity[i]=(fabs(delta_angle)-this->last_speeds[i]*t_acc/2.0)/(t_seg-t_acc/2.0);
acceleration[i]=fabs(velocity[i]-this->last_speeds[i])/t_acc;
position[i]=this->seq[step_id].position[i]+this->dir[i]*velocity[i]*t_acc/2.0;
this->time[i]=t_seg*1000;
this->last_speeds[i]=velocity[i];
}
}
}
else
{
position[i]=this->seq[step_id].position[i];
velocity[i]=0.0;
velocity[i]=this->seq[step_id].velocity[i];
acceleration[i]=this->seq[step_id].acceleration[i];
time[i]=0.0;
last_speeds[i]=0.0;
this->time[i]=0.0;
this->last_speeds[i]=0.0;
}
}
}
......@@ -462,10 +489,10 @@ class CTrajectory
else
throw CTrajectoryException(_HERE_,"move_relative_angles function not defined");
}
max=time[0];
max=this->time[0];
for(i=1;i<(this->motor_control->*this->get_num_motors_fnct)();i++)
if(time[i]>max)
max=time[i];
if(this->time[i]>max)
max=this->time[i];
return max;
}
};
......@@ -505,7 +532,6 @@ class CTrajectory
this->trajectory_thread_id=traj_id+"_trajectory_thread";
this->thread_server->create_thread(this->trajectory_thread_id);
this->thread_server->attach_thread(this->trajectory_thread_id,this->trajectory_thread,this);
this->thread_server->start_thread(this->trajectory_thread_id);
// initialize motion controller parameters
this->motor_control=motor_control;
this->set_stop_function(stop_fnct);
......@@ -513,6 +539,12 @@ class CTrajectory
this->set_move_absolute_angles_function(move_absolute_angles_fnct);
this->set_move_relative_angles_function(move_relative_angles_fnct);
this->set_get_num_motors_function(get_num_motors_fnct);
this->time=std::vector<int>((this->motor_control->*this->get_num_motors_fnct)(),0);
this->current_position=std::vector<double>((this->motor_control->*this->get_num_motors_fnct)(),0.0);
this->last_speeds=std::vector<double>((this->motor_control->*this->get_num_motors_fnct)(),0.0);
this->dir=std::vector<int>((this->motor_control->*this->get_num_motors_fnct)(),1);
this->thread_server->start_thread(this->trajectory_thread_id);
this->traj_id=traj_id;
};
CTrajectory(const std::string &traj_id,T *motor_control,stop_fnct_ptr stop_fnct=NULL,get_current_angles_fnct_ptr get_current_angles_fnct=NULL,
move_absolute_angles_no_accel_fnct_ptr move_absolute_angles_fnct=NULL,move_relative_angles_no_accel_fnct_ptr move_relative_angles_fnct=NULL,
......@@ -541,7 +573,6 @@ class CTrajectory
this->trajectory_thread_id=traj_id+"_trajectory_thread";
this->thread_server->create_thread(this->trajectory_thread_id);
this->thread_server->attach_thread(this->trajectory_thread_id,this->trajectory_thread,this);
this->thread_server->start_thread(this->trajectory_thread_id);
// initialize motion controller parameters
this->motor_control=motor_control;
this->set_stop_function(stop_fnct);
......@@ -549,6 +580,12 @@ class CTrajectory
this->set_move_absolute_angles_function(move_absolute_angles_fnct);
this->set_move_relative_angles_function(move_relative_angles_fnct);
this->set_get_num_motors_function(get_num_motors_fnct);
this->time=std::vector<int>((this->motor_control->*this->get_num_motors_fnct)(),0);
this->current_position=std::vector<double>((this->motor_control->*this->get_num_motors_fnct)(),0.0);
this->last_speeds=std::vector<double>((this->motor_control->*this->get_num_motors_fnct)(),0.0);
this->dir=std::vector<int>((this->motor_control->*this->get_num_motors_fnct)(),1);
this->thread_server->start_thread(this->trajectory_thread_id);
this->traj_id=traj_id;
};
/**
......@@ -569,6 +606,7 @@ class CTrajectory
traj_t::step_iterator iterator;
TTrajectoryStep step;
struct stat buffer;
unsigned int i=0;
if(stat(filename.c_str(),&buffer)==0)
{
......@@ -584,6 +622,7 @@ class CTrajectory
this->trajectory_state=mtn_loaded;
}
// load the new trajectory
std::cout << "read file " << filename << std::endl;
std::auto_ptr<traj_t> traj(trajectory(filename.c_str(),xml_schema::flags::dont_validate));
if(traj->num_motors()!=(this->motor_control->*this->get_num_motors_fnct)())
{
......@@ -595,7 +634,7 @@ class CTrajectory
{
// erase the previous trajectory
this->seq.clear();
for(iterator=traj->step().begin();iterator!=traj->step().end();iterator++)
for(iterator=traj->step().begin(),i=0;iterator!=traj->step().end();iterator++,i++)
{
if(iterator->mode()=="absolute")
step.absolute_motion=true;
......@@ -615,6 +654,7 @@ class CTrajectory
this->trajectory_state=mtn_loaded;
this->trajectory_current_step=0;
}
std::cout << "done reading file " << filename << std::endl;
this->motion_access.exit();
}catch (const xml_schema::exception& e){
this->trajectory_state=mtn_empty;
......@@ -622,11 +662,16 @@ class CTrajectory
/* handle exceptions */
std::ostringstream os;
os << e;
std::cout << os.str() << std::endl;
throw CTrajectoryException(_HERE_,os.str());
}
}catch(...){
std::cout << "Unknown exception" << std::endl;
}
}
else
{
CTrajectoryException(_HERE_,"The motion trajectory file does not exist");
}
};
/**
* \brief
......
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