Speedrunning Labs 5-7

Retesting Kalman Filter and PID Loops

During the course of this lab, my car (previously named Puyo) has been unfortunately relocated to a landfill. Thus, I had to use a previous year's car (henceforth named Puyo Puyo) for this lab. Becuase of this, I had to redo the PID tunings and retest the Kalman filter implementation on the new car. I tuned the angle PID loop with the final parameters of \(K_p = 2.5\), \(K_i = 0\), and \(K_d = 0.2\), which resulted in a very stable loop that was able to keep the car at a set angle with a maximum error of 0.1 degrees on the smooth surface of Tang Hall's floor. For the distance PID loop, I tuned it with the final parameters of \(K_p = 0.065\), \(K_i = 0.001\), and \(K_d = 0.04\), which resulted in a loop that was able to keep the car at a set distance with a maximum error of about 10 mm, which is pretty good considering the noise in the ToF sensor measurements. I also retested the Kalman filter implementation on the new car, and it was able to accurately predict the position of the car without any modifications. Below is a plot of the Kalman filter predictions compared to the raw measurements to show that it is working correctly.

Puyo Puyo's First Appearance (GAS GAS GAS)

Drifts

I switched to drifting, which consists of driving fast, spinning the car around to face the opposite direction, and then driving away. I needed to add a lot of angle control to get the car to do a clean drift, which consisted of a PID loop to control the angle during the drift, but also a pre-emptive angle control while it went towards the wall since sometimes the car would suddenly veer to one side during the ram, which would cause it to hit the wall at an angle and mess up the drift. The main challenge was finding the right distance and speed to drive the car and initiate the drift at to make it not hit the wall. I ended up driving the car at 150 PWM towards the wall. Below are three successful attempts at drifting, as well as the code for the drift attempt.

void DRIFT(float setpoint, float angle_threshold = 2.5)
{
    // move fowrards at a high speed (150) until we're within 3ft (914.4 mm) of the wall,
    // then turn around using rotate pid 180 degrees
    // then move backwards at a high speed (150) until we're more than 3ft from the wall again, then stop

    int distanceB = -1;
    int extrapolated_distanceB = -1;
    float kalman_filtered_distanceB = -1;

    double currYaw = yaw;

    driveCombined(150, 0);
    pid_rotate.reset();
    pid_rotate.setSetpoint(currYaw);
    while (extrapolated_distanceB > setpoint || extrapolated_distanceB == -1)
    {
        get_DMP_data();
        int rotate_output = pid_rotate.compute(yaw);
        driveCombined(150, rotate_output);

        distanceB = getDistance(sensorB);
        extrapolated_distanceB = distanceB;
        kalman_filtered_distanceB = distanceB;
        if (distanceB == -1) { // only compute if we have a valid distance reading
            extrapolated_distanceB = predictDistance();
            kalman_filtered_distanceB = kalmanFilter.step(0, distanceB); 
        }
    }

    double targetYaw = currYaw + 180;
    // wrap target yaw to [-180, 180]
    if (targetYaw > 180) {
        targetYaw -= 360;
    } else if (targetYaw < -180) {
        targetYaw += 360;
    }

    pid_rotate.reset();
    pid_rotate.setSetpoint(targetYaw);
    while (abs(pid_rotate.setpoint - yaw) > angle_threshold)
    {
        get_DMP_data();
        int rotate_output = pid_rotate.compute(yaw);
        driveCombined(0, rotate_output);

        distanceB = getDistance(sensorB);
        extrapolated_distanceB = distanceB;
        kalman_filtered_distanceB = distanceB;
        if (distanceB == -1) { // only compute if we have a valid distance reading
            extrapolated_distanceB = predictDistance();
            kalman_filtered_distanceB = kalmanFilter.step(0, distanceB); 
        }
    }

    // drive the next 2 seconds to make sure we are fully turned around
    int start_time = millis();
    while (millis() - start_time < 2000)
    {
        get_DMP_data();
        int rotate_output = pid_rotate.compute(yaw);
        driveCombined(150, rotate_output);

        distanceB = getDistance(sensorB);
        extrapolated_distanceB = distanceB;
        kalman_filtered_distanceB = distanceB;
        if (distanceB == -1) { // only compute if we have a valid distance reading
            extrapolated_distanceB = predictDistance();
            kalman_filtered_distanceB = kalmanFilter.step(0, distanceB); 
        }
    }

}

Trial 1

Trial 2

Trial 3

Below are the plots of the angle and distance measurements during the drift, and the motor output of the right motor, which show that the car was able to maintain a stable angle during the drift.

RIP PUYO

The Best Ones Die So Young

In place of a Blooper Video I have a funeral video of Puyo. ==========> SEIZURE WARNING <============