Lab 8: Stunts!

Approach Phase — Speed PID (Extra Credit)

During approach, instead of just a constant PWM, a speed PID uses the KF velocity estimate as feedback to track a target speed (mm/s). This means a slightly drained battery doesn't make the robot trigger late, and tile vs. carpet doesn't really change the behavior. The output is clamped between the deadband and a max PWM cap so the integrator can't wind up forever also.

float speed_error = approach_speed - kf_velocity;
speed_integral   += speed_error * dt;
speed_integral    = constrain(speed_integral, -2000.0, 2000.0);
float speed_deriv = (speed_error - speed_prev_err) / dt;

float pid_out = speed_Kp * speed_error
              + speed_Ki * speed_integral
              + speed_Kd * speed_deriv;
motor_pwm = constrain(pid_out, (float)DEADBAND, approach_pwm_max);

Kalman Filter Reuse

The KF from Lab 7 was reused as-is. Predict runs every loop using the current motor PWM (normalized by step size) as input, and update only runs when the ToF actually has a fresh reading. This is what lets the controller make decisions at ~100Hz instead of being stuck at the ~50Hz ToF rate, which matters a lot when you're driving fast and need to trigger the turn at exactly the right distance.

Task B: Drift

For the drift, the orientation PID from Lab 6 is reused inside the turning phase. When the KF distance drops below the trigger threshold, the current yaw is captured and the setpoint is set to yaw + 180. The PD controller then spins the robot in place until the error drops inside the tolerance, and then return takes over.

void run_drift_turn() {
    ori_error = ori_setpoint - yaw;
    while (ori_error >  180.0) ori_error -= 360.0;
    while (ori_error < -180.0) ori_error += 360.0;

    if (abs(ori_error) < ORI_TOLERANCE) {
        stunt_state = STUNT_RETURN;
        stunt_phase_start = millis();
        drive_straight(return_pwm);
        return;
    }
    ori_deriv_filtered = ORI_DERIV_ALPHA * current_gyro_z
                       + (1.0 - ORI_DERIV_ALPHA) * ori_deriv_filtered;
    float p_term = ori_Kp * ori_error;
    float d_term = -ori_Kd * ori_deriv_filtered;
    spin((int)constrain(p_term + d_term, -255.0, 255.0));
}

The orientation Kp was bumped up from Lab 6 so the turn finishes fast enough to look like an actual drift instead of a slow twist in place.

Tuning Process

Tuning went roughly like this:

• Started with conservative approach speed (~1500 mm/s) and a high trigger distance to make sure nothing crashed on run 1.
• Bumped target speed up gradually until the approach looked aggressive but the robot still hit the trigger reliably.
• Pulled the trigger distance down step by step until the timing of the turn lined up with the geometry the lab asked for.
• Adjusted the orientation Kp to make the maneuver actually complete cleanly.
• Caused a bunch of bloopers along the way of increasingly strange variety.
• Also some of the runs are without PID, even though I did make and use it. It helped me get very clean runs but often broke.
• Oh! And the discrete Ad and Bd matrices are recomputed each loop iteration using the actual measured dt, so the KF stays consistent even when loop timing varies.

Results

The robot successfully completed the stunt at least three times. Plots from one of the cleaner runs, ignore the end where I kicked it by accident.

Figure: Top — raw ToF (dots) and KF estimated distance (line) vs time. The vertical dashed line marks the moment the trigger fires and the state machine transitions out of APPROACH. Bottom — KF velocity estimate, showing the speed PID holding through the approach.

Figure: Motor PWM over time, color-coded by state. APPROACH holds steady positive PWM, then drops to a spin command during TURNING, then ramps back up to return PWM. Yaw trace overlaid shows the full 180° rotation.

The approach phase was really fast, according to data I received back on bluetooth, the KF triggered the turn at 940mm (right around the 914mm/3ft target) after only 0.58s from the start command, with a peak KF velocity of 4132 mm/s (~4.1 m/s). The turn started at 0.58s and went for about a second. The yaw at the moment of return was 212°, which matches the fact that I bumped the yaw setpoint up to 210° to overshoot slightly and account for the robot not quite hitting exactly 180°. Total runtime was 3 seconds, which is honestly in time with the ~3–4.5s that the previous years' reports reference.

Demonstration

Three successful runs:

Video: Three successful stunt runs.

Video: Bloopers where my robot randomly spasms for no apprent reason?! I swear I was trying to do the normal lab when this happened. And then it never happened again