Capstone: Build a Real Autonomous

Use everything you've learned to build a routine that wins points.

Requirements

• Pseudocode first. Write the plan in English before you write C++. Save it as a comment block at the top of your code.
• Finite state machine. Organize the routine as a list of named states.
• At least one PID call. A drive PID, a turn PID, or both. Tune the constants for your robot.
• At least one sensor-based decision. Use the distance sensor, gyro, optical, or bumper to make a choice, not just timing.
• At least one custom class or set of functions. Don't put everything in main(). Build a Drive class or a set of helpers.
• A timeout on every loop. No infinite loops, ever. Every PID and every sensor-wait gets a maximum time.
• Print state to the Brain screen. So you can debug during runs.

Skeleton to build on

#include "vex.h"
// using namespace vex;  (auto-included by VEXcode IQ)

// === Hardware ===
brain    Brain;
motor    LeftMotor   = motor(PORT1, false);
motor    RightMotor  = motor(PORT6, true);
inertial Gyro        = inertial(PORT2);
distance EyeFront    = distance(PORT5);

// === Pseudocode ===
// 1. drive forward 24 inches with PID
// 2. turn right to heading 90° with PID
// 3. drive forward until distance sensor < 150mm
// 4. release game piece
// 5. back away 12 inches

// === Constants ===
const double WHEEL_DIAM_IN = 2.75;
const double WHEEL_CIRC    = 3.14159 * WHEEL_DIAM_IN;
const double DEG_PER_INCH  = 360.0 / WHEEL_CIRC;

// === Drive PID (from week 8) ===
void drivePID(double inches) { /* ... your PID code here ... */ }

// === Turn PID (from week 7) ===
void turnTo(double targetHeading) { /* ... your turn code here ... */ }

// === States ===
enum AutoState { DRIVE_1, TURN_1, APPROACH, RELEASE, BACK_AWAY, DONE };

int main() {
  Gyro.calibrate();
  while (Gyro.isCalibrating()) wait(50, msec);

  AutoState state = DRIVE_1;
  double stateStart = Brain.timer(msec);

  while (state != DONE) {
    Brain.Screen.clearScreen();
    Brain.Screen.setCursor(1, 1);
    Brain.Screen.print("state: %d", state);

    switch (state) {
      case DRIVE_1:
        drivePID(24);
        state = TURN_1;
        stateStart = Brain.timer(msec);
        break;

      case TURN_1:
        turnTo(90);
        state = APPROACH;
        stateStart = Brain.timer(msec);
        break;

      case APPROACH:
        LeftMotor.spin(forward, 30, percent);
        RightMotor.spin(forward, 30, percent);
        if (EyeFront.objectDistance(mm) < 150) {
          LeftMotor.stop();
          RightMotor.stop();
          state = RELEASE;
          stateStart = Brain.timer(msec);
        }
        // timeout fallback
        if (Brain.timer(msec) - stateStart > 4000) {
          LeftMotor.stop();
          RightMotor.stop();
          state = DONE;   // give up gracefully
        }
        break;

      case RELEASE:
        // ... actuate your scoring mechanism ...
        wait(500, msec);
        state = BACK_AWAY;
        stateStart = Brain.timer(msec);
        break;

      case BACK_AWAY:
        drivePID(-12);
        state = DONE;
        break;

      case DONE:
        break;
    }
    wait(20, msec);
  }

  Brain.Screen.print("done!");
}

Self-assessment rubric

Score yourself out of 14. Honest scores beat inflated scores.

• 2 pts: Pseudocode is written before the code and matches what the code does.
• 2 pts: FSM has named states and only one is active at a time.
• 2 pts: At least one PID is tuned and works reliably.
• 2 pts: At least one sensor drives a real decision (not just timing).
• 2 pts: Code is organized into functions or a class, not one giant main().
• 2 pts: Every loop has a timeout.
• 2 pts: Routine completes 4 of 5 runs (i.e., it's reliable, not lucky).

Reflection questions

1. Where did your routine fail in testing? What did you change to fix it?
2. If you had two more weeks, what would you upgrade? Odometry, more sensors, better PID tuning, or something else?
3. What's one piece of code you'd reuse in next season's autonomous?

Where to go next

• readPurdue SIGBots Wiki: broader robotics software topics
• readVEX Forum: where teams share code and ask questions
• docsVEX Library: official documentation

int main() {
  AutoState state = DRIVE_1;
  double stateStart = Brain.timer(msec);

  while (state != DONE) {
    switch (state) {
      case DRIVE_1:
        drivePID(24);
        state = TURN_1;
        break;
      case TURN_1:
        turnTo(90);  // uses Gyro internally
        state = DONE;
        break;
      default: break;
    }
    wait(20, msec);
  }
}

enum AutoState { DRIVE_1, TURN_1, APPROACH, DONE };

int main() {
  int state = 0;

  while (state != DONE) {
    switch (state) {
      case DRIVE_1:
        drivePID(24);
        state = TURN_1;
        break;
      case TURN_1:
        turnTo(90);
        state = APPROACH;
        break;
      case APPROACH:
        // ...
        state = DONE;
        break;
    }
    wait(20, msec);
  }
}