FTC Mecanum Pathing System

A self-contained autonomous pathing library for an FTC mecanum drivetrain. It offers four interchangeable path followers, a full Pedro-style Bezier path engine, two localizer options, and ready-to-run example and tuning OpModes. Everything is editable live through FTC Dashboard.

Conventions

• Units: inches, seconds, radians.
• Field frame: +x forward (away from driver), +y left, heading CCW-positive.
• Use Pose.fromDegrees(x, y, deg) for human-friendly heading entry.

---

How the whole system works

At the top level the system answers one question, 50+ times per second: "Given where the robot is and where the path wants it to be, what power should each wheel get?" That answer is produced by a pipeline of four stages. Everything else in the library is a piece of one of these stages.

   sensors                geometry              control                motors
 ┌──────────┐   pose    ┌──────────┐  target  ┌──────────┐  vector  ┌──────────┐
 │Localizer │ ────────▶ │  Path /  │ ───────▶ │ Follower │ ───────▶ │ Mecanum  │
 │          │           │ PathChain│          │+controllers│        │  Drive   │
 └──────────┘           └──────────┘          └──────────┘          └──────────┘
   where am I?          where should            how hard, which       spin the
                        I be heading?           direction?            wheels

The control loop

Every OpMode does the same thing once per loop, inside Follower.update():

1. Localize. localizer.update() reads the odometry sensors and computes the robot's field Pose (x, y, heading).
2. Project onto the path. The follower finds the parameter t ∈ [0, 1] of the closest point on the active Path to the robot (path.getClosestT(pose)). t is "how far along the path" the robot currently is.
3. Compute a drive signal. The follower asks its controllers for a field-relative translation Vector plus a turn power — this is the only part that differs between the four followers.
4. Drive. MecanumDrive rotates the field vector into the robot's frame, mixes it with the turn into four wheel powers, normalizes, and writes them to the motors.
5. Advance. If the robot has reached the end of the current path (within tolerances), the follower steps to the next path in the chain, or finishes.

Because step 3 is the only difference between followers, swapping new PIDFollower(...) for new PedroFollower(...) changes the behavior while everything else stays identical.

---

Stage 1 — Geometry: describing where to go

Vectors and poses (geometry/Vector, geometry/Pose)

Vector is an immutable 2D field vector with the usual operations (add, scale, dot, rotate, fromPolar). Pose is a position plus heading. Pose.normalizeAngle wraps angles to [-π, π] so heading errors always take the shortest way around (turning from 170° to -170° is +20°, not -340°).

Bezier curves (geometry/BezierCurve)

A path's shape is a Bezier curve defined by control points. The curve is evaluated with De Casteljau's algorithm — repeated linear interpolation between control points — which is numerically stable for any degree. From the curve we derive everything the followers need:

• getPoint(t) — the point on the curve at parameter t.
• getDerivative(t) — the tangent (direction of travel). Mathematically the derivative of a degree-n Bezier is a degree-(n-1) Bezier on the control-point differences, so we reuse De Casteljau on those.
• getSecondDerivative(t) — used for curvature.
• getCurvature(t) = (x'·y'' − y'·x'') / |d'|³ — signed; positive means the curve bends left. This feeds the Pedro follower's centripetal term.
• length() — arc length, estimated by sampling 100 points and summing the chords.
• getClosestT(target) — projects the robot onto the curve. It samples coarsely to find the nearest point, then runs a few Newton iterations on the squared-distance gradient to refine t.

BezierLine is a 2-point (straight) curve; BezierPoint is a degenerate single-point curve used for hold-position behavior.

Heading interpolation (geometry/HeadingInterpolator)

Translation and heading are decoupled — a mecanum robot can face any direction while moving any direction. A HeadingInterpolator decides the goal heading at each t:

• constant — face one heading the whole path.
• linear — sweep smoothly from a start to an end heading (uses normalizeAngle to take the short way).
• tangent — face along the path tangent (optionally offset, or reversed to drive backward).

Path and PathChain (geometry/Path, geometry/PathChain, geometry/PathBuilder)

A Path bundles one curve + one heading interpolator + the end conditions that decide when it's "done" (t-value threshold, translational/heading/velocity tolerances, and a settle timeout). Defaults come from FollowerConstants so they're tunable in one place.

A PathChain is an ordered list of Paths run back-to-back. PathBuilder is the fluent API for building one:

PathChain chain = new PathBuilder()
        .addLine(new Vector(0, 0), new Vector(24, 0))
            .setConstantHeadingInterpolation(0)
        .addCurve(new Vector(24, 0), new Vector(40, 0), new Vector(48, 24))
            .setLinearHeadingInterpolation(0, Math.toRadians(90))
        .build();

---

Stage 2 — Localization: knowing where you are

A Localizer reports the robot's field pose and velocity. update() is called once per loop; the factory Localizers.create() builds the one chosen in LocalizerConstants.

PinpointLocalizer

Wraps a goBILDA Pinpoint computer, which fuses two odometry pods with an internal IMU and reports pose directly. The wrapper just maps the Pinpoint frame to the library convention (Pinpoint X → field x forward, Pinpoint Y → field y left, heading in radians) and converts to inches.

ThreeDeadWheelLocalizer

Computes pose itself from three encoders + the hub IMU:

1. Two parallel pods measure forward travel (averaged); one perpendicular pod measures strafe.
2. Heading comes from the IMU (more reliable than integrating the wheels, which drift on slip).
3. The perpendicular pod's reading is corrected for the part caused purely by rotation about the tracking center (strafe − perpOffset·Δheading).
4. The robot-frame displacement is integrated with a pose-exponential (constant-curvature) model, so motion along a curve is tracked accurately rather than as a straight chord.

setPose lets you align odometry to the field at the start of autonomous (it stores a heading offset so the IMU's absolute yaw maps to your chosen starting heading).

---

Stage 3 — Control: deciding how hard to push

These are the building blocks the followers combine. All live in control/.

• PIDController — proportional-integral-derivative feedback. Hardened per the FRC controls book: real dt in seconds with a first-loop guard, integral anti-windup clamping, output saturation limits, and a tolerance band that zeroes tiny errors to stop jitter.
• SquIDController — output is sqrt(kP·error)·sign(error). It grows with the square root of error, so it pushes hard far away but eases in near the target without needing a D term.
• Feedforward — kS·sign(v) + kV·v + kV2·v·|v| + kA·a. This is open-loop: given a desired velocity and acceleration it predicts the power needed. kS/kV/kA are the FRC book §5.10 terms (static friction + velocity + acceleration). kV2·v·|v| is an optional quadratic-drag term (off by default, see below).
• TrapezoidProfile — given a distance and velocity/acceleration limits, produces the commanded position/velocity/acceleration at any time: accelerate, optionally cruise, decelerate to a stop. From FRC book appendix H.4. Kept for reference; the FeedforwardFollower now uses the S-curve.
• SCurveProfile — a jerk-limited version of the trapezoid: the acceleration itself ramps at a bounded rate maxJerk (seven phases) instead of stepping instantly. Smoother, less wheel slip. As maxJerk → ∞ it collapses back to the trapezoid.
• KinematicLimiter — an acceleration-aware speed cap for the followers that have no profile of their own. Each loop it returns the fastest safe speed: brake via v=√(2·a·d) into the target and ramp up via a slew-rate limit Δv ≤ a·dt on launch.

Feedback vs feedforward: feedback (PID/SQuID) reacts to error after it happens; feedforward predicts the effort before error appears. The best follower (FeedforwardFollower) uses feedforward for the bulk of the effort and a small PID to clean up the residual — the standard motion-profiled control structure.

Acceleration accounting — four mechanisms, and how they differ

This library accounts for acceleration in four distinct ways. They are not alternatives to each other; they operate at different points in the pipeline, and a follower may use more than one.

Mechanism	What it is	Acts on	Used by	Tune
Linear accel feedforward kA·a	Predicts the extra power to change velocity at a commanded rate. Needs a profile to supply a. Voltage↔accel is linear on FTC drivetrains, so this term stays linear (not quadratic).	the commanded acceleration setpoint	FeedforwardFollower	kA
S-curve / jerk limit	Shapes the profile so acceleration ramps at ±maxJerk instead of jumping. Trades a little time for smoothness; cuts wheel slip and shake.	the motion profile	FeedforwardFollower	maxJerk
Kinematic velocity cap	Gives feedback followers an accel-aware speed limit: brake v=√(2·a·d), ramp Δv ≤ a·dt. Caps how hard they push so they don't demand impossible accelerations.	the commanded speed	PID, SQuID, Pedro	maxAcceleration (Pedro braking also zeroPowerAccelerationMultiplier)
Quadratic drag kV2·v·|v|	A v²-resistance feedforward term (F_drag = ½ρ·Cd·A·v²) for fast robots. Off by default. Only meaningful where a velocity feedforward exists — it cannot attach to pure PID/SQuID feedback, which output power straight from position error with no velocity to apply it to.	the feedforward output	FeedforwardFollower (when useQuadraticDrag)	useQuadraticDrag, kV2

The short version:

• kA is the acceleration feedforward — it's the term that literally costs power to accelerate, and it stays linear because motor voltage↔acceleration is linear.
• The S-curve is a smoothness upgrade to profiling (it limits jerk, the rate of change of acceleration).
• The kinematic cap is how the non-profiled followers (PID/SQuID/Pedro) become acceleration-aware at all — it's a speed limit derived from kinematics, not a feedforward.
• Quadratic drag is velocity-squared resistance, not acceleration; it's optional polish for fast robots and rides on top of the feedforward, never on raw feedback.

---

Stage 4 — Drive: spinning the wheels (drive/MecanumDrive)

The follower outputs a field-relative translation vector + a turn. The drive:

1. Rotates the field vector into the robot frame by −heading (so "drive toward field +x" works no matter which way the robot faces).
2. Mixes forward / strafe / turn into four wheel powers with the standard mecanum equations.
3. Normalizes — if any wheel exceeds 1.0, all four are scaled down together so the direction of travel is preserved (you never lose the requested heading just because you asked for too much power).
4. Applies the global maxPower cap and writes the motors.

Motor names, per-motor reverse flags, brake/coast, and a strafe multiplier all come from DriveConstants.

---

The four followers (follower/)

All extend Follower, which runs the shared loop above and the path-end logic. Each implements only computeDriveSignal(path, t, pose).

PIDFollower — the "normal control system"

Drives straight at the path's end point: a translational PID on the distance to the target produces the vector magnitude, a heading PID produces the turn. Simple and predictable; ideal for lines and point-to-point moves. (It aims at the end point, so it cuts corners on curves — use Pedro for those.) A KinematicLimiter caps the speed so it ramps acceleration in on launch and brakes into the end rather than slamming to full PID output.

SquIDFollower

Identical structure to PIDFollower (including the KinematicLimiter speed cap) but with SquIDControllers. One gain per axis, gentle braking into the target, no derivative term to tune.

FeedforwardFollower — FRC style

On each new path it builds an SCurveProfile (jerk-limited) over the path's arc length. Every loop it asks the profile for the commanded position/velocity/acceleration at the current time, maps the commanded position to a target point on the path, and outputs:

• a feedforward vector along the path tangent sized by kS/kV/kA (plus optional kV2 drag) from the profile's velocity and acceleration (does most of the work), plus
• a correction PID vector pulling the robot onto the profiled target point, plus
• a heading PID.

The path only completes once the profile has finished and the robot has settled at the end.

PedroFollower — vector combining

Tracks curved paths tightly by summing three field-relative vectors plus a heading correction:

1. Drive vector — along the path tangent, scaled by an acceleration-limited velocity curve (a KinematicLimiter): it ramps up on launch and brakes via v = sqrt(2·a·d) (where d is the remaining distance) so it slows smoothly into the end. Braking uses the tuned zeroPowerAccelerationMultiplier; ramp-up uses the plain maxAcceleration.
2. Centripetal vector — perpendicular to the tangent, centripetalScaling · mass · v² · curvature, which counteracts the robot sliding to the outside of a curve.
3. Translational correction — a PID pulling the robot back onto the closest point of the path.

The drive normalizes the sum, so all three blend into one motion.

Path-end detection (shared)

A path is "done" when any motion profile has finished, the robot is within t-value and translational tolerance of the end, and either it has settled (velocity + heading within tolerance) or a settle timeout elapses as a failsafe. holdPoint(pose) follows a single-point path and never auto-completes, so the robot actively holds position.

---

Constants & FTC Dashboard

All tunables live in constants/, annotated @Config so FTC Dashboard (http://192.168.43.1:8080/dash) can edit them live without redeploying. Followers re-read their gains every loop, so a Dashboard change takes effect immediately. util/Drawing pushes a field overlay (robot + path) to the same dashboard so you can watch the robot track the path in real time.

File	Holds
DriveConstants	motor names, reverse flags, brake/coast, strafe multiplier, max power
FollowerConstants	all PID/SQuID gains, kS/kV/kA, optional kV2 drag, profile limits (maxVelocity/maxAcceleration/maxJerk), Pedro params, path-end tolerances
LocalizerConstants	localizer choice + Pinpoint and 3-deadwheel configuration, IMU orientation

---

Tuning order

Run the OpModes in the pathing-tuning group in order, editing constants live in Dashboard:

1. Localization Test — verify pose tracks reality (fix direction flags in LocalizerConstants).
2. Forward/Strafe Push Test — set distance scaling / ticks-per-inch.
3. Translational PID — tune FollowerConstants.translational*.
4. Heading PID — tune FollowerConstants.heading*.
5. Feedforward (kS/kV/kA) — only for FeedforwardFollower.
6. SQuID kP — only for SquIDFollower.

Then polish the Pedro follower's centripetalScaling, mass, and zeroPowerAccelerationMultiplier on real curves.

Acceleration tuning (after the above)

These all default to safe values; tune only if you see slip, overshoot, or velocity-tracking error.

• maxAcceleration (in/s²) — the traction limit shared by the kinematic speed cap (PID/SQuID/ Pedro ramp-up and braking) and the profile. Raise until the wheels start to slip on launch/stop, then back off ~20%.
• maxJerk (in/s³, FeedforwardFollower only) — how fast acceleration is allowed to change. Start high (near-trapezoid); lower it until the start/stop transitions stop jerking and the wheels stop chirping. Lower = smoother but slightly slower.
• useQuadraticDrag + kV2 (FeedforwardFollower only, fast robots) — leave off unless the robot tops out fast enough that kV·v under-predicts power at high speed (the profiled velocity lags the target at the top end). Then set useQuadraticDrag = true and raise kV2 (power/(in/s)²) until high-speed velocity tracking flattens out. On slow robots this is negligible — leave it off.

---

Quick start

1. In DriveConstants, set the four motor names and reverse flags for your robot.
2. In LocalizerConstants, choose and configure your localizer.
3. Run Tuning 1: Localization Test and confirm the pose is correct.
4. Run an OpMode from the pathing-examples group (start with Straight Line (PID)):

Follower follower = new PedroFollower(hardwareMap);   // or PIDFollower / SquIDFollower / FeedforwardFollower
follower.setStartingPose(new Pose(0, 0, 0));
follower.followPath(chain);
while (opModeIsActive() && follower.isBusy()) {
    follower.update();
}

5. Tune in the order above until paths run cleanly.

Examples (pathing-examples group)

OpMode	Shows
Straight Line (PID)	basic point-to-point with the PID follower
Bezier Curve (Pedro)	following a curve with the vector follower
Path Chain (Pedro)	multi-segment chain + all three heading modes
SQuID Follow	move + turn with the SQuID follower
Feedforward Profile (FRC)	motion-profiled move with kS/kV/kA
Hold Point	actively holding a pose; push it and it returns
Field-Centric TeleOp	driving the drive + localizer directly, no follower

Dependencies

FTC Dashboard (com.acmerobotics.dashboard:dashboard:0.4.16) is added in build.dependencies.gradle with the bundled RobotCore excluded so it uses the SDK's copy.