Biorobotics / Robot-Software_jesse

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of Robot-Software by Biorobotics

help_functions/PID_controller.h@13:3482d315877c, 2018-10-29 (annotated)

Committer:
Peppypeppy
Date:
Mon Oct 29 12:55:38 2018 +0000
Revision:
13:3482d315877c
Parent:
10:7339dca7d604 
hello;

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;
Peppypeppy 13:3482d315877c 6 extern double samplingfreq = 1000;
MaikOvermars 0:4cb1de41d049 7
Peppypeppy 13:3482d315877c 8 void PID_controller(double error1, double error2, float &u1, float &u2)
MaikOvermars 10:7339dca7d604 9 {
MaikOvermars 10:7339dca7d604 10 double u_k = Kp * error1;
MaikOvermars 0:4cb1de41d049 11
MaikOvermars 0:4cb1de41d049 12 static double error_integral = 0;
MaikOvermars 10:7339dca7d604 13 static double error_prev = error1; // initialization with this value only done once!
MaikOvermars 0:4cb1de41d049 14 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
MaikOvermars 0:4cb1de41d049 15
MaikOvermars 10:7339dca7d604 16 error_integral = error_integral + error1 * 1/samplingfreq;
MaikOvermars 0:4cb1de41d049 17 double u_i = Ki * error_integral;
MaikOvermars 0:4cb1de41d049 18
MaikOvermars 10:7339dca7d604 19 double error_derivative = (error1 - error_prev)*samplingfreq;
MaikOvermars 0:4cb1de41d049 20 double filtered_error_derivative = LowPassFilter.step(error_derivative);
MaikOvermars 0:4cb1de41d049 21 double u_d = Kd * filtered_error_derivative;
MaikOvermars 10:7339dca7d604 22 error_prev = error1;
MaikOvermars 10:7339dca7d604 23 u1 = u_k+u_i+u_d;
MaikOvermars 0:4cb1de41d049 24 }