
PID Controller
main.cpp
- Committer:
- Wimboo
- Date:
- 2018-10-22
- Revision:
- 0:d566a0281325
File content as of revision 0:d566a0281325:
#include "mbed.h" #include "HIDScope.h" DigitalOut gpo(D0); DigitalOut led(LED_RED); HIDScope scope(2); const float Kp = 2; const float Ki = 0.2; const float Kd = 0; const float Timestep = 0.001; float G = 15; //input float Output = 0 ; //Starting value float P = 0 ; // Starting value float e1 = 0 ; //Starting value float e2 = 0 ; // Starting value float e3; float Output_Last; // Remember previous position float Y; // Value that is outputted to motor control float P_Last = 0; // Starting position const float Max_Speed = 400; //Max speed of the motor int main() { while (true) { P_Last = P; e1 = e2; e2 = e3; e3 = G - P; Output_Last = Output; Output = Kp * (e3 - e2) + Output_Last +Ki * e3 + Kd * (e3 - 2*e2 + e1); Y = Output; if (Output >= 1){ Y = 1; } else if (Output <= -1){ Y = -1; } P = P_Last + Y * Timestep * Max_Speed; wait(0.1f); scope.set(0,Output); scope.set(1,P); scope.send(); } }