← BACK

PID Autonomous Control

Archived
Timeline

Apr 2020 - Apr 2021

Role & Context

C++ Implementation

Core Tech
C++PID ControlOdometrySensor Fusion

Project Summary

Developed a robust custom PID controller in C++ for VEX Robotics 'Change Up' season to enable reliable autonomous scoring.

Key Features

  • Adaptive Gain Scheduling adjusting kP, kD, kI based on target distance
  • Triple-Redundant Inertial Sensor Fusion with outlier rejection
  • Lookup-Table Based Slew Rate Limiter for traction control
  • State Machine Logic for complex multi-goal autonomous routines

Impact & Takeaways

  • Achieved high-reliability navigation for 15-second autonomous period
  • Eliminated wheel slip and mechanical stress via motion profiling
  • Successfully navigated all 9 goal zones in Autonomous Skills Strategy

Context & Motivation

As the Lead Programmer for Team 8823A during the VEX Robotics "Change Up" season, I was responsible for creating an autonomous routine that could reliably score points in the 15-second autonomous period. Finding standard time-based movements too inaccurate, I developed a robust custom PID (Proportional-Integral-Derivative) controller in C++.

System Architecture

Target State Distance / Heading
Error
Adaptive PID Gain Scheduling
Power
Slew Rate Ramp Limits
Voltage
V5 Motors Actuation

Engineering Implementation

Adaptive PID Controller

We found that one set of constants doesn't fit all movements and short, precise adjustments need different gain values than long cross-field sprints. I implemented an Adaptive PID system that dynamically switches gain scheduling based on the target distance:

  • Gain Scheduling: The system checks the target distance against defined ranges (e.g., 0-11, 11-24, 24-48 inches) and loads the optimal kP, kD, and kI values from a lookup table.
  • Struct-Based Configuration: A custom PIDValue struct stores these tuned constants, along with motorPowerThresholds to prevent stalling.
const PIDValue movepidValues[] = {
  {0, 11, 0.1155, 0.045, 0.0325, ...}, // Precise Short Range
  {24, 48, 0.1155, 0.045, 0.010, ...}, // Mid Range
};

Inertial Sensor Fusion

To combat gyroscope drift and electro-mechanical noise, I implemented a Triple-Redundant Sensor Fusion algorithm:

  • Hardware Redundancy: We mounted three separate V5 Inertial Sensors (A, B, C) on the chassis.
  • Voter Algorithm: The code continuously calculates the mean and standard deviation of all three sensors. If any single sensor deviates significantly from the consensus (by > 1 standard deviation), it is dynamically excluded, and the heading is derived from the remaining two.
// Outlier Rejection Logic (Turns.cpp)
avgAll = (InertialA + InertialB + InertialC) / 3;
standardDev = sqrt((1/3) * (pow(InertialA - avgAll, 2) + ...));
if (fabs(InertialA) > fabs(standardDev + avgAll)) {
  currentDeg = avgBC; // Exclude Sensor A if it's an outlier
} ...
else {
  currentDeg = avgAll;
}

Motion Profiling (Slew Rate)

To prevent wheel slip and reduce mechanical stress on the drivetrain, I implemented a Lookup-Table Based Slew Rate Limiter:

  • Acceleration Curves: Instead of calculating acceleration in real-time, the system references a pre-computed array speedChange[] = {1, 1.5, 2... 81}.
  • Traction Control: This ensures the voltage applied to the motors ramps up according to a curve optimized for the robot's mass and wheel friction, preventing "burnouts" on the anti-static foam tiles.

Autonomous Skills Strategy

For the "Change Up" Skills Challenge, the robot had to autonomously score balls in 9 goals distributed across the arena. I architected a robust skillsBackLeftRoom routine:

  • State Machine Logic: The routine is broken down into discrete "Goal" states. Each state consists of a sequence: Move -> Turn -> Intake -> Score.
  • Jig-Based Calibration: The routine relies on a precise starting "Jig" placement (Top/Left slots) to minimize initial angular error.
  • Performance: The routine successfully navigates to all 9 goals, using Turn(degrees, direction) commands with timeout failsafes to ensure the robot never gets stuck in an infinite loop if a sensor fails.
BACKEnd of File