Outside-the-Box Algorithms

There's usually a clever way to solve the problem. Find it.

The same task, three ways

"Drive across the field and park on the platform." Three approaches, three trade-offs.

Approach 1: Dead reckoning

Drive 48 inches at 70%. Easy to code. Breaks when the battery is low or the floor changes.

Approach 2: Drive until sensor

Drive forward at 70% until the distance sensor sees the platform 4 inches away. Robust to battery changes. Fails if the sensor angle is off or the platform has gaps.

Approach 3: Drive by odometry

Drive to (48, 0) using PID and the tracked pose. Cleanest, most adaptable. Slowest to set up, but reusable across every routine.

"Make it work. Make it right. Make it fast."

This is the order:

1. Make it work. Get the basic functionality going by any means. Ugly is fine. Hardcoded values are fine.
2. Make it right. Refactor for clarity. Replace magic numbers with constants. Pull repeated logic into functions. Don't change behavior.
3. Make it fast. Profile, find the bottleneck, optimize. Often this isn't needed at all, most code is fast enough once it's right.

Teams that try to write fast, clever, refactored code on the first attempt usually end up with buggy, slow, clever, refactored code. Get it working first.

Design for the worst case

At competition:

• The battery will be 70%, not 100%.
• An alliance partner might bump you.
• The field surface will be slightly different.
• You'll be nervous and forget to start the gyro calibration on the bench.

Smart code accounts for this. Add timeouts so a stuck PID doesn't run forever. Add fallbacks so a missing sensor reading doesn't crash autonomous. Validate gyro calibration before the match.

// PID with timeout fallback, good defensive programming
double startTime = Brain.timer(msec);
double timeoutMs = 2500;

while (true) {
  // ... normal PID loop ...
  if (Brain.timer(msec) - startTime > timeoutMs) {
    Brain.Screen.print("PID timed out, moving on");
    break;
  }
  wait(20, msec);
}

The "if I had to do it without that part" exercise

For any autonomous routine, ask: "what if the gyro fails?" Can you still complete it? What if the optical sensor breaks? What if a motor stalls? Walking through these scenarios uncovers brittle code before competition does.

Real-world inspiration

The Pilons (team 5225), 2018 World Champions, built their autonomous around a small library of well-tuned primitives: turnToAngle(), driveToPoint(), driveCurve(). Each routine was a short list of calls to these primitives. The result: easier to write new routines, fewer bugs, more time to test.

Curated references

• readPurdue SIGBots: "Make it work, make it right, make it fast" (bottom of page)
• readVEX Forum: What is Odometry?, strategy discussion

void drivePID(double target) {
  double kp = 0.4;
  double error = target - Distance2.objectDistance(mm);
  while (error > 0) {
    error  = target - Distance2.objectDistance(mm);
    double output = kp * error;
    if (output >  100) output =  100;
    if (output < -100) output = -100;
    Motor1.setVelocity(output, percent);
    wait(0.04, seconds);
  }
  Motor1.stop();
}

int main() {
  drivePID(200);
  turnTo(90);
}

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

while (state != DONE) {
  switch (state) {
    case DRIVE_1:
      drivePID(24);
      if (Brain.timer(msec) - stateStart > 3000) state = DONE;
      else state = TURN_1;
      break;
    case TURN_1:
      turnTo(90);
      if (Brain.timer(msec) - stateStart > 3000) state = DONE;
      else state = APPROACH;
      break;
    // ...
  }
  wait(20, msec);
}