20171016_PID_controller_working
Dependencies: MODSERIAL QEI biquadFilter mbed
Fork of 20171013_PI_controller_working_tryingtoaddPID by
main.cpp@3:7e616fcf471d, 2017-10-17 (annotated)
- Committer:
- vd
- Date:
- Tue Oct 17 16:22:41 2017 +0000
- Revision:
- 3:7e616fcf471d
- Parent:
- 2:02b385b96543
working PID and printing works too;
Who changed what in which revision?
User | Revision | Line number | New 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 | 3:7e616fcf471d | 26 | const double M1_TS = 0.01; // (was 0.0001) 0.001 and 0.01 work without biquad filter. // // Sample time (motor - timestep) |
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 | 3:7e616fcf471d | 32 | const float M1_KP = 10, M1_KI = 0.5, 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 | 3:7e616fcf471d | 47 | /* Stukje code dat hier stond maar is verplaatst naar line 29: |
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 | 3:7e616fcf471d | 66 | double e_der = (e - e_prev)/Ts; // Ts = motor1-timestep |
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 | |
vd | 3:7e616fcf471d | 204 | |
Arnoud113 | 0:85ca550760e9 | 205 | int main(){ |
vd | 2:02b385b96543 | 206 | pc.baud(9600); |
Arnoud113 | 1:b66e14435f70 | 207 | //controller.attach(&m1_Controller, CONTROLLER_TS); --> P one |
Arnoud113 | 1:b66e14435f70 | 208 | |
Arnoud113 | 1:b66e14435f70 | 209 | controller.attach(&m1_Controller, M1_TS); |
Arnoud113 | 0:85ca550760e9 | 210 | |
Arnoud113 | 0:85ca550760e9 | 211 | while(1){ |
Arnoud113 | 0:85ca550760e9 | 212 | double reference = 10*potMeter1.read(); |
Arnoud113 | 0:85ca550760e9 | 213 | double position = RAD_PER_PULSE*Encoder1.getPulses(); |
Arnoud113 | 1:b66e14435f70 | 214 | // double motor1 = P(reference-position, M1_KP); --> old one |
vd | 2:02b385b96543 | 215 | //float motor1 = PI(reference - position, M1_KP, M1_KI, M1_TS, m1_err_int); |
vd | 2:02b385b96543 | 216 | //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 | 217 | 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 | 218 | |
vd | 3:7e616fcf471d | 219 | pc.printf("\r value motor1: %f. reference Pot: %f. Position: %f \n", motor1, reference, position); |
vd | 3:7e616fcf471d | 220 | |
Arnoud113 | 0:85ca550760e9 | 221 | } |
Arnoud113 | 0:85ca550760e9 | 222 | } |
vd | 2:02b385b96543 | 223 | |
vd | 3:7e616fcf471d | 224 | // Voor Mac: screen /dev/tty.usbmodem1422 9600 |