Biorobotics / Robot-Software_jesse

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of Robot-Software by Biorobotics

help_functions/PID_controller.h@1:f1b8f0462e2a, 2018-10-22 (annotated)

Committer:
SvenD97
Date:
Mon Oct 22 13:02:29 2018 +0000
Revision:
1:f1b8f0462e2a
Parent:
0:4cb1de41d049
Child:
10:7339dca7d604
Removed comment from PID_controller.h;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MaikOvermars 0:4cb1de41d049 1 #include "mbed.h"
MaikOvermars 0:4cb1de41d049 2
MaikOvermars 0:4cb1de41d049 3 double Kp = 7.5;
MaikOvermars 0:4cb1de41d049 4 double Ki = 1.02;
MaikOvermars 0:4cb1de41d049 5 double Kd = 10;
MaikOvermars 0:4cb1de41d049 6 double samplingfreq = 1000;
MaikOvermars 0:4cb1de41d049 7
MaikOvermars 0:4cb1de41d049 8 double PID_controller(double error)
MaikOvermars 0:4cb1de41d049 9 {
MaikOvermars 0:4cb1de41d049 10
MaikOvermars 0:4cb1de41d049 11 double u_k = Kp * error;
MaikOvermars 0:4cb1de41d049 12
MaikOvermars 0:4cb1de41d049 13 static double error_integral = 0;
MaikOvermars 0:4cb1de41d049 14 static double error_prev = error; // initialization with this value only done once!
MaikOvermars 0:4cb1de41d049 15 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
MaikOvermars 0:4cb1de41d049 16
MaikOvermars 0:4cb1de41d049 17 error_integral = error_integral + error * 1/samplingfreq;
MaikOvermars 0:4cb1de41d049 18 double u_i = Ki * error_integral;
MaikOvermars 0:4cb1de41d049 19
MaikOvermars 0:4cb1de41d049 20 double error_derivative = (error - error_prev)*samplingfreq;
MaikOvermars 0:4cb1de41d049 21 double filtered_error_derivative = LowPassFilter.step(error_derivative);
MaikOvermars 0:4cb1de41d049 22 double u_d = Kd * filtered_error_derivative;
MaikOvermars 0:4cb1de41d049 23 error_prev = error;
MaikOvermars 0:4cb1de41d049 24 return u_k+u_i+u_d;
MaikOvermars 0:4cb1de41d049 25 }