A PID class (with methods) implemented for Principles of Engineering, Olin College of Engineering.

PIDClass.cpp

class PID_Controller {
public:
		// initialize variables used in the loop
		float previousError = 0;
		float error = 0;

		float integral = 0;
		float derivative = 0;

		float output = 0;

		// tuning params
		float setPoint;
		float Kp;
		float Ki;
		float Kd;

		float wheelBase = 10; // cm

		unsigned long lastRun;

		// linear velocity
		float linVel = 50.0; // hey how fast is this?

		// initialization function for tuning PID
		PID_Controller(float setPoint, float Kp, float Ki, float Kd);

		// empty constructor for initialization
		void setPIDConstants(float setPoint, float Kp, float Ki, float Kd);

		// Defines a function that steps the motor speeds based on PID
		float loopStep(float leftSensor, float rightSensor, Speeds *motor_speed);
};



// ...



PID_Controller::PID_Controller(float setPoint_new, float Kp_new, float Ki_new, float Kd_new) {
		// construct!
		// PID_Controller PID = PID_Controller(5.0, 6.0, 6.0, 6.0)

		setPoint = setPoint_new;
		Kp = Kp_new;
		Ki = Ki_new;
		Kd = Kd_new;
		lastRun = micros();
};


void PID_Controller::setPIDConstants(float setPoint_new, float Kp_new, float Ki_new, float Kd_new) {
		// change the PID constants used in the live loop

		setPoint = setPoint_new;
		Kp = Kp_new;
		Ki = Ki_new;
		Kd = Kd_new;
}


float PID_Controller::loopStep(float leftSensor, float rightSensor, Speeds *motor_speed) {
		// Takes in the current left and right sensor values every loop
		// Runs a step to update the motor speeds (speed) based on the error in the PID loop
		// output represented as an angular velocity

		// timing math
		unsigned long now = micros();
		unsigned long dt = now - lastRun;
		lastRun = now;

		// run the loop
		// 	error = setpoint - measured_value <- difference btw sensors
		error = setPoint - (leftSensor - rightSensor); // +- 1023
		// 	integral = integral + error * dt
		integral = integral + (error * dt);
		// 	derivative = (error - previous_error) / dt
		derivative = (error - previousError) / dt;
		// 	output = Kp * error + Ki * integral + Kd * derivative
		output = (Kp * error) + (Ki * integral) + (Kd * derivative);
		// 	previous_error = error
		previousError = error;

		// set speed with speed.left // speed.right = output;
		// idea: output of PID as angular velocity

		// Vl = s - (w d)/2
		// vr = s + (w d)/2
		// minimum of output: 0
		// maximum of output: 
		motor_speed->left  = linVel - (output * wheelBase) / 2;
		motor_speed->right = linVel + (output * wheelBase) / 2;

		return output;


};