X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=modules%2Fdevices%2Frobot%2Ftrajectory_manager%2Ftrajectory_manager_utils.c;h=5ec5755bca5cd9ec20330d32d9c3cad20cf6b719;hp=0cf01acb2e452eee3ec3fbe05afc658643d25860;hb=d3a38d6cce69bc7debce828b4ecd3a9c0c866e96;hpb=91987ff2747a521681d087935148964eed6b3556

diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_utils.c b/modules/devices/robot/trajectory_manager/trajectory_manager_utils.c
index 0cf01ac..5ec5755 100644
--- 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,94 @@ uint8_t is_robot_in_angle_window(struct trajectory *traj, double a_win_rad)
 	a *= 2.;
 	return ABS(a) < a_win_rad;
 }
+
+enum trajectory_state trajectory_get_state(struct trajectory *traj)
+{
+	return traj->state;
+}
+
+/* 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));
+}
+