work on trajectory, update cobboard and ballboard too

[aversive.git] / modules / devices / robot / trajectory_manager / trajectory_manager_utils.c

diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_utils.c b/modules/devices/robot/trajectory_manager/trajectory_manager_utils.c
index 0cf01ac..c48b592 100644 (file)
--- a/modules/devices/robot/trajectory_manager/trajectory_manager_utils.c
+++ b/modules/devices/robot/trajectory_manager/trajectory_manager_utils.c
@@ -41,7 +41,7 @@
 #include "trajectory_manager_core.h"
 
 /** set speed consign in quadramp filter */
-void set_quadramp_speed(struct trajectory *traj, int16_t d_speed, int16_t a_speed)
+void set_quadramp_speed(struct trajectory *traj, double d_speed, double a_speed)
 {
        struct quadramp_filter * q_d, * q_a;
        q_d = traj->csm_distance->consign_filter_params;
@@ -51,7 +51,7 @@ void set_quadramp_speed(struct trajectory *traj, int16_t d_speed, int16_t a_spee
 }
 
 /** get angle speed consign in quadramp filter */
-uint32_t get_quadramp_angle_speed(struct trajectory *traj)
+double get_quadramp_angle_speed(struct trajectory *traj)
 {
        struct quadramp_filter *q_a;
        q_a = traj->csm_angle->consign_filter_params;
@@ -59,17 +59,28 @@ uint32_t get_quadramp_angle_speed(struct trajectory *traj)
 }
 
 /** get distance speed consign in quadramp filter */
-uint32_t get_quadramp_distance_speed(struct trajectory *traj)
+double get_quadramp_distance_speed(struct trajectory *traj)
 {
        struct quadramp_filter *q_d;
        q_d = traj->csm_distance->consign_filter_params;
        return q_d->var_1st_ord_pos;
 }
 
+/** set speed consign in quadramp filter */
+void set_quadramp_acc(struct trajectory *traj, double d_acc, double a_acc)
+{
+       struct quadramp_filter * q_d, * q_a;
+       q_d = traj->csm_distance->consign_filter_params;
+       q_a = traj->csm_angle->consign_filter_params;
+       quadramp_set_2nd_order_vars(q_d, ABS(d_acc), ABS(d_acc));
+       quadramp_set_2nd_order_vars(q_a, ABS(a_acc), ABS(a_acc));
+}
+
 /** remove event if any */
 void delete_event(struct trajectory *traj)
 {
        set_quadramp_speed(traj, traj->d_speed, traj->a_speed);
+       set_quadramp_acc(traj, traj->d_acc, traj->a_acc);
        if ( traj->scheduler_task != -1) {
                DEBUG(E_TRAJECTORY, "Delete event");
                scheduler_del_event(traj->scheduler_task);
@@ -143,3 +154,89 @@ uint8_t is_robot_in_angle_window(struct trajectory *traj, double a_win_rad)
        a *= 2.;
        return ABS(a) < a_win_rad;
 }
+
+/* distance unit conversions */
+
+double pos_mm2imp(struct trajectory *traj, double pos)
+{
+       return pos * traj->position->phys.distance_imp_per_mm;
+}
+
+double pos_imp2mm(struct trajectory *traj, double pos)
+{
+       return pos / traj->position->phys.distance_imp_per_mm;
+}
+
+double speed_mm2imp(struct trajectory *traj, double speed)
+{
+       return speed *
+               traj->position->phys.distance_imp_per_mm /
+               traj->cs_hz;
+}
+
+double speed_imp2mm(struct trajectory *traj, double speed)
+{
+       return speed * traj->cs_hz /
+               traj->position->phys.distance_imp_per_mm;
+}
+
+double acc_mm2imp(struct trajectory *traj, double acc)
+{
+       return acc * traj->position->phys.distance_imp_per_mm /
+               (traj->cs_hz * traj->cs_hz);
+}
+
+double acc_imp2mm(struct trajectory *traj, double acc)
+{
+       return acc * traj->cs_hz * traj->cs_hz /
+               traj->position->phys.distance_imp_per_mm;
+}
+
+/* angle unit conversions */
+
+double pos_rd2imp(struct trajectory *traj, double pos)
+{
+       return pos *
+               (traj->position->phys.distance_imp_per_mm *
+                traj->position->phys.track_mm / 2.);
+}
+
+double pos_imp2rd(struct trajectory *traj, double pos)
+{
+       return pos /
+               (traj->position->phys.distance_imp_per_mm *
+                traj->position->phys.track_mm / 2.);
+}
+
+double speed_rd2imp(struct trajectory *traj, double speed)
+{
+       return speed *
+               (traj->position->phys.distance_imp_per_mm *
+                traj->position->phys.track_mm /
+                (2. * traj->cs_hz));
+}
+
+double speed_imp2rd(struct trajectory *traj, double speed)
+{
+       return speed /
+               (traj->position->phys.distance_imp_per_mm *
+                traj->position->phys.track_mm /
+                (2. * traj->cs_hz));
+}
+
+double acc_rd2imp(struct trajectory *traj, double acc)
+{
+       return acc *
+               (traj->position->phys.distance_imp_per_mm *
+                traj->position->phys.track_mm /
+                (2. * traj->cs_hz * traj->cs_hz));
+}
+
+double acc_imp2rd(struct trajectory *traj, double acc)
+{
+       return acc /
+               (traj->position->phys.distance_imp_per_mm *
+                traj->position->phys.track_mm /
+                (2. * traj->cs_hz * traj->cs_hz));
+}
+