Control and Actuation Subsystem

The Control and Actuation subsystem translates spatial navigation targets and steering setpoints into physical motor commands, accounting for skid-steer kinematics, ground friction, and chassis pitch/roll limits.

1. Algorithmic Architecture

The control pipeline translates target headings and velocities into dual-track motor commands:

[Path / Steering Setpoint]
       |
       v
[Lateral Controller Selection]
 - PIDController (Heading tracking)
 - PredictiveStanleyController (Cross-track & heading tracking with Unicycle prediction)
 - PurePursuitController (Lookahead circle-arc pursuit)
       |
       v
[Inverse Kinematics] (DifferentialDrive.hpp)
 - Deadband Filtering (0.02 threshold, rescaled to full dynamic range)
 - Arcade Drive or Curvature Drive Models
 - Input Squaring (DRIVE_SQUARE_CONTROL_INPUTS)
       |
       v
[Safety & Terrain Multipliers] (DriveBoard.cpp)
 - Inclinometer Slope Damping (Pitch & Roll weighting)
 - Master Throttle Scaling (SETMAXSPEED callback)
 - Maximum Safe Power Clamp (DRIVE_MAX_SAFE_POWER)
       |
       v
[Network Actuation]
 - RoveComm UDP Packet (DRIVELEFTRIGHT) -> Core Board Microcontroller

2. Lateral Tracking Controllers

The autonomy software includes three lateral controllers:

1. PID Controller (`PIDController.cpp`)

Used for orienting the rover toward single waypoints, search legs, or during visual servoing:

Evaluates heading error: $e_\theta = \theta_{\text{goal}} - \theta_{\text{actual}}$.
Continuous angle wraparound ($0^\circ$ to $360^\circ$) prevents $350^\circ \rightarrow 10^\circ$ boundary wraps from triggering full reverse spins.
Output produces a normalized turn effort ($u \in [-1.0, 1.0]$).

2. Predictive Stanley Controller (`PredictiveStanleyController.cpp`)

Used for following continuous, multi-waypoint paths generated by GeoPlanner:

Simultaneously minimizes heading error ($\theta_e$) and cross-track error ($e_{\text{ct}}$, perpendicular distance to the reference path line).
Implements a unicycle kinematic prediction model (UnicycleModel.hpp) projecting the rover state $N$ timesteps into the future to compensate for chassis mass and actuation lag.

3. Pure Pursuit Controller (`PurePursuitController.cpp`)

Alternative geometric path follower:

Identifies a lookahead waypoint at a defined lookahead distance ($L_d$) along the path.
Calculates the constant curvature arc required to reach that point from the rover’s current pose.

3. Differential Drive Kinematics (`DifferentialDrive.hpp`)

Because the rover uses skid-steer (tank-style) drive rather than Ackermann steering, turning requires independent velocity control of the left and right wheel sets.

A. Deadband and Input Scaling

Inputs with absolute magnitude $< 0.02$ are zeroed. Values above the deadband are rescaled so the remaining interval maps smoothly across $[0.0, 1.0]$, preventing sudden motor jump at low throttle.

B. Arcade Drive Kinematics

Maps forward speed $v$ and rotation rate $\omega$: $\text{Left Power} = v + \omega$ $\text{Right Power} = v - \omega$ Powers are normalized if either exceeds $\pm 1.0$: $\text{Power}_{\text{max}} = \max(|\text{Left}|, |\text{Right}|, 1.0)$ $\text{Left} = \frac{\text{Left}}{\text{Power}_{\text{max}}}, \quad \text{Right} = \frac{\text{Right}}{\text{Power}_{\text{max}}}$

C. Curvature Drive Kinematics

Controls the radius of curvature rather than raw turning rate:

At higher speeds, steering sensitivity is dynamically scaled down to prevent violent rollovers.
When forward velocity is near zero, point-turning is permitted if DRIVE_CURVATURE_KINEMATICS_ALLOW_TURN_WHILE_STOPPED is true.

D. Squared Input Sensitivity

When DRIVE_SQUARE_CONTROL_INPUTS is enabled, input magnitudes are squared while preserving sign: $u_{\text{squared}} = \text{sgn}(u) \cdot u^2$ This provides fine, granular steering control at low speeds while retaining full power at maximum deflection.

4. Drive Board Safety and Terrain Multipliers (`DriveBoard.cpp`)

Raw kinematic commands pass through multi-layered safety conditioning before transmission:

Inclinometer Slope Damping

The DriveBoard subscribes to telemetry from the rover’s onboard inclinometer:

Computes effective slope angle $\phi$ as a weighted combination of roll and pitch: $\phi = w_{\text{roll}} \cdot |\text{Roll}| + w_{\text{pitch}} \cdot |\text{Pitch}|$ (Roll is weighted higher because skid-steer rovers are more susceptible to lateral roll-overs).
If $\phi < \text{constants::DRIVE_BOARD_MIN_SLOPE}$ (e.g., $10^\circ$), damping multiplier is $1.0$ (no attenuation).
If $\phi \ge \text{constants::DRIVE_BOARD_MAX_SLOPE}$ (e.g., $30^\circ$), multiplier clamps to constants::DRIVE_BOARD_MIN_DAMP (e.g., $0.50$).
Between these bounds, a linear interpolation scales the maximum allowed drive power down smoothly.

Master Throttle Control

A RoveComm UDP callback listens for SETMAXSPEED packets from the Basestation GUI, allowing operators to scale rover velocity across $[0.0, 1.0]$ in real time.

Hard Safety Power Ceiling

All final track outputs are clamped to constants::DRIVE_MAX_SAFE_POWER (e.g., $0.90$) to prevent motor stall overcurrent and fuse trips.

5. Inputs, Outputs, and Network Actuation

Inputs

Target heading and speed setpoints from active state or path follower.
Actual compass heading and velocity from NavigationBoard.
Inclinometer pitch/roll packets over RoveComm.

Outputs

manifest::Core::COMMANDS["DRIVELEFTRIGHT"]: RoveComm UDP packet containing two 32-bit floats $[P_{\text{left}}, P_{\text{right}}] \in [-1.0, 1.0]$.