Simple autonomous

Convert wheel rotation to distance#

One wheel revolution travels approximately its circumference. Convert the change in motor encoder position into wheel revolutions, then multiply by π times the wheel diameter. External gearing must be included: a motor that turns twice per wheel turn needs a motor-revolutions-per-wheel-revolution value of 2.

The motor cartridge ratio is handled by the configured motor encoder. The separate ratio in this equation is the external transmission between the motor output shaft and wheel. Measure actual travel on the field; wheel compression and slipping still introduce error.

distance = (motor degrees ÷ 360 ÷ external ratio) × π × wheel diameter

References: VEX Robotics: Motor and Motor Group — C++ API (opens in a new tab) · PROS: Motors tutorial (opens in a new tab)

Drive a set distance#

Add this helper above autonomous(). It records the starting encoder readings instead of zeroing them, so another subsystem can continue tracking their position. Each side receives its own proportional correction.

Positive inches move forward; negative inches reverse. The function returns true after both sides remain within 0.3 inches for about 100 ms. It returns false on timeout. The gain, tolerance, speed limit, and timeout are starting values you must test. This basic example controls wheel travel, not field heading.

constexpr double wheelDiameter = 3.25;
constexpr double motorTurnsPerWheelTurn = 1.0;

double motorDegreesToInches(double degreesMoved) {
  return degreesMoved / 360.0 / motorTurnsPerWheelTurn
       * pi * wheelDiameter;
}

bool driveDistance(double inches, double maxPct = 40,
                   double timeoutMs = 2500) {
  if (!std::isfinite(inches) || maxPct <= 0 || timeoutMs <= 0)
    { stopDrive(); return false; }
  maxPct = limit(maxPct, 1, 100);
  const double leftStart = leftDrive.position(degrees);
  const double rightStart = rightDrive.position(degrees);
  timer elapsed;
  elapsed.reset();
  int settledMs = 0;
  while (elapsed.time(msec) < timeoutMs) {
    double leftDistance = motorDegreesToInches(
      leftDrive.position(degrees) - leftStart);
    double rightDistance = motorDegreesToInches(
      rightDrive.position(degrees) - rightStart);
    double leftError = inches - leftDistance;
    double rightError = inches - rightDistance;
    bool inside = std::fabs(leftError) < 0.3 && std::fabs(rightError) < 0.3;
    settledMs = inside ? settledMs + 10 : 0;
    if (settledMs >= 100) { stopDrive(); return true; }
    leftDrive.spin(forward, limit(5.0 * leftError, -maxPct, maxPct), pct);
    rightDrive.spin(forward, limit(5.0 * rightError, -maxPct, maxPct), pct);
    wait(10, msec);
  }
  stopDrive();
  return false;
}

References: VEX Robotics: Motor and Motor Group — C++ API (opens in a new tab)

Turn to a heading#

A target of 90° means 90° clockwise from the calibrated heading, not an additional 90° turn. wrap180() selects the short signed error: from 359° to 1°, the error is +2°, not −358°.

The robot must stay near the target before success is reported. A timeout prevents a stalled or misconfigured turn from blocking the rest of the program indefinitely. The PID guide adds damping and a velocity condition for more controlled settling.

bool turnToHeading(double targetHeading, double timeoutMs = 2500) {
  if (!sensorsReady || !std::isfinite(targetHeading) || timeoutMs <= 0)
    { stopDrive(); return false; }
  timer elapsed;
  elapsed.reset();
  int settledMs = 0;
  while (elapsed.time(msec) < timeoutMs) {
    double error = wrap180(targetHeading - imu.heading(degrees));
    settledMs = std::fabs(error) < 1.5 ? settledMs + 10 : 0;
    if (settledMs >= 150) { stopDrive(); return true; }
    double output = limit(0.7 * error, -35, 35);
    leftDrive.spin(forward, output, pct);
    rightDrive.spin(forward, -output, pct);
    wait(10, msec);
  }
  stopDrive();
  return false;
}

References: VEX Robotics: Inertial Sensor — C++ API (opens in a new tab)

Build a routine#

Replace autonomous() with the sequence below. Each helper finishes before the next one starts. If a movement times out, return immediately rather than continuing from a position the robot may never have reached.

This travels 24 inches forward, faces the 90° heading, then reverses 12 inches. Run each movement on its own first. A working sequence should not depend on the battery being at exactly the same voltage every time.

void autonomous() {
  if (!sensorsReady) { stopDrive(); return; }
  if (!driveDistance(24)) return;
  if (!turnToHeading(90)) return;
  if (!driveDistance(-12, 30, 2000)) return;
  stopDrive();
}

Add an intake helper#

Add this helper above autonomous(). It assumes an intake motor on Port 3. Call runIntake(50, 500) inside your routine to run at 50% for half a second, then stop.

A blocking helper is easiest to understand. For simultaneous intake and driving, start the intake with spin(), run the drive helper, and stop the intake on every exit path. You do not need a new task just to keep a motor spinning.

motor intake(PORT3, ratio18_1, false);
void runIntake(double powerPct, int durationMs) {
  if (durationMs <= 0) { intake.stop(coast); return; }
  intake.spin(forward, limit(powerPct, -100, 100), pct);
  wait(durationMs, msec);
  intake.stop(coast);
}

References: VEX Robotics: Motor and Motor Group — C++ API (opens in a new tab) · PROS: Multitasking and synchronization (opens in a new tab)

Test the functions#

1. Mark a starting line and command 12, 24, and 36 inches. Test reverse travel too.
2. If every distance is wrong by a similar percentage, check wheel diameter and external gearing before changing the controller gain.
3. Test headings near 0°/360° and turns in both directions. Verify a positive output turns clockwise.
4. Test the failure path by setting a short timeout with wheels raised. Confirm both motors stop and the next movement does not run.