Writing Autonomous Routines

Key LemLib Movement Functions

LemLib provides these primary movement functions. All distances are in inches, angles in degrees.

Async vs synchronous moves

By default, movement functions are asynchronous: they start the move and return immediately. The next line of code runs while the robot is still moving. Pass false as the last parameter to make them synchronous (wait until done).

// Synchronous – wait until the robot arrives:
chassis.moveToPoint(24, 24, 5000, {}, false);

// Async + waitUntil – start move, activate intake after 10 inches:
chassis.moveToPoint(24, 24, 5000);   // returns immediately
chassis.waitUntil(10);              // block here until 10" traveled
intakeIntoBasket();                  // now activate intake mid-move
chassis.waitUntilDone();             // wait for full move to finish

Movement params

All movement functions accept an optional MoveToPointParams struct:

chassis.moveToPoint(24, 24, 5000, {
    .forwards      = true,  // drive forwards (default) or backwards
    .maxSpeed      = 100,  // 0–127 cap on speed
    .minSpeed      = 20,   // minimum output (prevents stalling)
    .earlyExitRange = 3   // exit when within 3" (skip final deceleration)
});

The Custom moveRelative Helper

LemLib's built-in moves use absolute coordinates. Sometimes you want to move a fixed distance from wherever you currently are. The Chassis subclass adds moveRelative:

include/systems/chassis.hppvoid moveRelative(double dist, int timeout, ...) {
  this->waitUntilDone();
  lemlib::Pose pose = this->getPose(true); // true = radians

  // Calculate target point in direction of current heading
  this->moveToPoint(
    pose.x + std::sin(pose.theta) * dist,
    pose.y + std::cos(pose.theta) * dist,
    timeout, ...
  );
}

Auton Selector

A single button on the brain LCD cycles through auton options. The selected auton name is stored in the global selectedAuton string.

src/main.cppvoid on_center_button() {
  auto it = std::find(autonOptions.begin(), autonOptions.end(), selectedAuton);
  if (it != autonOptions.end()) {
    int index = std::distance(autonOptions.begin(), it);
    index = (index + 1) % autonOptions.size();
    selectedAuton = autonOptions[index];
  } else {
    selectedAuton = autonOptions[0];
  }
}

// Registered in initialize():
pros::lcd::register_btn1_cb(on_center_button);

In autonomous(), the selected string dispatches to the right function:

src/main.cppvoid autonomous() {
  if      (selectedAuton == "RED_6_BLOCK_LEFT")  redLeft6Block();
  else if (selectedAuton == "BLUE_6_BLOCK_RIGHT") blueRight6Block();
  // … etc …
  else if (selectedAuton == "SKILLS")            skills();
}

Structuring Helper Functions

Auton code quickly becomes repetitive. Extract repeated actions into helper functions. This robot defines these helpers in autons.cpp:

src/autons/autons.cppvoid intakeIntoBasket() {
  firstStageIntake.move(-127);
  secondStageIntake.move(127);
}

void scoreLongGoal() {
  firstStageIntake.move(127);
  pros::delay(170);
  firstStageIntake.move(-127);
  secondStageIntake.move(-127);
}

void idleIntake() {
  firstStageIntake.move(0);
  secondStageIntake.move(0);
}

Skills Run Overview

The skills run targets 75+ points across:

Skills run structure

src/autons/autons.cpp  (skills() — simplified)// ── Starting position ─────────────────────────────────
chassis.setPose(-2*TILE_UNIT-1.75, 16.25, 90);

// ── Phase 1: Initial block cluster ────────────────────
chassis.turnToPoint(-TILE_UNIT, TILE_UNIT, 10000);
intakeIntoBasket();
chassis.moveToPoint(-TILE_UNIT, TILE_UNIT, 10000);

// ── Phase 2: Park zone clear + position reset ──────────
runParkClear(false);
chassis.turnToHeading(90, 10000, {.minSpeed = 20}, false);
chassis.tareXPos(-70.4375, &distanceBack,  -2.75, 1.00);        // ← RESET
chassis.tareYPos(-70.4375, &distanceRight, -4.75, 1.00, M_PI/2); // ← RESET

// ── Phase 3: Score loader 1 into long goal ─────────────
chassis.moveToPoint(-3*TILE_UNIT+loaderRobotOffsetFromWall, -2*TILE_UNIT, 3000, {...});
wiggleLoader(); // wiggle to clear all 5 blocks from loader
chassis.moveToPoint(-goalXPosition, -2*TILE_UNIT, 3000, {.forwards = false});
scoreLongGoal();
pros::delay(1500);

// ── Phase 4: Reset after scoring ──────────────────────
chassis.turnToHeading(90, 10000, {...}, false);
chassis.tareXPos(-70.4375, &distanceBack,  -2.75, 1.00);        // ← RESET
chassis.tareYPos(-70.4375, &distanceRight, -4.75, 1.00, M_PI/2); // ← RESET

// … loader 2, loader 3, loader 4 follow the same pattern …

// ── Final: Park ───────────────────────────────────────
runPark();

The wiggleLoader technique

Loader positions vary slightly between fields (they can be ~1" off). Rather than relying on exact positioning, the robot oscillates left/right for ~1.2 seconds while at the loader to ensure all 5 blocks drop through:

void wiggleLoader() {
  pros::delay(1200);
  chassis.cancelAllMotions();
  leftMotorGroup.move(30);  rightMotorGroup.move(0);   pros::delay(200);
  leftMotorGroup.move(0);   rightMotorGroup.move(30);  pros::delay(200);
  // … repeat …
}