make controller replacable

Author: baggepinnen

src/balance_original.jl

@@ -4,6 +4,7 @@ include(joinpath(@__DIR__, "DiscreteKalmanFilter.jl"))
 export BalanceController, compute_pwm!, handle_motion_mode!, balance_car!
 export MotionMode, STOP, START, FORWARD, BACKWARD, LEFT, RIGHT
 export car_stop!, apply_motor_output!
+export ParallelPID
 
 # =============================================================================
 # Hardware Interface Stubs (override for actual hardware)
@@ -26,30 +27,76 @@ const PWMB_RIGHT = 0
 const STBY_PIN = 0
 
 # =============================================================================
-# Constants (matching original BalanceCar.h parameters)
+# ParallelPID Controller Parameters
 # =============================================================================
 
-# PID parameters
-const KP_BALANCE = 55.0f0
-const KD_BALANCE = 0.75f0
-const KP_SPEED = 10.0f0
-const KI_SPEED = 0.26f0
-const KP_TURN = 2.5f0
-const KD_TURN = 0.5f0
-
-# Angle limits (degrees)
-const BALANCE_ANGLE_MIN = -22.0f0
-const BALANCE_ANGLE_MAX = 22.0f0
-
-# PWM limits
-const PWM_MAX = 255.0f0
-const PWM_MIN = -255.0f0
+"""
+    ParallelPID
+
+Controller parameters for parallel PD/PI architecture (original Tumbller).
+Stores all tunable gains and limits for the balance car controller.
+Default values match the original Tumbller code.
+
+# Fields
+- `kp_balance`, `kd_balance`: Balance control (PD on tilt angle)
+- `kp_speed`, `ki_speed`: Speed control (PI on wheel speed)
+- `kp_turn`, `kd_turn`: Turn control
+- `balance_angle_min`, `balance_angle_max`: Angle limits (degrees)
+- `pwm_min`, `pwm_max`: PWM output limits
+- `integral_limit`: Anti-windup limit for speed integrator
+- `speed_control_period`: Decimation period for speed control loop
+"""
+@kwdef mutable struct ParallelPID
+    # Parameters
+    # Balance control (PD)
+    kp_balance::Float32 = 55.0f0
+    kd_balance::Float32 = 0.75f0
+
+    # Speed control (PI)
+    kp_speed::Float32 = 10.0f0
+    ki_speed::Float32 = 0.26f0
+
+    # Turn control
+    kp_turn::Float32 = 2.5f0
+    kd_turn::Float32 = 0.5f0
+
+    # Angle limits (degrees)
+    balance_angle_min::Float32 = -22.0f0
+    balance_angle_max::Float32 = 22.0f0
+
+    # PWM limits
+    pwm_min::Float32 = -255.0f0
+    pwm_max::Float32 = 255.0f0
+
+    # Integral anti-windup limit
+    integral_limit::Float32 = 3000.0f0
+
+    # Speed control decimation period
+    speed_control_period::Int = 8
+
+    # State (mutable, initialized to zero)
+    speed_filter::Float32 = 0.0f0
+    speed_filter_old::Float32 = 0.0f0
+    car_speed_integral::Float32 = 0.0f0
+    speed_control_period_count::Int = 0
+    speed_control_output::Float32 = 0.0f0
+    rotation_control_output::Float32 = 0.0f0
+end
 
-# Integral anti-windup limit
-const INTEGRAL_LIMIT = 3000.0f0
+"""
+    reset_state!(c::ParallelPID)
 
-# Speed control decimation (runs every N cycles)
-const SPEED_CONTROL_PERIOD = 8
+Reset controller state to initial values (zeros).
+"""
+function reset_state!(c::ParallelPID)
+    c.speed_filter = 0.0f0
+    c.speed_filter_old = 0.0f0
+    c.car_speed_integral = 0.0f0
+    c.speed_control_period_count = 0
+    c.speed_control_output = 0.0f0
+    c.rotation_control_output = 0.0f0
+    nothing
+end
 
 # =============================================================================
 # Motion Mode Enum
@@ -61,24 +108,24 @@ const SPEED_CONTROL_PERIOD = 8
 # Controller State Struct
 # =============================================================================
 
-mutable struct BalanceController
+"""
+    BalanceController{C}
+
+Balance controller state with parametric controller type `C`.
+The controller type determines the control algorithm (e.g., `ParallelPID`).
+Controller-specific state is stored in the controller itself.
+"""
+mutable struct BalanceController{C}
+    # Controller (contains parameters and algorithm-specific state)
+    controller::C
+
     # Kalman filter for angle estimation
     kf::IMUKalmanFilter
 
     # Encoder pulse accumulators (signed based on PWM direction)
     encoder_left_pulse::Int
     encoder_right_pulse::Int
 
-    # Speed control state (PI integrator)
-    speed_filter::Float32
-    speed_filter_old::Float32
-    car_speed_integral::Float32
-    speed_control_period_count::Int
-
-    # Control outputs
-    speed_control_output::Float32
-    rotation_control_output::Float32
-
     # Setpoints
     setting_car_speed::Int
     setting_turn_speed::Int
@@ -96,21 +143,20 @@ mutable struct BalanceController
 end
 
 """
-    BalanceController(; angle_zero=0.0f0, angular_velocity_zero=0.0f0)
+    BalanceController(; controller=ParallelPID(), angle_zero=0.0f0, angular_velocity_zero=0.0f0)
 
-Create a balance controller with default parameters matching the original Tumbller code.
+Create a balance controller with the specified controller type (defaults to ParallelPID).
 """
-function BalanceController(; angle_zero::Float32=0.0f0, angular_velocity_zero::Float32=0.0f0)
-    BalanceController(
+function BalanceController(;
+    controller::C = ParallelPID(),
+    angle_zero::Float32 = 0.0f0,
+    angular_velocity_zero::Float32 = 0.0f0
+) where C
+    BalanceController{C}(
+        controller,
         IMUKalmanFilter(),      # kf
         0,                      # encoder_left_pulse
         0,                      # encoder_right_pulse
-        0.0f0,                  # speed_filter
-        0.0f0,                  # speed_filter_old
-        0.0f0,                  # car_speed_integral
-        0,                      # speed_control_period_count
-        0.0f0,                  # speed_control_output
-        0.0f0,                  # rotation_control_output
         0,                      # setting_car_speed
         0,                      # setting_turn_speed
         0.0f0,                  # pwm_left
@@ -133,17 +179,18 @@ Updates `ctrl.pwm_left` and `ctrl.pwm_right` in place.
 
 Returns the Kalman-filtered angle for use in motion mode handling.
 
-# Control Architecture (parallel, not cascade):
+# Control Architecture for ParallelPID (parallel, not cascade):
 - Balance control: PD on tilt angle
-- Speed control: PI on wheel speed (runs every 8 cycles)
+- Speed control: PI on wheel speed (runs every N cycles)
 - Turn control: P + D on turn rate
 
 Final: `pwm = balance - speed ± rotation`
 """
-function compute_pwm!(ctrl::BalanceController,
+function compute_pwm!(ctrl::BalanceController{ParallelPID},
                       encoder_count_left::Integer, encoder_count_right::Integer,
                       ax::Integer, ay::Integer, az::Integer,
                       gx::Integer, gy::Integer, gz::Integer)
+    c = ctrl.controller
 
     # Accumulate encoder pulses with sign based on current PWM direction
     ctrl.encoder_left_pulse += ctrl.pwm_left < 0 ? -encoder_count_left : encoder_count_left
@@ -157,43 +204,43 @@ function compute_pwm!(ctrl::BalanceController,
     kalman_angle = angle(ctrl.kf)
 
     # Balance control (PD on tilt angle)
-    balance_control_output = KP_BALANCE * (kalman_angle - ctrl.angle_zero) +
-                             KD_BALANCE * (gyro_x - ctrl.angular_velocity_zero)
+    balance_control_output = c.kp_balance * (kalman_angle - ctrl.angle_zero) +
+                             c.kd_balance * (gyro_x - ctrl.angular_velocity_zero)
 
-    # Speed control (PI, runs every 8 cycles = 40ms at 5ms sample time)
-    ctrl.speed_control_period_count += 1
-    if ctrl.speed_control_period_count >= SPEED_CONTROL_PERIOD
-        ctrl.speed_control_period_count = 0
+    # Speed control (PI, runs every N cycles)
+    c.speed_control_period_count += 1
+    if c.speed_control_period_count >= c.speed_control_period
+        c.speed_control_period_count = 0
 
         # Average wheel speed from encoder pulses
         car_speed = (ctrl.encoder_left_pulse + ctrl.encoder_right_pulse) * 0.5f0
         ctrl.encoder_left_pulse = 0
         ctrl.encoder_right_pulse = 0
 
         # Low-pass filter
-        ctrl.speed_filter = ctrl.speed_filter_old * 0.7f0 + car_speed * 0.3f0
-        ctrl.speed_filter_old = ctrl.speed_filter
+        c.speed_filter = c.speed_filter_old * 0.7f0 + car_speed * 0.3f0
+        c.speed_filter_old = c.speed_filter
 
         # PI integrator with setpoint
-        ctrl.car_speed_integral += ctrl.speed_filter - ctrl.setting_car_speed
+        c.car_speed_integral += c.speed_filter - ctrl.setting_car_speed
 
         # Anti-windup
-        ctrl.car_speed_integral = clamp(ctrl.car_speed_integral, -INTEGRAL_LIMIT, INTEGRAL_LIMIT)
+        c.car_speed_integral = clamp(c.car_speed_integral, -c.integral_limit, c.integral_limit)
 
         # Speed control output (negative feedback)
-        ctrl.speed_control_output = -KP_SPEED * ctrl.speed_filter - KI_SPEED * ctrl.car_speed_integral
+        c.speed_control_output = -c.kp_speed * c.speed_filter - c.ki_speed * c.car_speed_integral
 
         # Turn control output (computed at same rate as speed control)
-        ctrl.rotation_control_output = ctrl.setting_turn_speed + KD_TURN * gyro_z
+        c.rotation_control_output = ctrl.setting_turn_speed + c.kd_turn * gyro_z
     end
 
     # Combine control outputs
-    ctrl.pwm_left = balance_control_output - ctrl.speed_control_output - ctrl.rotation_control_output
-    ctrl.pwm_right = balance_control_output - ctrl.speed_control_output + ctrl.rotation_control_output
+    ctrl.pwm_left = balance_control_output - c.speed_control_output - c.rotation_control_output
+    ctrl.pwm_right = balance_control_output - c.speed_control_output + c.rotation_control_output
 
     # Clamp to PWM limits
-    ctrl.pwm_left = clamp(ctrl.pwm_left, PWM_MIN, PWM_MAX)
-    ctrl.pwm_right = clamp(ctrl.pwm_right, PWM_MIN, PWM_MAX)
+    ctrl.pwm_left = clamp(ctrl.pwm_left, c.pwm_min, c.pwm_max)
+    ctrl.pwm_right = clamp(ctrl.pwm_right, c.pwm_min, c.pwm_max)
 
     return kalman_angle
 end
@@ -211,11 +258,12 @@ Modifies `ctrl.pwm_left`, `ctrl.pwm_right`, `ctrl.motion_mode`, and integrator s
 Returns `true` if motors should be stopped (car_stop! should be called).
 """
 function handle_motion_mode!(ctrl::BalanceController, kalman_angle::Float32; key_flag::Char='0')
+    c = ctrl.controller
     should_stop_motors = false
 
     # Check angle limits - force STOP if exceeded (except during START or STOP modes)
     if ctrl.motion_mode != START && ctrl.motion_mode != STOP
-        if kalman_angle < BALANCE_ANGLE_MIN || kalman_angle > BALANCE_ANGLE_MAX
+        if kalman_angle < c.balance_angle_min || kalman_angle > c.balance_angle_max
             ctrl.motion_mode = STOP
             should_stop_motors = true
         end
@@ -225,14 +273,14 @@ function handle_motion_mode!(ctrl::BalanceController, kalman_angle::Float32; key
     if ctrl.motion_mode == STOP
         if key_flag != '4'
             # Full stop - zero everything
-            ctrl.car_speed_integral = 0.0f0
+            reset_state!(c)
             ctrl.setting_car_speed = 0
             ctrl.pwm_left = 0.0f0
             ctrl.pwm_right = 0.0f0
             should_stop_motors = true
         else
             # key_flag == '4': Reset state but don't stop motors (allows restart)
-            ctrl.car_speed_integral = 0.0f0
+            reset_state!(c)
             ctrl.setting_car_speed = 0
             ctrl.pwm_left = 0.0f0
             ctrl.pwm_right = 0.0f0