add a get_State() func

[aversive.git] / modules / devices / robot / trajectory_manager / trajectory_manager.h

diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager.h b/modules/devices/robot/trajectory_manager/trajectory_manager.h
index 43730ea..7cab785 100644 (file)
--- 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);