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@26:23b0b6fce3a8, 2018-10-29 (annotated)

Committer:
MaikOvermars
Date:
Mon Oct 29 14:53:16 2018 +0000
Revision:
26:23b0b6fce3a8
Parent:
24:53c300d1320e
Child:
45:829a3992d689
updated filter pid_controller;

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 26:23b0b6fce3a8 20 static BiQuad LowPassFilter1(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
MaikOvermars 26:23b0b6fce3a8 21 static BiQuad LowPassFilter2(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
MaikOvermars 0:4cb1de41d049 22
bjonkheer 24:53c300d1320e 23 // integral part
bjonkheer 24:53c300d1320e 24 error1_integral = error1_integral + error1 * T;
bjonkheer 24:53c300d1320e 25 double u1_i = Ki * error1_integral;
MaikOvermars 0:4cb1de41d049 26
bjonkheer 24:53c300d1320e 27 error2_integral = error2_integral + error2 * T;
bjonkheer 24:53c300d1320e 28 double u2_i = Ki * error2_integral;
bjonkheer 24:53c300d1320e 29
bjonkheer 24:53c300d1320e 30 // derivative part
bjonkheer 24:53c300d1320e 31 double error1_derivative = (error1 - error1_prev) / T;
MaikOvermars 26:23b0b6fce3a8 32 double u1_d = Kd * LowPassFilter1.step(error1_derivative); // apply low pass filter to remove noise due to derivative amplification
bjonkheer 24:53c300d1320e 33 error1_prev = error1;
bjonkheer 24:53c300d1320e 34
bjonkheer 24:53c300d1320e 35 double error2_derivative = (error2 - error2_prev) / T;
MaikOvermars 26:23b0b6fce3a8 36 double u2_d = Kd * LowPassFilter2.step(error2_derivative); // apply low pass filter to remove noise due to derivative amplification
bjonkheer 24:53c300d1320e 37 error2_prev = error2;
bjonkheer 24:53c300d1320e 38
bjonkheer 24:53c300d1320e 39 u1 = u1_k+u1_i+u1_d;
bjonkheer 24:53c300d1320e 40 u2 = u2_k+u2_i+u2_d;
MaikOvermars 0:4cb1de41d049 41 }