Biorobotics / Robot-Software

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed Servo

help_functions/PID_controller.h@24:53c300d1320e, 2018-10-29 (annotated)

Committer:
bjonkheer
Date:
Mon Oct 29 12:38:04 2018 +0000
Revision:
24:53c300d1320e
Parent:
22:31065a83d9e8
Child:
26:23b0b6fce3a8
Fixed PID

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;
bjonkheer 20:31876566d70f 6 extern double samplingfreq = 1000;
MaikOvermars 0:4cb1de41d049 7
MaikOvermars 17:1f93c83e211f 8 void PID_controller(double error1, double error2, double &u1, double &u2, const double &T)
MaikOvermars 10:7339dca7d604 9 {
bjonkheer 24:53c300d1320e 10 // proportianal part
bjonkheer 24:53c300d1320e 11 double u1_k = Kp * error1;
bjonkheer 24:53c300d1320e 12 double u2_k = Kp * error2;
MaikOvermars 0:4cb1de41d049 13
bjonkheer 24:53c300d1320e 14 static double error1_integral = 0; //
bjonkheer 24:53c300d1320e 15 static double error1_prev = error1; // initialization with this value only done once!
bjonkheer 24:53c300d1320e 16
bjonkheer 24:53c300d1320e 17 static double error2_integral = 0;
bjonkheer 24:53c300d1320e 18 static double error2_prev = error2; // initialization with this value only done once!
bjonkheer 24:53c300d1320e 19
MaikOvermars 0:4cb1de41d049 20 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
MaikOvermars 0:4cb1de41d049 21
bjonkheer 24:53c300d1320e 22 // integral part
bjonkheer 24:53c300d1320e 23 error1_integral = error1_integral + error1 * T;
bjonkheer 24:53c300d1320e 24 double u1_i = Ki * error1_integral;
MaikOvermars 0:4cb1de41d049 25
MaikOvermars 17:1f93c83e211f 26 double error_derivative = (error1 - error_prev) / T;
MaikOvermars 0:4cb1de41d049 27 double filtered_error_derivative = LowPassFilter.step(error_derivative);
MaikOvermars 0:4cb1de41d049 28 double u_d = Kd * filtered_error_derivative;
MaikOvermars 10:7339dca7d604 29 error_prev = error1;
MaikOvermars 10:7339dca7d604 30 u1 = u_k+u_i+u_d;
bjonkheer 24:53c300d1320e 31 error2_integral = error2_integral + error2 * T;
bjonkheer 24:53c300d1320e 32 double u2_i = Ki * error2_integral;
bjonkheer 24:53c300d1320e 33
bjonkheer 24:53c300d1320e 34 // derivative part
bjonkheer 24:53c300d1320e 35 double error1_derivative = (error1 - error1_prev) / T;
bjonkheer 24:53c300d1320e 36 double u1_d = Kd * LowPassFilter.step(error1_derivative); // apply low pass filter to remove noise due to derivative amplification
bjonkheer 24:53c300d1320e 37 error1_prev = error1;
bjonkheer 24:53c300d1320e 38
bjonkheer 24:53c300d1320e 39 double error2_derivative = (error2 - error2_prev) / T;
bjonkheer 24:53c300d1320e 40 double u2_d = Kd * LowPassFilter.step(error2_derivative); // apply low pass filter to remove noise due to derivative amplification
bjonkheer 24:53c300d1320e 41 error2_prev = error2;
bjonkheer 24:53c300d1320e 42
bjonkheer 24:53c300d1320e 43 u1 = u1_k+u1_i+u1_d;
bjonkheer 24:53c300d1320e 44 u2 = u2_k+u2_i+u2_d;
MaikOvermars 0:4cb1de41d049 45 }