Turning ROT_PID into a Callable Function RPID
I converted the ROT_PID case into a standalone callable function, RPID(float s). This means rotational control is no longer tied to a BLE command, any other case can just call it directly and the program blocks until the turn finishes. The end condition is getting within 2 degrees of the setpoint, checked continuously via gyro integration, which makes the rotation genuinely closed loop when it's embedded inside a bigger maneuver.
One deliberate choice was having the function immediately drive forward once the turn completes rather than stopping. This speeds up the exit from the drift and carries momentum into the final straight.
void RPID(float s) {
float setpoint = s;
float kp = 2.0f;
float ki = 0.07f;
int max_pwm = 60;
int min_pwm = 45;
float integral_error = 0.0f;
float yaw_angle_deg = 0.0f;
float gyro_z_bias = 0.0f;
for (int i = 0; i < 2; i++) {
myICM.getAGMT();
gyro_z_bias += myICM.gyrZ();
}
gyro_z_bias /= 2.0f;
unsigned long last_control_us = micros();
float error = 180.0f;
while (fabs(error) > 1.0f) {
myICM.getAGMT();
unsigned long now_us = micros();
float dt_control = (now_us - last_control_us) / 1000000.0f;
if (dt_control <= 0.0f) continue;
last_control_us = now_us;
float gyro_z_dps = myICM.gyrZ() - gyro_z_bias;
yaw_angle_deg += gyro_z_dps * dt_control;
error = setpoint - yaw_angle_deg;
integral_error += error * dt_control;
if (integral_error > 100.0f) integral_error = 100.0f;
if (integral_error < -100.0f) integral_error = -100.0f;
float control = kp * error + ki * integral_error;
int pwm_val = 0;
if (fabs(error) < 2.0f) {
car.kill();
break;
} else {
pwm_val = (int)fabs(control);
if (pwm_val < min_pwm) pwm_val = min_pwm;
if (pwm_val > max_pwm) pwm_val = max_pwm;
}
if (error > 0.0f) { car.tl(pwm_val); }
else { car.tr(pwm_val); }
}
car.fwd(200);
}
Kalman Filter as a Callable Function
I wrote the Kalman filter logic from Lab 7 into a standalone function that takes a PWM value and a setpoint, and returns a bool for whether the estimated position has crossed the setpoint. The PWM input scales the control input to the motion model, which keeps the filter accurate at the higher speeds used in the stunt. On the first call it blocks to grab a real ToF reading to seed the state vector, then carries kf_mu and kf_sigma forward as globals between calls.
In the end I didn't use the function in the final STUNT case, I inlined the KF logic directly into the case to keep the loop structure explicit and avoid the init path overhead. I kept the standalone function around for use in other future cases though.
bool KF(int signed_pwm, float setpoint) {
if (!kf_init) {
tof1.startRanging();
unsigned long t0 = millis();
while (!tof1.checkForDataReady() && millis() - t0 < 50) { delay(1); }
if (tof1.checkForDataReady()) {
kf_mu(0,0) = (float)tof1.getDistance();
kf_mu(1,0) = 0.0f;
tof1.clearInterrupt();
}
tof1.stopRanging();
tof1.startRanging();
kf_init = true;
}
Matrix<1,1> kf_u = {(float)signed_pwm / 100.0f};
Matrix<2,1> mu_p = Ad * kf_mu + Bd * kf_u;
Matrix<2,2> sigma_p = Ad * kf_sigma * ~Ad + su;
if (tof1.checkForDataReady()) {
int tof_value = tof1.getDistance();
tof1.clearInterrupt();
tof1.stopRanging();
tof1.startRanging();
Matrix<1,1> kf_y = {(float)tof_value};
Matrix<1,1> sigma_m = C * sigma_p * ~C + sz;
Matrix<2,1> K = sigma_p * ~C * Inverse(sigma_m);
Matrix<1,1> y_m = kf_y - C * mu_p;
Matrix<2,2> I2;
I2.Fill(0); I2(0,0) = 1.0f; I2(1,1) = 1.0f;
kf_mu = mu_p + K * y_m;
kf_sigma = (I2 - K * C) * sigma_p;
} else {
kf_mu = mu_p;
kf_sigma = sigma_p;
}
return kf_mu(0, 0) <= setpoint;
}
Putting it Together: The STUNT Case
With RPID callable and the KF logic working, I assembled the full stunt in a 3-phase STUNT case. First, the car drives forward at 200 pwm while the Kalman filter runs inline in the loop to track distance to the wall without ever stalling waiting for a sensor reading. I set the turn to trigger when the KF estimate drops below 609mm, roughly two feet, which is intentionally closer than the three-foot marker in the demo to push the drift harder into the wall. Once the RPID detects the car within 2 degrees of the target angle it immediately kicks into a forward drive, and the case runs that for 1.2 seconds before killing the motors.
case STUNT: {
tof1.setDistanceModeLong();
int setpoint;
if (!robot_cmd.get_next_value(setpoint)) break;
tof1.startRanging();
while (!tof1.checkForDataReady()) { delay(1); }
int first_tof = tof1.getDistance();
tof1.clearInterrupt();
tof1.stopRanging();
Matrix<2,1> kf_mu = {(float)first_tof, 0.0f};
Matrix<2,2> kf_sigma = {100, 0, 0, 100};
tof1.startRanging();
// Phase 1: drive forward until KF estimate drops below 609mm
car.fwd(200);
while (kf_mu(0,0) > 609.0f) {
Matrix<1,1> kf_u = {150.0f / 100.0f};
Matrix<2,1> mu_p = Ad * kf_mu + Bd * kf_u;
Matrix<2,2> sigma_p = Ad * kf_sigma * ~Ad + su;
if (tof1.checkForDataReady()) {
int tof_value = tof1.getDistance();
tof1.clearInterrupt();
tof1.stopRanging();
tof1.startRanging();
Matrix<1,1> kf_y = {(float)tof_value};
Matrix<1,1> sigma_m = C * sigma_p * ~C + sz;
Matrix<2,1> K = sigma_p * ~C * Inverse(sigma_m);
Matrix<1,1> y_m = kf_y - C * mu_p;
Matrix<2,2> I2;
I2.Fill(0); I2(0,0) = 1.0f; I2(1,1) = 1.0f;
kf_mu = mu_p + K * y_m;
kf_sigma = (I2 - K * C) * sigma_p;
} else {
kf_mu = mu_p;
kf_sigma = sigma_p;
}
}
car.kill();
tof1.stopRanging();
// Phase 2: 180 turn via RPID
RPID(setpoint);
// Phase 3: drive forward for 1.2 seconds
unsigned long start = millis();
car.fwd(200);
while (millis() - start < 1200);
car.kill();
break;
}
Demo
The pen in the video is placed exactly three feet from the wall. The car begins its turn at roughly two feet, bringing the drift noticeably closer to the wall than the 3ft marker.
References
I worked with Coby Lai extensively on this lab. I also used AI assistance to check Arduino and HTML syntax.