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@21:1321ff770f99, 2018-10-26 (annotated)

Committer:
MaikOvermars
Date:
Fri Oct 26 15:20:57 2018 +0000
Branch:
bla
Revision:
21:1321ff770f99
Parent:
17:1f93c83e211f 
Adjusted PID_controller. Now calculates both u1 and u2. Code should be working now.

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