PART 4: Following a Trajectory
Objective
The goal of this task was to program the VEX V5 robot to follow an arbitrary trajectory defined
by a series of (x, y) waypoints using proportional control (P-control) to compute linear and
angular velocities at each time step. An arbitrary zigzag trajectory was used.
Approach
The sample code provided in class was ported to the VEX V5 robot in Python. At each iteration
of the control loop, the robot computes the distance and heading error to the next waypoint and
applies P-control to determine the linear velocity, v and angular velocity, ω. These are then
converted to individual wheel RPMs via inverse kinematics expressed in Equations 1 and 2.
d ×ω
vl = v - (1)
2
d ×ω
vr = v + (2)
2
The missing blocks, inverse kinematics and wheel velocity commands were filled in, and the
robot parameters (wheel radius = 5.08 cm, baseline = 29 cm) were set to match the physical
robot.
Parameter Tuning
Time Step (dt)
Three values were tested: dt = 0.1, 0.05, and 0.01 seconds. Larger values caused the Euler
integration to be inaccurate, the robot overshot waypoints and accumulated heading error
quickly. dt = 0.01 gave the smoothest trajectory tracking and was adopted for all subsequent
tests.
Gains (k_v and k_ω)
Tuning was done in two stages. First, k_v was fixed at 0.5 and k_ω was varied between 3 and 7.
It was observed that higher k_ω caused oscillation on straight segments, the robot would
overcorrect its heading repeatedly. k_ω was then swept downward. k_v of 0.4 and k_ω of 4.0
produced the best overall trajectory and were selected as final gains.
Maximum Speed
The motor speed was initially set to full 200 RPM. At full speed the robot moved too fast for the
P-controller to correct heading errors in time, causing it to miss waypoints entirely. 35% of
maximum speed was selected as the final setting achieving better results as compared to 20%,
40% and 50%.
Results
The robot successfully followed the zigzag trajectory with the final parameters. Waypoints were
captured cleanly and heading corrections were smooth without oscillation. Importantly, when the
same parameters were applied to a second, different trajectory without any re-tuning, the robot
followed it equally well. This suggests the gains are not trajectory-specific but rather reflect the
physical dynamics of the robot itself, a desirable property for a general-purpose controller.
Challenges
The primary challenge was the interdependence of the parameters. Changing k_ω alone affected
how aggressively the robot corrected heading, but this interacted with maximum velocity at high
speeds, even a well-tuned k_ω could not correct errors fast enough before the robot had already
passed the waypoint. This meant speed, gains, and dt had to be tuned together rather than
independently, making the process iterative and time-consuming.
A second challenge was translating the simulation to physical hardware. The simulated robot
assumes instantaneous motor response, while the real VEX V5 motors have a small spin-up lag.
This made the physical robot's behavior slightly different from what the simulation predicted,
requiring additional physical testing beyond what the gain sweep suggested.