Created
November 7, 2019 12:48
-
-
Save sdaitzman/91c3ad7e2ab27e058901f66b3829aefe to your computer and use it in GitHub Desktop.
A PID class (with methods) implemented for Principles of Engineering, Olin College of Engineering.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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; | |
}; |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment