V D / Mbed 2 deprecated 20171013_PI_controller_working_tryingtoaddPID

PIDcontroller_without_biquadfilter

Dependencies:   MODSERIAL QEI biquadFilter mbed

Fork of 20170910_PID_V1_0 by Arnoud Meutstege

main.cpp@2:02b385b96543, 2017-10-15 (annotated)

Committer:
vd
Date:
Sun Oct 15 21:19:11 2017 +0000
Revision:
2:02b385b96543
Parent:
1:b66e14435f70 
PIDcontroller_without_biquadfilter

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Arnoud113 0:85ca550760e9 1 #include "mbed.h"
Arnoud113 0:85ca550760e9 2 #include "QEI.h"
Arnoud113 0:85ca550760e9 3 #include "MODSERIAL.h"
Arnoud113 0:85ca550760e9 4 #include "math.h"
Arnoud113 0:85ca550760e9 5
Arnoud113 0:85ca550760e9 6 DigitalOut gpo(D0);
Arnoud113 0:85ca550760e9 7 DigitalOut ledb(LED_BLUE);
Arnoud113 0:85ca550760e9 8 DigitalOut ledr(LED_RED);
Arnoud113 0:85ca550760e9 9 DigitalOut ledg(LED_GREEN);
Arnoud113 0:85ca550760e9 10 DigitalOut motor1DC(D7);
Arnoud113 0:85ca550760e9 11 DigitalOut motor1PWM(D6);
Arnoud113 0:85ca550760e9 12 DigitalOut motor2DC(D4);
Arnoud113 0:85ca550760e9 13 DigitalOut motor2PWM(D5);
Arnoud113 0:85ca550760e9 14
Arnoud113 0:85ca550760e9 15 AnalogIn potMeter1(A0);
Arnoud113 0:85ca550760e9 16 AnalogIn potMeter2(A1);
Arnoud113 0:85ca550760e9 17 DigitalIn button1(D11);
Arnoud113 0:85ca550760e9 18 DigitalIn button2(D12);
Arnoud113 0:85ca550760e9 19 QEI Encoder1(D12,D13,NC,4200);
Arnoud113 0:85ca550760e9 20
Arnoud113 0:85ca550760e9 21 MODSERIAL pc(USBTX,USBRX);
Arnoud113 0:85ca550760e9 22
Arnoud113 0:85ca550760e9 23 Ticker controller;
Arnoud113 0:85ca550760e9 24
Arnoud113 0:85ca550760e9 25 const double pi = 3.1415926535897;
vd 2:02b385b96543 26 const double M1_TS = 0.01; // (was 0.0001) 0.001 and 0.01 work without biquad filter.
vd 2:02b385b96543 27
vd 2:02b385b96543 28 //verplaatst
Arnoud113 0:85ca550760e9 29 const float RAD_PER_PULSE = (2*pi)/4200;
vd 2:02b385b96543 30 const float CONTROLLER_TS = 0.01; //TIME INTERVAL/ hZ
vd 2:02b385b96543 31
vd 2:02b385b96543 32 const float M1_KP = 10, M1_KI = 1.0, M1_KD = 0.5; //was KP=10 KI=0.5 KD=0.5
vd 2:02b385b96543 33 double m1_err_int = 0, m1_prev_err = 0 ;
vd 2:02b385b96543 34 const double M1_F_A1 = 1.0 , M1_F_A2 = 2.0 , M1_F_B0 = 1.0 , M1_F_B1 = 3.0 , M1_F_B2 = 4.0 ;
vd 2:02b385b96543 35 double m1_f_v1 = 0 , m1_f_v2 = 0 ;
vd 2:02b385b96543 36
vd 2:02b385b96543 37
vd 2:02b385b96543 38 /*/ biquad filter
vd 2:02b385b96543 39 double biquad(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2){
vd 2:02b385b96543 40 double v = u - a1*v1 - a2*v2;
vd 2:02b385b96543 41 double y = b0*v + b1*v1 + b2*v2; // v=v1 vervangen
vd 2:02b385b96543 42 v2 = v1; v1=v;
vd 2:02b385b96543 43 return y;
vd 2:02b385b96543 44 }
vd 2:02b385b96543 45 */
vd 2:02b385b96543 46
vd 2:02b385b96543 47 /*
vd 2:02b385b96543 48 const float RAD_PER_PULSE = (2*pi)/4200;
vd 2:02b385b96543 49 const float CONTROLLER_TS = 0.001; //TIME INTERVAL/ hZ
vd 2:02b385b96543 50
vd 2:02b385b96543 51 */
vd 2:02b385b96543 52
vd 2:02b385b96543 53 //double m1_err_int = 0; --> PI
vd 2:02b385b96543 54
vd 2:02b385b96543 55
Arnoud113 0:85ca550760e9 56
Arnoud113 1:b66e14435f70 57 // the working P controller beneath here
Arnoud113 1:b66e14435f70 58 //float P(double error, const float Kp){
Arnoud113 1:b66e14435f70 59 // return Kp * error;
Arnoud113 1:b66e14435f70 60 // }
Arnoud113 1:b66e14435f70 61
vd 2:02b385b96543 62 //New PID
vd 2:02b385b96543 63 double PID(double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prev, double &f_v1, double &f_v2, const double f_a1, const double f_a2, const double f_b0, const double f_b1, const double f_b2){
vd 2:02b385b96543 64
vd 2:02b385b96543 65 // Derivative
vd 2:02b385b96543 66 double e_der = (e - e_prev)/Ts;
vd 2:02b385b96543 67
vd 2:02b385b96543 68
vd 2:02b385b96543 69 // biquad part, see slide
vd 2:02b385b96543 70 //e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2);
vd 2:02b385b96543 71
vd 2:02b385b96543 72
vd 2:02b385b96543 73
vd 2:02b385b96543 74 e_prev = e;
vd 2:02b385b96543 75
vd 2:02b385b96543 76
vd 2:02b385b96543 77 // Integral
vd 2:02b385b96543 78 e_int += Ts*e;
vd 2:02b385b96543 79
vd 2:02b385b96543 80
vd 2:02b385b96543 81 //PID
vd 2:02b385b96543 82 return Kp*e + Ki*e_int + Kd * e_der;
vd 2:02b385b96543 83
vd 2:02b385b96543 84 }
vd 2:02b385b96543 85
vd 2:02b385b96543 86
vd 2:02b385b96543 87
vd 2:02b385b96543 88 /*/ Working PI controller
Arnoud113 1:b66e14435f70 89
Arnoud113 1:b66e14435f70 90 double PI(double e, const double Kp, const double Ki, double Ts, double &e_int){
Arnoud113 1:b66e14435f70 91 e_int += Ts*e;
Arnoud113 1:b66e14435f70 92 return Kp*e+Ki*e_int;
Arnoud113 1:b66e14435f70 93 }
Arnoud113 1:b66e14435f70 94
vd 2:02b385b96543 95 */
vd 2:02b385b96543 96
vd 2:02b385b96543 97
Arnoud113 1:b66e14435f70 98 /* Working P controller part
Arnoud113 0:85ca550760e9 99
Arnoud113 0:85ca550760e9 100 void motor1_Controller(){
Arnoud113 0:85ca550760e9 101 double reference = 10*potMeter1.read();
Arnoud113 0:85ca550760e9 102 double position = RAD_PER_PULSE*Encoder1.getPulses();
Arnoud113 1:b66e14435f70 103 double motor1 = P(reference-position, M1_KP);
Arnoud113 0:85ca550760e9 104 motor1PWM = motor1;
Arnoud113 0:85ca550760e9 105
Arnoud113 0:85ca550760e9 106 if(motor1 > 0.1){
Arnoud113 0:85ca550760e9 107 motor1DC = 1;
Arnoud113 0:85ca550760e9 108
Arnoud113 0:85ca550760e9 109 ledr = 1;
Arnoud113 0:85ca550760e9 110 ledg = 1; //Blau
Arnoud113 0:85ca550760e9 111 ledb = 0;
Arnoud113 0:85ca550760e9 112 }
Arnoud113 0:85ca550760e9 113 else if (motor1<-0.1) {
Arnoud113 0:85ca550760e9 114 motor1DC = 0;
Arnoud113 0:85ca550760e9 115
Arnoud113 0:85ca550760e9 116 ledb = 1;
Arnoud113 0:85ca550760e9 117 ledr = 1;
Arnoud113 0:85ca550760e9 118 ledg = 0; //Groen
Arnoud113 0:85ca550760e9 119
Arnoud113 0:85ca550760e9 120 }
Arnoud113 0:85ca550760e9 121 else{
Arnoud113 0:85ca550760e9 122 motor1PWM = 0;
Arnoud113 0:85ca550760e9 123
Arnoud113 0:85ca550760e9 124 ledb = 1; //Rood
Arnoud113 0:85ca550760e9 125 ledr = 0;
Arnoud113 0:85ca550760e9 126 ledg = 1;
Arnoud113 0:85ca550760e9 127 }
Arnoud113 0:85ca550760e9 128
Arnoud113 0:85ca550760e9 129
Arnoud113 0:85ca550760e9 130 }
Arnoud113 1:b66e14435f70 131 */
Arnoud113 1:b66e14435f70 132
vd 2:02b385b96543 133 // New PID
Arnoud113 1:b66e14435f70 134
Arnoud113 1:b66e14435f70 135 void m1_Controller(){
Arnoud113 1:b66e14435f70 136 double reference = 10*potMeter1.read();
Arnoud113 1:b66e14435f70 137 double position = RAD_PER_PULSE*Encoder1.getPulses();
vd 2:02b385b96543 138 float motor1 = PID(reference - position, M1_KP, M1_KI, M1_KD, M1_TS, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, M1_F_A1, M1_F_A2, M1_F_B0, M1_F_B1, M1_F_B2);
Arnoud113 1:b66e14435f70 139 motor1PWM = motor1;
Arnoud113 1:b66e14435f70 140
vd 2:02b385b96543 141 if(motor1 > 0.1){
Arnoud113 1:b66e14435f70 142 motor1DC = 1;
Arnoud113 1:b66e14435f70 143
Arnoud113 1:b66e14435f70 144 ledr = 1;
Arnoud113 1:b66e14435f70 145 ledg = 1; //Blau
Arnoud113 1:b66e14435f70 146 ledb = 0;
Arnoud113 1:b66e14435f70 147 }
vd 2:02b385b96543 148 else if (motor1< -0.1) {
Arnoud113 1:b66e14435f70 149 motor1DC = 0;
Arnoud113 1:b66e14435f70 150
Arnoud113 1:b66e14435f70 151 ledb = 1;
Arnoud113 1:b66e14435f70 152 ledr = 1;
Arnoud113 1:b66e14435f70 153 ledg = 0; //Groen
Arnoud113 1:b66e14435f70 154
Arnoud113 1:b66e14435f70 155 }
Arnoud113 1:b66e14435f70 156 else{
Arnoud113 1:b66e14435f70 157 motor1PWM = 0;
Arnoud113 1:b66e14435f70 158
Arnoud113 1:b66e14435f70 159 ledb = 1; //Rood
Arnoud113 1:b66e14435f70 160 ledr = 0;
Arnoud113 1:b66e14435f70 161 ledg = 1;
Arnoud113 1:b66e14435f70 162 }
Arnoud113 1:b66e14435f70 163
Arnoud113 1:b66e14435f70 164
Arnoud113 1:b66e14435f70 165 }
Arnoud113 0:85ca550760e9 166
vd 2:02b385b96543 167
vd 2:02b385b96543 168 // New PI part
vd 2:02b385b96543 169 /*
vd 2:02b385b96543 170 void m1_Controller(){
vd 2:02b385b96543 171 double reference = 10*potMeter1.read();
vd 2:02b385b96543 172 double position = RAD_PER_PULSE*Encoder1.getPulses();
vd 2:02b385b96543 173 float motor1 = PI(reference - position, M1_KP, M1_KI, M1_TS, m1_err_int);
vd 2:02b385b96543 174 motor1PWM = motor1;
vd 2:02b385b96543 175
vd 2:02b385b96543 176 if(motor1 > 0.1){
vd 2:02b385b96543 177 motor1DC = 1;
vd 2:02b385b96543 178
vd 2:02b385b96543 179 ledr = 1;
vd 2:02b385b96543 180 ledg = 1; //Blau
vd 2:02b385b96543 181 ledb = 0;
vd 2:02b385b96543 182 }
vd 2:02b385b96543 183 else if (motor1< -0.1) {
vd 2:02b385b96543 184 motor1DC = 0;
vd 2:02b385b96543 185
vd 2:02b385b96543 186 ledb = 1;
vd 2:02b385b96543 187 ledr = 1;
vd 2:02b385b96543 188 ledg = 0; //Groen
vd 2:02b385b96543 189
vd 2:02b385b96543 190 }
vd 2:02b385b96543 191 else{
vd 2:02b385b96543 192 motor1PWM = 0;
vd 2:02b385b96543 193
vd 2:02b385b96543 194 ledb = 1; //Rood
vd 2:02b385b96543 195 ledr = 0;
vd 2:02b385b96543 196 ledg = 1;
vd 2:02b385b96543 197 }
vd 2:02b385b96543 198
vd 2:02b385b96543 199
vd 2:02b385b96543 200 }
vd 2:02b385b96543 201
vd 2:02b385b96543 202 */
vd 2:02b385b96543 203
Arnoud113 0:85ca550760e9 204 int main(){
vd 2:02b385b96543 205 pc.baud(9600);
Arnoud113 1:b66e14435f70 206 //controller.attach(&m1_Controller, CONTROLLER_TS); --> P one
Arnoud113 1:b66e14435f70 207
Arnoud113 1:b66e14435f70 208 controller.attach(&m1_Controller, M1_TS);
Arnoud113 0:85ca550760e9 209
Arnoud113 0:85ca550760e9 210 while(1){
Arnoud113 0:85ca550760e9 211 double reference = 10*potMeter1.read();
Arnoud113 0:85ca550760e9 212 double position = RAD_PER_PULSE*Encoder1.getPulses();
Arnoud113 1:b66e14435f70 213 // double motor1 = P(reference-position, M1_KP); --> old one
vd 2:02b385b96543 214 //float motor1 = PI(reference - position, M1_KP, M1_KI, M1_TS, m1_err_int);
vd 2:02b385b96543 215 //float motor1 = PID(reference - position, M1_KP, M1_KI, M1_KD, M1_TS, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, M1_F_A1, M1_F_A2, M1_F_B0, M1_F_B1, M1_F_B2);
vd 2:02b385b96543 216 double motor1 = PID(reference - position, M1_KP, M1_KI, M1_KD, M1_TS, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, M1_F_A1, M1_F_A2, M1_F_B0, M1_F_B1, M1_F_B2);
vd 2:02b385b96543 217
vd 2:02b385b96543 218 pc.printf("\r value motor1: %f. reference Pot: %f. Position: %f \n", motor1, reference, position);
Arnoud113 0:85ca550760e9 219 }
Arnoud113 0:85ca550760e9 220 }
vd 2:02b385b96543 221