Task 2: Implement the PID Algorithm

Objective

Write the PID controller logic using proportional, integral, and derivative terms. The controller should compute the control output based on the error between the setpoint and measured ball position.

Implementation Guidance

• Define global variables for Kp, Ki, and Kd
• Compute error as setpoint minus measured position
• Accumulate the integral term using error multiplied by dt
• Compute the derivative term using the change in error divided by dt
• Store the previous error for the next iteration
• Constrain the control output to safe actuator limits
• Implement integral windup protection by clamping the integral term

// controller.h#ifndef CONTROLLER_H#define CONTROLLER_Hvoid initController(float kp, float ki, float kd);float computePID(float setpoint, float measured, float dt);void resetController();

#endif

// controller.cpp#include "controller.h"static float Kp, Ki, Kd;static float previousError = 0;static float integral = 0;const float INTEGRAL_MAX = 100.0;  // Anti-windup limitconst float OUTPUT_MAX = 255.0;const float OUTPUT_MIN = -255.0;void initController(float kp, float ki, float kd) {    Kp = kp;    Ki = ki;    Kd = kd;    resetController();}void resetController() {    previousError = 0;    integral = 0;}

float computePID(float setpoint, float measured, float dt) { // Calculate error float error = setpoint - measured; // Proportional term float P = Kp * error; // Integral term with anti-windup integral += error * dt; if (integral > INTEGRAL_MAX) integral = INTEGRAL_MAX; if (integral < -INTEGRAL_MAX) integral = -INTEGRAL_MAX; float I = Ki * integral; // Derivative term float derivative = (error - previousError) / dt; float D = Kd * derivative; previousError = error; // Compute total output float output = P + I + D; // Constrain to actuator limits if (output > OUTPUT_MAX) output = OUTPUT_MAX; if (output < OUTPUT_MIN) output = OUTPUT_MIN; return output;}

Anti-Windup Protection

Integral windup occurs when the integral term accumulates to very large values during sustained errors (e.g., when the ball is stuck or the system is saturated). This can cause massive overshoot when the error finally reduces.

Solution: Clamp the integral term to a maximum value to prevent runaway accumulation.