|
|
4d2b008c48
|
feat(controls): adaptive PID balance controller with gain scheduling (Issue #136)
Pure modules (no ROS2 dep, fully unit-tested):
- pid_controller.py:
GainSet — (kp,ki,kd) with safety clamp helper
PIDController — anti-windup integral, D-on-error, output clamping
GainScheduler — 3-class weight table (empty/light/heavy), exponential
gain blending (alpha per tick), safety bounds clamping, manual
override, immediate revert-to-defaults on instability
InstabilityDetector — dual criteria: tilt threshold (>50% of window)
+ sign-reversal count (oscillation)
- weight_estimator.py:
WeightEstimator — rolling-window current→weight, steady-state gating
(|tilt|≤threshold), change detection (threshold_kg)
CalibrationRoutine — IDLE→ROCKING→SETTLING→DONE/FAILED state machine;
sinusoidal rocking output, settling current sampling, weight estimate
from avg current; abort() / restart supported
- adaptive_pid_node.py: 100 Hz ROS2 node
Sub: /saltybot/imu (Imu, pitch from quaternion), /saltybot/motor_current
Pub: /saltybot/balance_effort (Float32), /saltybot/weight_estimate,
/saltybot/pid_state (JSON: gains, class, weight_kg, flags)
Srv: /saltybot/calibrate_balance (std_srvs/Trigger)
IMU watchdog (0.5 s), dynamic reconfigure via override_enabled param,
instability → revert + PID reset, structured INFO/WARN logging
- config/adaptive_pid_params.yaml, launch/adaptive_pid.launch.py,
package.xml, setup.py, setup.cfg
- test/test_adaptive_pid.py: 68/68 unit tests passing
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
|
2026-03-02 09:38:46 -05:00 |
|
