Biorobotics / Robot-Software_jesse

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of Robot-Software by Biorobotics

help_functions/PID_controller.h@0:4cb1de41d049, 2018-10-22 (annotated)

Committer:
MaikOvermars
Date:
Mon Oct 22 12:57:34 2018 +0000
Revision:
0:4cb1de41d049
Child:
1:f1b8f0462e2a
HEuy

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 //Kp = potmeter2.read()*3;
MaikOvermars 0:4cb1de41d049 11
MaikOvermars 0:4cb1de41d049 12 double u_k = Kp * error;
MaikOvermars 0:4cb1de41d049 13
MaikOvermars 0:4cb1de41d049 14 static double error_integral = 0;
MaikOvermars 0:4cb1de41d049 15 static double error_prev = error; // initialization with this value only done once!
MaikOvermars 0:4cb1de41d049 16 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
MaikOvermars 0:4cb1de41d049 17
MaikOvermars 0:4cb1de41d049 18 error_integral = error_integral + error * 1/samplingfreq;
MaikOvermars 0:4cb1de41d049 19 double u_i = Ki * error_integral;
MaikOvermars 0:4cb1de41d049 20
MaikOvermars 0:4cb1de41d049 21 double error_derivative = (error - error_prev)*samplingfreq;
MaikOvermars 0:4cb1de41d049 22 double filtered_error_derivative = LowPassFilter.step(error_derivative);
MaikOvermars 0:4cb1de41d049 23 double u_d = Kd * filtered_error_derivative;
MaikOvermars 0:4cb1de41d049 24 error_prev = error;
MaikOvermars 0:4cb1de41d049 25 return u_k+u_i+u_d;
MaikOvermars 0:4cb1de41d049 26 }