Lab 5 Linear PID Control

P / I / D Controller Design

Initial Proportional Controller

I began with a proportional-only controller. My initial proportional gain estimate was chosen from the desired relationship between a proportional error of 1, proportional error = (tof reading - setpoint)/setpoint, and commanded PWM. Specifically, I wanted a proportional error of approximately 1 to correspond to a motor input near 75 PWM, so I used an initial guess of KP = 75. For initial safety testing on the physical robot, I reduced this slightly to KP = 70. The proportional controller multiplied the normalized error by the gain and then converted the result to a commanded PWM input. Positive error produced a forward command, while negative error caused the robot to back up.

I also bounded the controller output with practical motor-drive limits. For large errors, I capped the PWM at 90 so that the robot would not accelerate aggressively into a wall. For moderate errors, I imposed a minimum command around 35 PWM whenever the error too small, since values below that range were typically insufficient to overcome friction and move the chassis. This produced a controllable and physically safe first implementation. The code for which is below.

        float control = kp * abs_error;
        int pwm_val = (int)control;

        if (abs_error > 1.5f) {
            pwm_val = 80;
        } else {
            if (pwm_val > 80) pwm_val = 80;
            if (pwm_val < 35 && abs_error > 0.05f) pwm_val = 35;
            if (abs_error < 0.05f) pwm_val = 0;
        }

        if (pwm_val == 0) {
            car.kill();
            signed_pwm = 0;
        } else if (error > 0.0f) {
            car.fwd(control_step_ms, pwm_val);
            signed_pwm = pwm_val;
        } else {
            car.back(control_step_ms, pwm_val);
            signed_pwm = -pwm_val;
        }
     

Proportional Control Video

Below is a proportional control demonstration. This was the first controller that was verified to work reliably on the robot.

Derivative Term and PD Testing

After validating proportional control, I extended the controller to include derivative action primarily to confirm that the derivative term was behaving as expected. The derivative component contributes control effort in response to the rate of change of error, so it is most useful during transient response and when responding to purturbations. In practice, I also implemented extrapolation between valid ToF readings by using the difference in time and the recent change in response to estimate how the error was changing between tof readings. This improved responsiveness when the sensor had not yet produced a fresh reading.

Integral Term and Final PID Controller

I then implemented a PID controller. The integral term accumulates error over time and multiplies that accumulated value by KI, which helps eliminate steady state offset by centering the response on the desired setpoint. I tuned the final controller using heuristic method 2 discussed in class. The final gain set that produced the best overall behavior was:

• K_P = 70
• K_D = 0.02
• K_I = 2

These values provided a reasonable balance between responsiveness and settling. The proportional term produced the primary control response, the derivative term damped transient response, and the integral term reduced steady-state error. My implementation at this stage did not include explicit windup protection for integral accumulation, but it was sufficient to demonstrate the use of the full PID controller on my robot.

Representative Code

Bluetooth-Based PID Command Structure

The PID controller was implemented as a Bluetooth command case that accepted a setpoint and controller gains from Python. The loop remained active until a separate stop command was received, allowing me to perturb the robot during a run and observe whether the controller would recover after the initial transient.

        int now_ms = millis();

        float dt_control = (now_ms - last_control_ms) / 1000.0f;

        float error = pid_last_error;
        float derivative = 0.0f;
        int signed_pwm = pid_last_pwm;
        int tof_value = pid_last_tof;

        // ToF update
        if (tof1.checkForDataReady()) {
            int distance = tof1.getDistance();
            tof1.clearInterrupt();

            tof_value = distance;
            error = ((float)distance - (float)setpoint) / (float)setpoint;

            if (pid_log_count >= 1) {
                float last_error = pid_error_array[pid_log_count - 1];
                derivative = (error - last_error) / dt_control;
            }
        } // get error values to predict new error
        else {
            if (pid_log_count >= 2) {
                float last_error = pid_error_array[pid_log_count - 1];
                float prev_error = pid_error_array[pid_log_count - 2];

                int last_time = pid_time_ms_array[pid_log_count - 1];
                int prev_time = pid_time_ms_array[pid_log_count - 2];

                float dt_err = (last_time - prev_time) / 1000.0f;
                if (dt_err > 0.0f) {
                    float error_rate = (last_error - prev_error) / dt_err;
                    float dt_since_last = (now_ms - last_time) / 1000.0f;

                    error = last_error + error_rate * dt_since_last;
                    derivative = error_rate;
                    tof_value = pid_tof_array[pid_log_count - 1];
                }
            }
        }

        integral_error += error * dt_control;

        float abs_error = fabs(error);
        float abs_derivative = fabs(derivative);

        float control = kp * abs_error + kd * abs_derivative + ki * fabs(integral_error);
        int pwm_val = (int)control;
    

Results

ToF vs Time and Motor Input vs Time

The graph below shows the measured ToF response and commanded motor input over time. This plot was the primary diagnostic tool used during controller tuning. It allowed me to compare the measured distance against the setpoint while also seeing how aggressively the controller was commanding the motors and how much it was causing overshoot.

Error vs Time

The error-time plot was especially useful for determining whether the response was converging smoothly or oscillating around the target. In the proportional-only case, the error decreased but did not reject steady-state offset as effectively. The integral term improved this substantially by accumulating the remaining bias over time.

Controller Performance Video

The video below shows the final controller behavior. Although the associated graph from this run is labeled as a P-controller in my local plotting script, the final tuned run shown here used the full PID controller with the gains reported above.