GitHubLinkedIn

One-Wheel Odometry and PID on VEX V5

9/8/2025 • 5 min read

Why dig into the math?

The VEX control stack in my repos looks like VEXcode boilerplate at first glance, but the chassis only behaves if the motion math behind it is right. This post breaks down the control theory I leaned on when I built a reusable drivetrain library: how each PID loop is shaped, how odometry works when you only have one passive tracking wheel plus an inertial sensor, and how those equations show up in code you can run on the V5 Brain.

Hardware model

For the math, assume:

  • Tank drivetrain with left and right motor groups.
  • One passive forward tracking wheel with radius r tied to a V5 rotation sensor.
  • An IMU supplying yaw, with angle measured in radians.
  • Loop period T_s (20 ms in my driver control loop).

The tracking wheel sits on the robot centerline, so it measures fore/aft translation only. Lateral motion relies on the IMU.

PID refresher

Every motion primitive in the library boils down to a proportional-integral-derivative controller. Continuous form:

u(t)=Kpe(t)+Ki∫0te(τ)dτ+Kdde(t)dtu(t) = K_p e(t) + K_i \int_0^t e(\tau) d\tau + K_d \frac{d e(t)}{dt}

Discrete, inside the V5 event loop:

e[k]=r[k]−y[k]I[k]=clamp⁡(I[k−1]+e[k]Ts)D[k]=e[k]−e[k−1]Tsu[k]=Kpe[k]+KiI[k]+KdD[k]\begin{aligned} e[k] &= r[k] - y[k] \\ I[k] &= \operatorname{clamp}(I[k-1] + e[k] T_s) \\ D[k] &= \frac{e[k] - e[k-1]}{T_s} \\ u[k] &= K_p e[k] + K_i I[k] + K_d D[k] \end{aligned}

Clamping the integral prevents windup when the chassis saturates the motors. Each motion type has its own gain set:

  • drive: distance error from left/right wheel encoders or odometry.
  • heading: error between desired heading and IMU measurement.
  • turn: rotates in place with differential voltage.
  • swing: locks one side and pivots on the other.

Exit conditions stop a loop when both error and derivative sit inside tolerances for a minimum time, so the robot settles instead of ringing.

Odometry with one wheel + IMU

Let the tracking wheel measure arc length s and the IMU measure yaw theta. Between loop iterations k-1 and k:

Δs=r⋅(ϕk−ϕk−1),Δθ=θk−θk−1\Delta s = r \cdot (\phi_k - \phi_{k-1}), \quad \Delta \theta = \theta_k - \theta_{k-1}

The incremental body-frame displacement is v_b = [Δs, 0]^T. Rotate it into the global frame using the midpoint heading:

R(ψ)=[cos⁡ψ−sin⁡ψsin⁡ψcos⁡ψ],ψ=θk−1+12ΔθR(\psi) = \begin{bmatrix} \cos \psi & -\sin \psi \\ \sin \psi & \cos \psi \end{bmatrix}, \quad \psi = \theta_{k-1} + \frac{1}{2}\Delta \theta vg=R(ψ)vb\mathbf{v}_g = R(\psi) \mathbf{v}_b

Pose update:

xk=xk−1+vg,xyk=yk−1+vg,yθk=θk−1+Δθ\begin{aligned} x_k &= x_{k-1} + v_{g,x} \\ y_k &= y_{k-1} + v_{g,y} \\ \theta_k &= \theta_{k-1} + \Delta \theta \end{aligned}

Even with a single wheel, this captures forward motion accurately and lets the IMU absorb drift. Adding lateral wheels later simply gives direct measurement of Δy.

Drive-to-point control

To drive toward field coordinate (x_d, y_d):

Δx=xd−xΔy=yd−yed=(Δx)2+(Δy)2θd=atan2⁡(Δy,Δx)eθ=θd−θ\begin{aligned} \Delta x &= x_d - x \\ \Delta y &= y_d - y \\ e_d &= \sqrt{(\Delta x)^2 + (\Delta y)^2} \\ \theta_d &= \operatorname{atan2}(\Delta y, \Delta x) \\ e_\theta &= \theta_d - \theta \end{aligned}

Distance PID produces a forward voltage u_drive, heading PID produces a correction u_turn:

udrive=PIDd(ed)uturn=PIDθ(eθ)VL=udrive−uturnVR=udrive+uturn\begin{aligned} u_{\text{drive}} &= \text{PID}_d(e_d) \\ u_{\text{turn}} &= \text{PID}_\theta(e_\theta) \\ V_L &= u_{\text{drive}} - u_{\text{turn}} \\ V_R &= u_{\text{drive}} + u_{\text{turn}} \end{aligned}

Near the target, I clamp u_drive to avoid overshoot and lower heading gains so the robot glides into place.

Example implementation sketch

// inside a timed loop running every Ts seconds
const double drive_error = target_distance - odom.distance_traveled();
const double drive_cmd = drive_pid.step(drive_error);
 
const double heading_error = wrapRadians(target_heading - imu.rotation(rad));
const double turn_cmd = heading_pid.step(heading_error);
 
const double left_voltage = drive_cmd - turn_cmd;
const double right_voltage = drive_cmd + turn_cmd;
 
chassis.drive_with_voltage(left_voltage, right_voltage);

The helper wrapRadians keeps e_theta in [-pi, pi] to prevent the controller from commanding a 350 degree spin when 10 degrees the other way is faster.

Tuning workflow

  1. Start with K_d = 0. Raise K_p until the robot reaches the target quickly but overshoots slightly.
  2. Add K_d to dampen overshoot. Increase until the oscillation vanishes.
  3. Introduce a small K_i if the drivetrain cannot reach the target (static friction), but guard it with integral clamp.
  4. Adjust exit tolerances so the competition robot holds points inside field tolerances without burning match time.
  5. Record loops at match voltage. Drivetrains sag under battery load; retune if the battery pack changes.

Lessons learned

  • Math first, configuration second: once the equations were locked in, swapping wheel diameter or tracking offsets was a quick constant edit.
  • IMU fusion cleans up encoder slip, especially when driving over discs or triballs. Reset the gyro any time the robot is lifted.
  • One tracking wheel is enough for simple tank bots. When we tested an X-drive, adding the sideways wheel slotted into the same equations—just another component in v_b.
  • Codifying PID profiles in default_constants() gave the whole team a safe baseline. From there we tweak by logging error curves during practice runs.