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@46:9b60a3c1acab, 2018-11-01 (annotated)

Committer:
Spekdon
Date:
Thu Nov 01 20:54:51 2018 +0000
Revision:
46:9b60a3c1acab
Parent:
45:829a3992d689 
Presentation code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MaikOvermars 0:4cb1de41d049 1 #include "mbed.h"
MaikOvermars 0:4cb1de41d049 2
Spekdon 45:829a3992d689 3 double Kp = 0.1;
Spekdon 45:829a3992d689 4 double Ki = 0.4;
Spekdon 46:9b60a3c1acab 5 double Kd = 5;
bjonkheer 20:31876566d70f 6 extern double samplingfreq = 1000;
MaikOvermars 0:4cb1de41d049 7
Spekdon 45:829a3992d689 8 //void PID_controller(double error1, double error2, double &u1, double &u2, const double &T)
Spekdon 45:829a3992d689 9 //{
Spekdon 45:829a3992d689 10 // // proportianal part
Spekdon 45:829a3992d689 11 // double u1_k = Kp * error1;
Spekdon 45:829a3992d689 12 // double u2_k = Kp * error2;
Spekdon 45:829a3992d689 13 //
Spekdon 45:829a3992d689 14 // static double error1_integral = 0; //
Spekdon 45:829a3992d689 15 // static double error1_prev = error1; // initialization with this value only done once!
Spekdon 45:829a3992d689 16 //
Spekdon 45:829a3992d689 17 // static double error2_integral = 0;
Spekdon 45:829a3992d689 18 // static double error2_prev = error2; // initialization with this value only done once!
Spekdon 45:829a3992d689 19 //
Spekdon 45:829a3992d689 20 // static BiQuad LowPassFilter1(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
Spekdon 45:829a3992d689 21 // static BiQuad LowPassFilter2(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
Spekdon 45:829a3992d689 22 //
Spekdon 45:829a3992d689 23 // // integral part
Spekdon 45:829a3992d689 24 // error1_integral = error1_integral + error1 * T;
Spekdon 45:829a3992d689 25 // double u1_i = Ki * error1_integral;
Spekdon 45:829a3992d689 26 //
Spekdon 45:829a3992d689 27 // error2_integral = error2_integral + error2 * T;
Spekdon 45:829a3992d689 28 // double u2_i = Ki * error2_integral;
Spekdon 45:829a3992d689 29 //
Spekdon 45:829a3992d689 30 // // derivative part
Spekdon 45:829a3992d689 31 // double error1_derivative = (error1 - error1_prev) / T;
Spekdon 45:829a3992d689 32 // double u1_d = Kd * LowPassFilter1.step(error1_derivative); // apply low pass filter to remove noise due to derivative amplification
Spekdon 45:829a3992d689 33 // error1_prev = error1;
Spekdon 45:829a3992d689 34 //
Spekdon 45:829a3992d689 35 // double error2_derivative = (error2 - error2_prev) / T;
Spekdon 45:829a3992d689 36 // double u2_d = Kd * LowPassFilter2.step(error2_derivative); // apply low pass filter to remove noise due to derivative amplification
Spekdon 45:829a3992d689 37 // error2_prev = error2;
Spekdon 45:829a3992d689 38 //
Spekdon 45:829a3992d689 39 // u1 = u1_k+u1_i+u1_d;
Spekdon 45:829a3992d689 40 // u2 = u2_k+u2_i+u2_d;
Spekdon 45:829a3992d689 41 //}
Spekdon 45:829a3992d689 42 void PID_controller(double error1, double error2, double &u1, double &u2, double T)
Spekdon 45:829a3992d689 43 {
Spekdon 45:829a3992d689 44 static double error_prev1 = error1; // initialization with this value only done once!
Spekdon 45:829a3992d689 45 static double error_prev2 = error2; // initialization with this value only done once!
Spekdon 45:829a3992d689 46 double u_pid2, u_pid1;
Spekdon 45:829a3992d689 47 static double error_integral1, error_integral2 = 0;
Spekdon 45:829a3992d689 48
Spekdon 45:829a3992d689 49 double u_k1 = Kp * error1;
Spekdon 45:829a3992d689 50 double u_k2 = Kp * error2;
bjonkheer 24:53c300d1320e 51
MaikOvermars 26:23b0b6fce3a8 52 static BiQuad LowPassFilter1(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
MaikOvermars 26:23b0b6fce3a8 53 static BiQuad LowPassFilter2(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
MaikOvermars 0:4cb1de41d049 54
Spekdon 45:829a3992d689 55 double error_derivative1 = (error1 - error_prev1)*T;
Spekdon 45:829a3992d689 56 double filtered_error_derivative1 = LowPassFilter1.step(error_derivative1);
Spekdon 45:829a3992d689 57 double u_d1 = Kd * filtered_error_derivative1;
MaikOvermars 0:4cb1de41d049 58
Spekdon 45:829a3992d689 59 double error_derivative2 = (error2 - error_prev2)*T;
Spekdon 45:829a3992d689 60 double filtered_error_derivative2 = LowPassFilter2.step(error_derivative2);
Spekdon 45:829a3992d689 61 double u_d2 = Kd * filtered_error_derivative2;
bjonkheer 24:53c300d1320e 62
Spekdon 45:829a3992d689 63 error_integral1 = error_integral1 +( error1 * T);
Spekdon 45:829a3992d689 64 error_integral2 = error_integral2 +( error2 * T);
Spekdon 45:829a3992d689 65 if (error_integral1 > 1){
Spekdon 45:829a3992d689 66 error_integral1 = 1;
Spekdon 45:829a3992d689 67 }
Spekdon 45:829a3992d689 68 else if (error_integral1 < -1){
Spekdon 45:829a3992d689 69 error_integral1 = -1;
Spekdon 45:829a3992d689 70 }
Spekdon 45:829a3992d689 71 if (error_integral2 > 1){
Spekdon 45:829a3992d689 72 error_integral2 = 1;
Spekdon 45:829a3992d689 73 }
Spekdon 45:829a3992d689 74 else if (error_integral2 < -1){
Spekdon 45:829a3992d689 75 error_integral2 = -1;
Spekdon 45:829a3992d689 76 }
bjonkheer 24:53c300d1320e 77
Spekdon 45:829a3992d689 78 double u_i1 = Ki*error_integral1;
Spekdon 45:829a3992d689 79 double u_i2 = Ki*error_integral2;
Spekdon 45:829a3992d689 80
Spekdon 45:829a3992d689 81 u_pid1 = u_k1 + u_d1 + u_i1;
Spekdon 45:829a3992d689 82 u_pid2 = u_k2 + u_d2 + u_i2;
bjonkheer 24:53c300d1320e 83
Spekdon 45:829a3992d689 84 u1 = -u_pid1;
Spekdon 45:829a3992d689 85 u2 = -u_pid2;
Spekdon 45:829a3992d689 86
Spekdon 45:829a3992d689 87 error_prev1 = error1;
Spekdon 45:829a3992d689 88 error_prev2 = error2;
MaikOvermars 0:4cb1de41d049 89 }