231 update lift code with new motion control and multiple lifts#233
Open
231 update lift code with new motion control and multiple lifts#233
Conversation
gdoffe
force-pushed
the
231-update-lift-code-with-new-motion-control-and-multiple-lifts
branch
from
6e08cc9 to
3e9c728
Compare
- New CAN message types (speed_order, path commands) pushed handler count beyond the previous 16-slot limit
- Add TrapezoidalProfile::compute_distance_for_duration() to convert (speed, duration) into a distance for profile generation, needed by speed PID tuning chains - Publish timeout duration as IO key so downstream controllers can size their profiles to match the engine timeout - Fix set_timeout_ms documentation (was labeled as cycles)
- Generates a trapezoidal velocity profile from target speed and duration, replacing the pose-based profile approach - Simplifies the tuning chain by removing the need for PoseErrorFilter, TargetChangeDetector, and TuningPoseReachedFilter
- Without this, the last speed command persists after timeout, causing unintended motor drive
- Allow controlling the robot by speed command (speed + duration) via CAN protobuf message for PID tuning workflows - Only accepted when a speed tuning chain is active - Reset pose_reached on controller switch to avoid stale state - Fix set_timeout_ms to pass milliseconds (not cycles)
- Use configurable IO keys instead of hardcoded strings so a single instance works for linear or angular telemetry - Convert values from per-period to per-second for readability - Send float instead of int64_t to preserve precision - Replace speed_command with pose_error for better diagnostics
- Replace pose-driven tuning pipeline with direct SpeedTuningProfileController for simpler speed PID tuning - Split telemetry into linear and angular instances so each axis reports its own data independently
Replace the hardcoded target_x/target_y/target_O IO keys with a generic etl::array of watched keys, templated on the array size. This allows the same controller to detect changes on any set of IO values, not just pose coordinates. Values are read as float or int and compared by int cast (1-unit threshold), like the original implementation. The trigger_state field, which was defined but never used, is removed. Since the class is now a template, the implementation moves from the .cpp into the header. Signed-off-by: Gilles DOFFE <g.doffe@gmail.com>
Add a speed_mode parameter to ProfileTrackerControllerParameters. When enabled, the controller computes the target distance from target_speed and duration_periods IO keys using TrapezoidalProfile::compute_distance_for_duration(), instead of reading pose_error directly. This allows reusing the same controller for both position tracking and speed PID tuning, with the same trapezoidal profile generation. A new duration_periods IO key is added to ProfileTrackerControllerIOKeys for this mode. Signed-off-by: Gilles DOFFE <g.doffe@gmail.com>
When using multi-waypoint paths, PathManagerFilter sets new_target=true when advancing to the next waypoint but nobody resets it to false. This causes PlatformEngine::process_outputs() to trigger force_moving_transition every cycle, flooding the CAN bus with pose_reached messages in an infinite loop. Add TargetChangeDetector to both QUADPID and QUADPID_TRACKER chains so that new_target is only true when target_pose actually changes. Also make TargetChangeDetector::reset() a no-op to prevent the chain reset from setting first_run_=true, which would spuriously trigger new_target and restart the flood loop. Signed-off-by: Gilles DOFFE <g.doffe@gmail.com>
When receiving a START_POSE CAN message, the handler forced pose_reached to 'reached'. If a PATH_START arrived shortly after, PathManagerFilter would read this stale 'reached' status on the next control cycle and immediately advance past the first waypoint. Replace the forced 'reached' with reset_pose_reached() and stop the current path, since setting a new start pose invalidates any ongoing navigation. Also disable the engine around path_start to prevent a race where the control loop reads stale pose_reached between path start and the reset. Signed-off-by: Gilles DOFFE <g.doffe@gmail.com>
Add AdaptivePurePursuitController that implements the Pure Pursuit algorithm for smooth curved path following. Unlike the waypoint-based QUADPID chains, Pure Pursuit generates continuous steering commands by tracking a lookahead point on the path. Key features: - Adaptive lookahead distance based on current speed - Curvature-based speed limiting for safe cornering - Handles both forward and backward motion - Initial rotation phase for large heading errors - Path interpolation between waypoints The controller chain includes: - AdaptivePurePursuitController (generates speed orders from path) - SpeedPIDControllers (closed-loop speed control) - AntiBlockingControllers (motor stall detection) Add ADAPTIVE_PURE_PURSUIT enum value to PB_Controller.proto for remote controller selection. Signed-off-by: Gilles DOFFE <g.doffe@gmail.com>
The controller switch in _handle_set_controller() had no case for ADAPTIVE_PURE_PURSUIT, so selecting it from the copilot fell through to the default QUADPID controller. Add the missing case and call the chain init() at startup. Signed-off-by: Gilles DOFFE <g.doffe@gmail.com>
The adaptive pure pursuit chain was missing from all three switch statements that reset controllers: pf_motion_control_reset_controllers, pf_pose_reached_cb and pf_handle_game_end. Without the reset, the PP controller's needs_path_init_ flag stayed false after a new path was started, causing stale segment tracking state and an immediate spurious pose_reached signal. Signed-off-by: Gilles DOFFE <g.doffe@gmail.com>
The Pure Pursuit controller was reporting pose_reached too early on the last path segment. compute_distance_to_goal() tracks the lookahead circle's intersection with the segment, not the robot's actual position. When the lookahead reaches the segment end (t=1.0), distance_to_goal drops to ~0 while the robot is still ~lookahead_distance behind. Use euclidean distance from the robot to the last waypoint for both pose_reached detection and deceleration, but only on the last segment. On earlier segments, euclidean distance to the last waypoint can be misleading on closed-loop paths (e.g., a rectangle returning to its start position would have ~0 distance at the beginning). Also limit find_lookahead_point() search to the current and next segment to prevent the lookahead from jumping across geometrically close but path-distant segments on closed-loop paths. Signed-off-by: Gilles DOFFE <g.doffe@gmail.com>
gdoffe
force-pushed
the
231-update-lift-code-with-new-motion-control-and-multiple-lifts
branch
2 times, most recently
from
a7036e2 to
156ca6a
Compare
The Pure Pursuit controller was arriving too fast at corners because it only reacted to the instantaneous curvature at the lookahead point, without anticipating upcoming turns. This caused abrupt speed changes at waypoints where the path direction changes sharply. Add corner anticipation that computes the turning angle at the next waypoint and decelerates in advance using v = sqrt(v_corner² + 2*a*d). The corner speed is scaled by the straightness of the turn so that sharp corners (90°) get much lower target speeds than gentle ones. Add a dedicated corner_deceleration parameter (250 mm/s², half of the normal deceleration) to control how early the braking starts before corners, independently from the general deceleration profile. Signed-off-by: Gilles DOFFE <g.doffe@gmail.com>
When multiple boards share the same CAN bus (e.g. two lift MCUs), parameter get/set requests are received by all boards. Previously, every board responded, even if it didn't own the requested parameter. Now handle_get/handle_set return etl::optional and only the board that owns the parameter sends a response. Also add the parameter handler to the pf-robot-motors platform so that lift boards can expose their PID parameters for remote tuning. Signed-off-by: Gilles DOFFE <g.doffe@gmail.com>
Add a new_target flag in MotorEngine to signal target changes to the profile tracker controller, enabling profile recomputation on new commands. Fix MotorPoseFilter to read pose_reached as the correct enum type instead of float. Improve debug output in SpeedPIDController and MotorPoseFilter, and remove noisy MetaController debug trace. Signed-off-by: Gilles DOFFE <g.doffe@gmail.com>
Refactor Motor to support two control modes: the classic cascaded DualPID chain and a new DUALPID_TRACKER chain using a trapezoidal velocity profile with feedforward and feedback correction. The mode is selected at construction based on whether profile tracker parameters are provided. Rework Lift init to use a 2-second timeout for the homing sequence, keep limit switches registered at runtime for protection, and fix active-high logic for switch handling. Add MOTOR_LIFT_2 to the protobuf enum for multi-lift support. Signed-off-by: Gilles DOFFE <g.doffe@gmail.com>
Add motion_control_common, profile_tracker_controller and tracker_combiner_controller modules needed by the DUALPID_TRACKER control mode. Add the dualpid_tracker_chain header defining IO keys and parameter instances for the tracker chain. Add debug logging in pf_actuators and pf_positional_actuators for CAN command tracing. Signed-off-by: Gilles DOFFE <g.doffe@gmail.com>
Rename cup2025-robot1-lift to robot-lift-control and add build-time LIFT_ID selection (1 or 2). Each lift has its own configuration header with mechanical limits, PID gains, and tracker parameters, allowing the same firmware to be flashed on multiple lift boards. Signed-off-by: Gilles DOFFE <g.doffe@gmail.com>
The motor driver can latch an overload fault at any time. Instead of only pulsing the clear_overload pin on new commands in Lift::actuate(), drive it low continuously from the MotorEngine control loop so faults are cleared within one period regardless of when they occur. Signed-off-by: Gilles DOFFE <g.doffe@gmail.com>
uuid_t is uint32_t, so %lX (long) causes a -Wformat error on x86_64 native builds where unsigned int is 32 bits. Use PRIX32 from inttypes.h for portable formatting. Signed-off-by: Gilles DOFFE <g.doffe@gmail.com>
The previous homing sequence moved up then down, which was unreliable when the lift was already at the top or when the upper limit switch triggered intermittently. Simplify init to only move down to find the zero position. Also change limit switch behavior outside init: switches now only brake the motor without resetting the measured position. During normal operation the position should come from the encoder, not from the switch; resetting it would corrupt the control loop state. Only the lower switch during init sets the zero reference. Signed-off-by: Gilles DOFFE <g.doffe@gmail.com>
Previously the engine disabled itself and braked the motor on timeout or blocked conditions. Braking puts the H-bridge in a mode that behaves like disable on our hardware, causing the lift to freewheel under gravity. Instead, set the target to the current position and let the control loop actively maintain it. Also disable the timeout to avoid re-triggering. Apply the same approach on pose_reached: disable the timeout rather than just resetting the counter, so that subsequent position drift does not cause a spurious timeout. Signed-off-by: Gilles DOFFE <g.doffe@gmail.com>
When a limit switch is pressed, stop() re-issues actuate() which enables a new timeout. If the motor cannot re-reach the target within 5 seconds (e.g. because the user moved it manually), the timeout fires and the lift loses its hold. Disable the timeout after stop() so the motor holds position indefinitely until a new command arrives. Signed-off-by: Gilles DOFFE <g.doffe@gmail.com>
The homing sequence was only triggered via CAN command, so the lift had no zero reference after power-on. Call init_sequence() right after creating the lift so the homing runs automatically at boot. Also force the odometer to upper_limit_mm before commanding the descent, otherwise the motor thinks it is already at zero and does not move. Signed-off-by: Gilles DOFFE <g.doffe@gmail.com>
Add a LOG_INFO that fires once per target when pose is first reached, printing current and target positions. Also add a pose_reached callback so external code (e.g. lift application) can react to target completion. Signed-off-by: Gilles DOFFE <g.doffe@gmail.com>
gdoffe
force-pushed
the
231-update-lift-code-with-new-motion-control-and-multiple-lifts
branch
from
156ca6a to
05c8da1
Compare
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
2 participants
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Summary
- Add DUALPID_TRACKER control mode to Motor using a trapezoidal velocity profile with tracking error correction, replacing the reactive cascaded PID for lifts