pbio/trajectory: Store user given speed.

This value can be used during control to test if the original speed target
was low. This can be used to reduce the control gains to increase
stability as a tradeoff against stiffness.

We use the user given speed instead of the actual top speed, so that
this won't affect commands that have low just only because the
maneuver is short.

Author: laurensvalk

lib/pbio/include/pbio/trajectory.h

@@ -66,6 +66,7 @@ typedef struct _pbio_trajectory_t {
     int32_t th1;                         /**<  Encoder count after the acceleration in-phase */
     int32_t th2;                         /**<  Encoder count at start of acceleration out-phase */
     int32_t th3;                         /**<  Encoder count at end of maneuver */
+    int32_t wu;                          /**<  Encoder rate target originally given by user */
     int32_t w0;                          /**<  Encoder rate at start of maneuver */
     int32_t w1;                          /**<  Encoder rate target when not accelerating */
     int32_t w3;                          /**<  Encoder rate target after the maneuver ends */
@@ -83,6 +84,7 @@ void pbio_trajectory_stretch(pbio_trajectory_t *trj, pbio_trajectory_t *leader);
 // Reference getter functions:
 
 uint32_t pbio_trajectory_get_duration(pbio_trajectory_t *trj);
+int32_t pbio_trajectory_get_abs_command_speed(pbio_trajectory_t *trj);
 void pbio_trajectory_get_endpoint(pbio_trajectory_t *trj, pbio_trajectory_reference_t *end);
 void pbio_trajectory_get_last_vertex(pbio_trajectory_t *trj, uint32_t time_ref, pbio_trajectory_reference_t *ref);
 void pbio_trajectory_get_reference(pbio_trajectory_t *trj, uint32_t time_ref, pbio_trajectory_reference_t *ref);

lib/pbio/src/trajectory.c

@@ -91,6 +91,7 @@ static void reverse_trajectory(pbio_trajectory_t *trj) {
     trj->th3 = -trj->th3;
 
     // Negate speeds and accelerations
+    trj->wu *= -1;
     trj->w0 *= -1;
     trj->w1 *= -1;
     trj->w3 *= -1;
@@ -119,8 +120,7 @@ void pbio_trajectory_make_constant(pbio_trajectory_t *trj, const pbio_trajectory
     pbio_trajectory_set_start(&trj->start, c);
 
     // Set speeds, scaled to ddeg/s.
-    trj->w0 = to_trajectory_speed(c->speed_target);
-    trj->w1 = to_trajectory_speed(c->speed_target);
+    trj->w0 = trj->w1 = trj->wu = to_trajectory_speed(c->speed_target);
     trj->w3 = c->continue_running ? to_trajectory_speed(c->speed_target): 0;
 }
 
@@ -256,7 +256,7 @@ static pbio_error_t pbio_trajectory_new_forward_time_command(pbio_trajectory_t *
     // Speed continues at target speed or goes to zero. And scale to ddeg/s.
     trj->w3 = c->continue_running ? to_trajectory_speed(c->speed_target) : 0;
     trj->w0 = to_trajectory_speed(c->speed_start);
-    int32_t wt = to_trajectory_speed(c->speed_target);
+    int32_t wt = trj->wu = to_trajectory_speed(c->speed_target);
     int32_t accel = to_trajectory_accel(c->acceleration);
     int32_t decel = to_trajectory_accel(c->deceleration);
 
@@ -365,7 +365,7 @@ static pbio_error_t pbio_trajectory_new_forward_angle_command(pbio_trajectory_t
     // Speed continues at target speed or goes to zero. And scale to ddeg/s.
     trj->w3 = c->continue_running ? to_trajectory_speed(c->speed_target) : 0;
     trj->w0 = to_trajectory_speed(c->speed_start);
-    int32_t wt = to_trajectory_speed(c->speed_target);
+    int32_t wt = trj->wu = to_trajectory_speed(c->speed_target);
     int32_t accel = to_trajectory_accel(c->acceleration);
     int32_t decel = to_trajectory_accel(c->deceleration);
 
@@ -669,6 +669,20 @@ void pbio_trajectory_get_endpoint(pbio_trajectory_t *trj, pbio_trajectory_refere
     pbio_trajectory_offset_start(end, &trj->start, trj->t3, trj->th3, trj->w3, 0);
 }
 
+/**
+ * Gets the originally commanded speed for the current trajectory, even if
+ * this top speed is not reached.
+ *
+ * This is an indicator of the expected speed, which may be used to to alter
+ * control behavior for motion with sustained low speed values.
+ *
+ * @param [in]  trj     The trajectory instance.
+ * @return              The user given speed in control units.
+ */
+int32_t pbio_trajectory_get_abs_command_speed(pbio_trajectory_t *trj) {
+    return to_control_speed(pbio_int_math_abs(trj->wu));
+}
+
 // Get trajectory endpoint.
 uint32_t pbio_trajectory_get_duration(pbio_trajectory_t *trj) {
     assert_time(trj->t3);