From 35c682fa52473e9a319faf2fedf41f26f9d2cd1d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Sergi=20Hern=C3=A0ndez=20Juan?= <shernand@iri.upc.edu> Date: Thu, 4 Jun 2015 08:13:49 +0000 Subject: [PATCH] Added the option to use move functions without acceleration for those controller that can't directly control it. --- src/trajectory.h | 77 ++++++++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 75 insertions(+), 2 deletions(-) diff --git a/src/trajectory.h b/src/trajectory.h index 6cdbc0a..063df34 100644 --- a/src/trajectory.h +++ b/src/trajectory.h @@ -97,6 +97,8 @@ class CTrajectory typedef void (T::*get_current_angles_fnct_ptr)(std::vector<double> &angles); typedef void (T::*move_absolute_angles_fnct_ptr)(std::vector<double> &angles,std::vector<double> &speed,std::vector<double> &accel); typedef void (T::*move_relative_angles_fnct_ptr)(std::vector<double> &angles,std::vector<double> &speed,std::vector<double> &accel); + typedef void (T::*move_absolute_angles_no_accel_fnct_ptr)(std::vector<double> &angles,std::vector<double> &speed); + typedef void (T::*move_relative_angles_no_accel_fnct_ptr)(std::vector<double> &angles,std::vector<double> &speed); typedef unsigned int (T::*get_num_motors_fnct_ptr)(void); /** * \brief @@ -118,6 +120,16 @@ class CTrajectory * */ move_relative_angles_fnct_ptr move_relative_angles_fnct; + /** + * \brief + * + */ + move_absolute_angles_no_accel_fnct_ptr move_absolute_angles_no_accel_fnct; + /** + * \brief + * + */ + move_relative_angles_no_accel_fnct_ptr move_relative_angles_no_accel_fnct; /** * \brief * @@ -436,14 +448,18 @@ class CTrajectory { if(this->move_absolute_angles_fnct!=NULL) (this->motor_control->*this->move_absolute_angles_fnct)(position,velocity,acceleration); - else + else if(this->move_absolute_angles_no_accel_fnct!=NULL) + (this->motor_control->*this->move_absolute_angles_no_accel_fnct)(position,velocity); + else throw CTrajectoryException(_HERE_,"move_absolute_angles function not defined"); } else { if(this->move_relative_angles_fnct!=NULL) (this->motor_control->*this->move_relative_angles_fnct)(position,velocity,acceleration); - else + else if(this->move_relative_angles_no_accel_fnct!=NULL) + (this->motor_control->*this->move_relative_angles_no_accel_fnct)(position,velocity); + else throw CTrajectoryException(_HERE_,"move_relative_angles function not defined"); } max=time[0]; @@ -498,6 +514,43 @@ class CTrajectory this->set_move_relative_angles_function(move_relative_angles_fnct); this->set_get_num_motors_function(get_num_motors_fnct); }; + 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, + get_num_motors_fnct_ptr get_num_motors_fnct=NULL) + { + this->trajectory_state=mtn_empty; + this->trajectory_current_step=-1; + this->trajectory_error_msg="Motion trajectory ended successfully"; + this->event_server=CEventServer::instance(); + this->finish_thread_event_id=traj_id+"_finish_thread_event"; + this->event_server->create_event(this->finish_thread_event_id); + this->trajectory_complete_event_id=traj_id+"_trajectory_complete_event"; + this->event_server->create_event(this->trajectory_complete_event_id); + this->event_server->set_event(this->trajectory_complete_event_id); + this->trajectory_error_event_id=traj_id+"_trajectory_error_event"; + this->event_server->create_event(this->trajectory_error_event_id); + this->start_trajectory_event_id=traj_id+"_start_trajectory_event"; + this->event_server->create_event(this->start_trajectory_event_id); + this->stop_trajectory_event_id=traj_id+"_stop_trajectory_event"; + this->event_server->create_event(this->stop_trajectory_event_id); + this->pause_trajectory_event_id=traj_id+"_pause_trajectory_event"; + this->event_server->create_event(this->pause_trajectory_event_id); + this->resume_trajectory_event_id=traj_id+"_resume_trajectory_event"; + this->event_server->create_event(this->resume_trajectory_event_id); + this->thread_server=CThreadServer::instance(); + 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); + this->set_get_current_angles_function(get_current_angles_fnct); + 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); + }; + /** * \brief * @@ -957,6 +1010,7 @@ class CTrajectory */ void set_move_absolute_angles_function(move_absolute_angles_fnct_ptr fnct) { + this->move_absolute_angles_no_accel_fnct=NULL; this->move_absolute_angles_fnct=fnct; } /** @@ -965,8 +1019,27 @@ class CTrajectory */ void set_move_relative_angles_function(move_relative_angles_fnct_ptr fnct) { + this->move_relative_angles_no_accel_fnct=NULL; this->move_relative_angles_fnct=fnct; } + /** + * \brief + * + */ + void set_move_absolute_angles_function(move_absolute_angles_no_accel_fnct_ptr fnct) + { + this->move_absolute_angles_no_accel_fnct=fnct; + this->move_absolute_angles_fnct=NULL; + } + /** + * \brief + * + */ + void set_move_relative_angles_function(move_relative_angles_no_accel_fnct_ptr fnct) + { + this->move_relative_angles_no_accel_fnct=fnct; + this->move_relative_angles_fnct=NULL; + } /** * \brief * -- GitLab