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