sl-controls created pull request seb/saltylab-firmware#149

sl-controls
Mon, 02 Mar 2026 09:39:12 -0500

149#feat(controls): adaptive PID balance controller with gain scheduling (Issue #136)#

Summary

pid_controller.py — GainSet (clamped kp/ki/kd), PIDController (anti-windup, D-on-error, output clamp), GainScheduler (empty/light/heavy weight table, exponential blending at gain_blend_alpha per tick, safety bounds, manual override, instant revert-to-defaults), InstabilityDetector (dual criteria: tilt threshold + oscillation sign-reversal count)

weight_estimator.py — WeightEstimator (rolling-window current→weight, steady-state gated by |tilt|, change detection), CalibrationRoutine (IDLE→ROCKING→SETTLING→DONE/FAILED; sinusoidal rocking output, settling current sampling, weight from avg current)

adaptive_pid_node.py — 100 Hz ROS2 node; /saltybot/imu + /saltybot/motor_current → /saltybot/balance_effort + /saltybot/weight_estimate + /saltybot/pid_state (JSON); /saltybot/calibrate_balance Trigger service; IMU watchdog; override_enabled dynamic reconfigure; instability → revert + PID reset + WARN log

config/adaptive_pid_params.yaml, launch/adaptive_pid.launch.py, package.xml, setup.py/cfg

test/test_adaptive_pid.py — 68/68 unit tests passing

How to use

# Launch
ros2 launch saltybot_adaptive_pid adaptive_pid.launch.py

# Trigger calibration
ros2 service call /saltybot/calibrate_balance std_srvs/srv/Trigger

# Manual gain override (dynamic reconfigure)
ros2 param set /adaptive_pid override_enabled true
ros2 param set /adaptive_pid override_kp 12.0
ros2 param set /adaptive_pid override_enabled false  # back to auto

# Monitor
ros2 topic echo /saltybot/pid_state

Test plan

68 unit tests pass (pure Python, no ROS2)

Verify gain transitions logged on weight class change

Trigger calibration service, confirm DONE + weight estimate in logs

Inject large tilt oscillations, confirm WARN + revert logged

Confirm override_enabled bypasses scheduling

Verify /saltybot/pid_state JSON fields on topic echo

Closes #136

🤖 Generated with Claude Code