From 672b49392b909d9a04919433626e96ccab94dc1f Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Mon, 20 Apr 2020 15:49:00 +0200 Subject: [PATCH] pbio/control: fix relative travel by 0 angle Previously, this behavior was not well-defined in case of trajectory patching. For now, this enforces hold behavior, going vertically to the origin in the angle/speed phase plot. This matches the behavior of angles > 0 In the future, relative angles >= 0 with speed in excess of what it can decelerate in time without overshoot, should curve around the origin of the phase plot, then decelerate smoothly back to the target. This is already done for angles < 0. --- lib/pbio/src/control.c | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/lib/pbio/src/control.c b/lib/pbio/src/control.c index 3dbcb1c89..999ee5a4f 100644 --- a/lib/pbio/src/control.c +++ b/lib/pbio/src/control.c @@ -182,6 +182,11 @@ pbio_error_t pbio_control_start_relative_angle_control(pbio_control_t *ctl, int3 // The target count is the start count plus the count to be traveled. If speed is negative, traveled count also flips. int32_t target_count = count_start + (target_rate < 0 ? -relative_target_count: relative_target_count); + // FIXME: Enable 0 angle and angle > 0 with excess speed as standard case instead to decelerate & return. + if (target_count == count_start) { + return pbio_control_start_hold_control(ctl, time_now, target_count); + } + return pbio_control_start_angle_control(ctl, time_now, count_now, target_count, rate_now, target_rate, acceleration, after_stop); }