X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=modules%2Fdevices%2Frobot%2Ftrajectory_manager%2Ftrajectory_manager.h;h=7cab78569b2fb2fc6cef8cda196d67e1208b3ec5;hp=43730ead7627aaa9ffc1fd7ed36fe9a3df39d79f;hb=d3a38d6cce69bc7debce828b4ecd3a9c0c866e96;hpb=d1cc3debac68c0012f2c5ca33da9bcf0882f4988

diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager.h b/modules/devices/robot/trajectory_manager/trajectory_manager.h
index 43730ea..7cab785 100644
--- a/modules/devices/robot/trajectory_manager/trajectory_manager.h
+++ b/modules/devices/robot/trajectory_manager/trajectory_manager.h
@@ -52,6 +52,10 @@ enum trajectory_state {
 
 	/* line */
 	RUNNING_LINE,
+
+	/* clitoid */
+	RUNNING_CLITOID_LINE,
+	RUNNING_CLITOID_CURVE,
 };
 
 struct circle_target {
@@ -64,10 +68,18 @@ struct circle_target {
 	uint8_t flags;   /**< flags for this trajectory */
 };
 
+/* for line and clitoid */
 struct line_target {
 	line_t line;
 	double angle;
 	double advance;
+
+	/* only for clitoid */
+	point_t turn_pt;
+	double Aa;
+	double Va;
+	double alpha;
+	double R;
 };
 
 struct trajectory {
@@ -86,19 +98,24 @@ struct trajectory {
 			    *    when a_target < a_start */
 	double circle_coef;/**<< corrective circle coef */
 
-	uint16_t d_speed;  /**<< distance speed consign */
-	uint16_t a_speed;  /**<< angle speed consign */
+	double d_speed;  /**<< distance speed consign */
+	double a_speed;  /**<< angle speed consign */
+
+	double d_acc;    /**<< distance acceleration consign */
+	double a_acc;    /**<< angle acceleration consign */
 
 	struct robot_position *position; /**<< associated robot_position */
 	struct robot_system *robot;      /**<< associated robot_system */
 	struct cs *csm_angle;     /**<< associated control system (angle) */
 	struct cs *csm_distance;  /**<< associated control system (distance) */
 
+	double cs_hz;
+
 	int8_t scheduler_task;    /**<< id of current task (-1 if no running task) */
 };
 
 /** structure initialization */
-void trajectory_init(struct trajectory *traj);
+void trajectory_init(struct trajectory *traj, double cs_hz);
 
 /** structure initialization */
 void trajectory_set_cs(struct trajectory *traj, struct cs *cs_d,
@@ -110,7 +127,10 @@ void trajectory_set_robot_params(struct trajectory *traj,
 				 struct robot_position *pos) ;
 
 /** set speed consign */
-void trajectory_set_speed(struct trajectory *traj, int16_t d_speed, int16_t a_speed);
+void trajectory_set_speed(struct trajectory *traj, double d_speed, double a_speed);
+
+/** set speed consign */
+void trajectory_set_acc(struct trajectory *traj, double d_acc, double a_acc);
 
 /**
  * set windows for trajectory.
@@ -129,10 +149,17 @@ void trajectory_set_windows(struct trajectory *traj, double d_win,
  */
 void trajectory_set_circle_coef(struct trajectory *traj, double coef);
 
+/**
+ * return trajectory state
+ */
+enum trajectory_state trajectory_get_state(struct trajectory *traj);
+
 /** return true if the position consign is equal to the filtered
  * position consign (after quadramp filter), for angle and
  * distance. */
 uint8_t trajectory_finished(struct trajectory *traj);
+uint8_t trajectory_angle_finished(struct trajectory *traj);
+uint8_t trajectory_distance_finished(struct trajectory *traj);
 
 /** return true if traj is nearly finished depending on specified
  *  parameters */
@@ -197,6 +224,15 @@ void trajectory_goto_xy_rel(struct trajectory *traj, double x_rel_mm, double y_r
 void trajectory_circle_rel(struct trajectory *traj, double x, double y,
 			   double radius_mm, double rel_a_deg, uint8_t flags);
 
+/*
+ * Compute the fastest distance and angle speeds matching the radius
+ * from current traj_speed
+ */
+void circle_get_da_speed_from_radius(struct trajectory *traj,
+				     double radius_mm,
+				     double *speed_d,
+				     double *speed_a);
+
 /* do a line */
 void trajectory_line_abs(struct trajectory *traj, double x1, double y1,
 			 double x2, double y2, double advance);