Code om de PID controller af te stellen aan de hand van een sinus golf

Dependencies:   mbed QEI MODSERIAL FastPWM biquadFilter

Committer:
aschut
Date:
Wed Apr 17 12:17:51 2019 +0000
Revision:
17:190c86f4828f
Parent:
14:30d32f51dd9f
Child:
18:161b7ca7f98d
sinus

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aschut 0:a9a42914138c 1 #include "mbed.h"
aschut 2:926d56babb1a 2 #include "MODSERIAL.h"
aschut 4:99f7fdce608e 3 #include "QEI.h"
aschut 6:14a9c4f30c86 4 #include "BiQuad.h"
aschut 13:b7a1a4245f37 5 #include "FastPWM.h"
aschut 4:99f7fdce608e 6 // Algemeen
aschut 17:190c86f4828f 7 Timer t;
aschut 1:58f34947c674 8 DigitalIn button2(SW2);
aschut 1:58f34947c674 9 DigitalIn button3(SW3);
aschut 17:190c86f4828f 10 DigitalIn But2(D12);
aschut 17:190c86f4828f 11 DigitalIn But1(D13);
aschut 2:926d56babb1a 12 DigitalOut led(LED_GREEN);
aschut 2:926d56babb1a 13 DigitalOut led2(LED_RED);
aschut 4:99f7fdce608e 14 DigitalOut led3(LED_BLUE);
aschut 2:926d56babb1a 15 MODSERIAL pc(USBTX, USBRX);
aschut 17:190c86f4828f 16 float A;
aschut 17:190c86f4828f 17 int i = 0;
aschut 17:190c86f4828f 18 float pi = 3.14159265359;
aschut 17:190c86f4828f 19
aschut 2:926d56babb1a 20
aschut 4:99f7fdce608e 21 //Motoren
aschut 0:a9a42914138c 22 DigitalOut direction1(D4);
aschut 13:b7a1a4245f37 23 FastPWM pwmpin1(D5);
aschut 13:b7a1a4245f37 24 FastPWM pwmpin2(D6);
aschut 0:a9a42914138c 25 DigitalOut direction2(D7);
aschut 9:aa5d6636197b 26 volatile float pwm1;
aschut 4:99f7fdce608e 27 volatile float pwm2;
aschut 4:99f7fdce608e 28
aschut 4:99f7fdce608e 29 //Encoder
aschut 17:190c86f4828f 30 QEI encoder1 (D1, D0, NC, 1200, QEI::X4_ENCODING);
aschut 17:190c86f4828f 31 QEI encoder2 (D3, D2, NC, 4800, QEI::X4_ENCODING);
aschut 9:aa5d6636197b 32 double Pulses1;
aschut 9:aa5d6636197b 33 double motor_position1;
aschut 6:14a9c4f30c86 34 double Pulses2;
aschut 6:14a9c4f30c86 35 double motor_position2;
aschut 9:aa5d6636197b 36 double error1;
aschut 2:926d56babb1a 37
aschut 2:926d56babb1a 38 //Pot meter
aschut 17:190c86f4828f 39 AnalogIn pot(A5);
aschut 6:14a9c4f30c86 40 AnalogIn pot0(A0);
aschut 4:99f7fdce608e 41 float Pot2;
aschut 6:14a9c4f30c86 42 float Pot1;
aschut 4:99f7fdce608e 43
aschut 4:99f7fdce608e 44 //Ticker
aschut 4:99f7fdce608e 45 Ticker Pwm;
aschut 2:926d56babb1a 46 Ticker PotRead;
aschut 7:9a1007e35bac 47 Ticker Kdc;
aschut 6:14a9c4f30c86 48
aschut 0:a9a42914138c 49
aschut 5:4b25551aeb6e 50 //Kinematica
aschut 9:aa5d6636197b 51 double stap1;
aschut 9:aa5d6636197b 52 double stap2;
aschut 6:14a9c4f30c86 53 double KPot;
aschut 5:4b25551aeb6e 54 float KPotabs;
aschut 9:aa5d6636197b 55
aschut 5:4b25551aeb6e 56 float ElbowReference;
aschut 5:4b25551aeb6e 57 float Ellebooghoek1;
aschut 5:4b25551aeb6e 58 float Ellebooghoek2;
aschut 5:4b25551aeb6e 59 float Ellebooghoek3;
aschut 5:4b25551aeb6e 60 float Ellebooghoek4;
aschut 9:aa5d6636197b 61
aschut 9:aa5d6636197b 62 float PolsReference;
aschut 9:aa5d6636197b 63 float Polshoek1;
aschut 9:aa5d6636197b 64 float Polshoek2;
aschut 9:aa5d6636197b 65 float Polshoek3;
aschut 9:aa5d6636197b 66 float Polshoek4;
aschut 9:aa5d6636197b 67
aschut 9:aa5d6636197b 68 float Hoeknieuw1;
aschut 9:aa5d6636197b 69 float Hoeknieuw2;
aschut 5:4b25551aeb6e 70
aschut 5:4b25551aeb6e 71 //Limiet in graden
aschut 13:b7a1a4245f37 72 float lowerlim1 = -900;
aschut 13:b7a1a4245f37 73 float upperlim1 = 900;
aschut 9:aa5d6636197b 74 float lowerlim2 = 0;
aschut 9:aa5d6636197b 75 float upperlim2 = 748.8; //40% van 1 ronde van het grote tandwiel is 2,08 rondes van de motor
aschut 2:926d56babb1a 76
aschut 6:14a9c4f30c86 77 // VARIABLES PID CONTROLLER
aschut 14:30d32f51dd9f 78 double Kp1 = 0;
aschut 9:aa5d6636197b 79 double Ki1 = 0;
aschut 9:aa5d6636197b 80 double Kd1 = 1;
aschut 9:aa5d6636197b 81 double Kp2 = 6; // Zonder arm: 6,0,1
aschut 9:aa5d6636197b 82 double Ki2 = 0;
aschut 9:aa5d6636197b 83 double Kd2 = 1;
aschut 11:2375b538b631 84 double Ts = 0.0005; // Sample time in seconds
aschut 6:14a9c4f30c86 85
aschut 9:aa5d6636197b 86 float Kinematics1(float KPot)
aschut 6:14a9c4f30c86 87 {
aschut 6:14a9c4f30c86 88
aschut 6:14a9c4f30c86 89 if (KPot > 0.45f) {
aschut 13:b7a1a4245f37 90 stap1 = KPot*450*Ts;
aschut 9:aa5d6636197b 91 Hoeknieuw1 = PolsReference + stap1;
aschut 9:aa5d6636197b 92 return Hoeknieuw1;
aschut 5:4b25551aeb6e 93 }
aschut 4:99f7fdce608e 94
aschut 6:14a9c4f30c86 95 else if (KPot < -0.45f) {
aschut 13:b7a1a4245f37 96 stap1 = KPot*450*Ts;
aschut 9:aa5d6636197b 97 Hoeknieuw1 = PolsReference + stap1;
aschut 9:aa5d6636197b 98 return Hoeknieuw1;
aschut 9:aa5d6636197b 99 }
aschut 9:aa5d6636197b 100
aschut 9:aa5d6636197b 101 else {
aschut 9:aa5d6636197b 102 return PolsReference;
aschut 9:aa5d6636197b 103 }
aschut 9:aa5d6636197b 104 }
aschut 9:aa5d6636197b 105 float Kinematics2(float KPot)
aschut 9:aa5d6636197b 106 {
aschut 9:aa5d6636197b 107
aschut 9:aa5d6636197b 108 if (KPot > 0.45f) {
aschut 9:aa5d6636197b 109 stap2 = KPot*150*Ts; // 144 graden van de arm in 5 seconden
aschut 9:aa5d6636197b 110 Hoeknieuw2 = ElbowReference + stap2;
aschut 9:aa5d6636197b 111 return Hoeknieuw2;
aschut 9:aa5d6636197b 112 }
aschut 9:aa5d6636197b 113
aschut 9:aa5d6636197b 114 else if (KPot < -0.45f) {
aschut 9:aa5d6636197b 115 stap2 = KPot*150*Ts;
aschut 9:aa5d6636197b 116 Hoeknieuw2 = ElbowReference + stap2;
aschut 9:aa5d6636197b 117 return Hoeknieuw2;
aschut 6:14a9c4f30c86 118 }
aschut 6:14a9c4f30c86 119
aschut 6:14a9c4f30c86 120 else {
aschut 6:14a9c4f30c86 121 return ElbowReference;
aschut 6:14a9c4f30c86 122 }
aschut 6:14a9c4f30c86 123 }
aschut 6:14a9c4f30c86 124
aschut 9:aa5d6636197b 125 float Limits1(float Polshoek2)
aschut 6:14a9c4f30c86 126 {
aschut 6:14a9c4f30c86 127
aschut 9:aa5d6636197b 128 if (Polshoek2 <= upperlim1 && Polshoek2 >= lowerlim1) { //Binnen de limieten
aschut 9:aa5d6636197b 129 Polshoek3 = Polshoek2;
aschut 9:aa5d6636197b 130 }
aschut 9:aa5d6636197b 131
aschut 9:aa5d6636197b 132 else {
aschut 9:aa5d6636197b 133 if (Polshoek2 >= upperlim1) { //Boven de limiet
aschut 9:aa5d6636197b 134 Polshoek3 = upperlim1;
aschut 9:aa5d6636197b 135 } else { //Onder de limiet
aschut 9:aa5d6636197b 136 Polshoek3 = lowerlim1;
aschut 9:aa5d6636197b 137 }
aschut 9:aa5d6636197b 138 }
aschut 9:aa5d6636197b 139
aschut 9:aa5d6636197b 140 return Polshoek3;
aschut 9:aa5d6636197b 141 }
aschut 9:aa5d6636197b 142
aschut 9:aa5d6636197b 143
aschut 9:aa5d6636197b 144 float Limits2(float Ellebooghoek2)
aschut 9:aa5d6636197b 145 {
aschut 9:aa5d6636197b 146
aschut 9:aa5d6636197b 147 if (Ellebooghoek2 <= upperlim2 && Ellebooghoek2 >= lowerlim2) { //Binnen de limieten
aschut 5:4b25551aeb6e 148 Ellebooghoek3 = Ellebooghoek2;
aschut 6:14a9c4f30c86 149 }
aschut 6:14a9c4f30c86 150
aschut 5:4b25551aeb6e 151 else {
aschut 9:aa5d6636197b 152 if (Ellebooghoek2 >= upperlim2) { //Boven de limiet
aschut 9:aa5d6636197b 153 Ellebooghoek3 = upperlim2;
aschut 6:14a9c4f30c86 154 } else { //Onder de limiet
aschut 9:aa5d6636197b 155 Ellebooghoek3 = lowerlim2;
aschut 5:4b25551aeb6e 156 }
aschut 5:4b25551aeb6e 157 }
aschut 6:14a9c4f30c86 158
aschut 5:4b25551aeb6e 159 return Ellebooghoek3;
aschut 6:14a9c4f30c86 160 }
aschut 6:14a9c4f30c86 161
aschut 6:14a9c4f30c86 162
aschut 4:99f7fdce608e 163
aschut 0:a9a42914138c 164
aschut 6:14a9c4f30c86 165 // ~~~~~~~~~~~~~~PID CONTROLLER~~~~~~~~~~~~~~~~~~
aschut 9:aa5d6636197b 166 double PID_controller1(double error1)
aschut 6:14a9c4f30c86 167 {
aschut 9:aa5d6636197b 168 static double error1_integral = 0;
aschut 9:aa5d6636197b 169 static double error1_prev = error1; // initialization with this value only done once!
aschut 6:14a9c4f30c86 170 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); //(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate, T bandwidth);
aschut 7:9a1007e35bac 171
aschut 14:30d32f51dd9f 172 //PID testing
aschut 14:30d32f51dd9f 173 Kp1 = 30 * Pot2;
aschut 9:aa5d6636197b 174 Ki1 = 10 * Pot1;
aschut 7:9a1007e35bac 175
aschut 7:9a1007e35bac 176 if (!But2) {
aschut 14:30d32f51dd9f 177 Kd1 = Kd1 + 0.001;
aschut 7:9a1007e35bac 178 }
aschut 7:9a1007e35bac 179 if (!But1){
aschut 14:30d32f51dd9f 180 Kd1 = Kd1 - 0.001;
aschut 7:9a1007e35bac 181 }
aschut 14:30d32f51dd9f 182
aschut 7:9a1007e35bac 183
aschut 6:14a9c4f30c86 184 // Proportional part:
aschut 9:aa5d6636197b 185 double u_k1 = Kp1 * error1;
aschut 6:14a9c4f30c86 186
aschut 6:14a9c4f30c86 187 // Integral part
aschut 9:aa5d6636197b 188 error1_integral = error1_integral + error1 * Ts;
aschut 9:aa5d6636197b 189 double u_i1 = Ki1* error1_integral;
aschut 6:14a9c4f30c86 190
aschut 6:14a9c4f30c86 191 // Derivative part
aschut 9:aa5d6636197b 192 double error1_derivative = (error1 - error1_prev)/Ts;
aschut 9:aa5d6636197b 193 double filtered_error1_derivative = LowPassFilter.step(error1_derivative);
aschut 9:aa5d6636197b 194 double u_d1 = Kd1 * filtered_error1_derivative;
aschut 9:aa5d6636197b 195 error1_prev = error1;
aschut 6:14a9c4f30c86 196
aschut 6:14a9c4f30c86 197 // Sum all parts and return it
aschut 9:aa5d6636197b 198 return u_k1 + u_i1 + u_d1;
aschut 9:aa5d6636197b 199 }
aschut 9:aa5d6636197b 200 double PID_controller2(double error2)
aschut 9:aa5d6636197b 201 {
aschut 9:aa5d6636197b 202 static double error2_integral = 0;
aschut 9:aa5d6636197b 203 static double error2_prev = error2; // initialization with this value only done once!
aschut 9:aa5d6636197b 204 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); //(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate, T bandwidth);
aschut 9:aa5d6636197b 205
aschut 9:aa5d6636197b 206 /* PID testing
aschut 9:aa5d6636197b 207 Kp2 = 10 * Pot2;
aschut 9:aa5d6636197b 208 Ki2 = 10 * Pot1;
aschut 9:aa5d6636197b 209
aschut 9:aa5d6636197b 210 if (!But2) {
aschut 9:aa5d6636197b 211 Kd2 = Kd2 + 0.01;
aschut 9:aa5d6636197b 212 }
aschut 9:aa5d6636197b 213 if (!But1){
aschut 9:aa5d6636197b 214 Kd2 = Kd2 - 0.01;
aschut 9:aa5d6636197b 215 }
aschut 9:aa5d6636197b 216 */
aschut 9:aa5d6636197b 217
aschut 9:aa5d6636197b 218 // Proportional part:
aschut 9:aa5d6636197b 219 double u_k2 = Kp2 * error2;
aschut 9:aa5d6636197b 220
aschut 9:aa5d6636197b 221 // Integral part
aschut 9:aa5d6636197b 222 error2_integral = error2_integral + error2 * Ts;
aschut 9:aa5d6636197b 223 double u_i2 = Ki2 * error2_integral;
aschut 9:aa5d6636197b 224
aschut 9:aa5d6636197b 225 // Derivative part
aschut 9:aa5d6636197b 226 double error2_derivative = (error2 - error2_prev)/Ts;
aschut 9:aa5d6636197b 227 double filtered_error2_derivative = LowPassFilter.step(error2_derivative);
aschut 9:aa5d6636197b 228 double u_d2 = Kd2 * filtered_error2_derivative;
aschut 9:aa5d6636197b 229 error2_prev = error2;
aschut 9:aa5d6636197b 230
aschut 9:aa5d6636197b 231 // Sum all parts and return it
aschut 9:aa5d6636197b 232 return u_k2 + u_i2 + u_d2;
aschut 6:14a9c4f30c86 233 }
aschut 6:14a9c4f30c86 234
aschut 9:aa5d6636197b 235 void moter1_control(double u1)
aschut 6:14a9c4f30c86 236 {
aschut 9:aa5d6636197b 237 direction1= u1 > 0.0f; //positief = CW
aschut 11:2375b538b631 238 if (fabs(u1)> 0.5f) {
aschut 11:2375b538b631 239 u1 = 0.5f;
aschut 6:14a9c4f30c86 240 } else {
aschut 9:aa5d6636197b 241 u1= u1;
aschut 0:a9a42914138c 242 }
aschut 13:b7a1a4245f37 243 pwmpin1.write(fabs(u1)) ; //pwmduty cycle canonlybepositive, floatingpoint absolute value
aschut 9:aa5d6636197b 244 }
aschut 9:aa5d6636197b 245
aschut 9:aa5d6636197b 246 void moter2_control(double u2)
aschut 9:aa5d6636197b 247 {
aschut 9:aa5d6636197b 248 direction2= u2 < 0.0f; //positief = CW
aschut 9:aa5d6636197b 249 if (fabs(u2)> 0.7f) {
aschut 9:aa5d6636197b 250 u2 = 0.7f;
aschut 9:aa5d6636197b 251 } else {
aschut 9:aa5d6636197b 252 u2= u2;
aschut 9:aa5d6636197b 253 }
aschut 13:b7a1a4245f37 254 pwmpin2.write(fabs(u2)) ; //pwmduty cycle canonlybepositive, floatingpoint absolute value
aschut 6:14a9c4f30c86 255 }
aschut 6:14a9c4f30c86 256
aschut 4:99f7fdce608e 257 void PwmMotor(void)
aschut 6:14a9c4f30c86 258 {
aschut 14:30d32f51dd9f 259 //Reference hoek berekenen, in graden
aschut 9:aa5d6636197b 260 float Ellebooghoek1 = Kinematics2(pwm2);
aschut 9:aa5d6636197b 261 float Ellebooghoek4 = Limits2(Ellebooghoek1);
aschut 14:30d32f51dd9f 262 //ElbowReference = Ellebooghoek4;
aschut 9:aa5d6636197b 263
aschut 14:30d32f51dd9f 264 //float Polshoek1 = Kinematics1(pwm2);
aschut 14:30d32f51dd9f 265 //float Polshoek4 = Limits1(Polshoek1);
aschut 14:30d32f51dd9f 266 //PolsReference = Polshoek4;
aschut 6:14a9c4f30c86 267
aschut 7:9a1007e35bac 268 // Positie motor berekenen, in graden
aschut 9:aa5d6636197b 269 Pulses1 = encoder1.getPulses();
aschut 9:aa5d6636197b 270 motor_position1 = -(Pulses1/1200)*360;
aschut 6:14a9c4f30c86 271 Pulses2 = encoder2.getPulses();
aschut 9:aa5d6636197b 272 motor_position2 = -(Pulses2/4800)*360;
aschut 2:926d56babb1a 273
aschut 9:aa5d6636197b 274 double error1 = PolsReference - motor_position1;
aschut 9:aa5d6636197b 275 double u1 = PID_controller1(error1);
aschut 9:aa5d6636197b 276 moter1_control(u1);
aschut 9:aa5d6636197b 277
aschut 9:aa5d6636197b 278 double error2 = ElbowReference - motor_position2;
aschut 9:aa5d6636197b 279 double u2 = PID_controller2(error2);
aschut 9:aa5d6636197b 280 moter2_control(u2);
aschut 6:14a9c4f30c86 281
aschut 6:14a9c4f30c86 282 }
aschut 6:14a9c4f30c86 283
aschut 1:58f34947c674 284 void MotorOn(void)
aschut 6:14a9c4f30c86 285 {
aschut 9:aa5d6636197b 286 pwmpin1 = 0;
aschut 4:99f7fdce608e 287 pwmpin2 = 0;
aschut 6:14a9c4f30c86 288 Pwm.attach (PwmMotor, Ts);
aschut 6:14a9c4f30c86 289
aschut 6:14a9c4f30c86 290 }
aschut 1:58f34947c674 291 void MotorOff(void)
aschut 6:14a9c4f30c86 292 {
aschut 6:14a9c4f30c86 293 Pwm.detach ();
aschut 1:58f34947c674 294 pwmpin2 = 0;
aschut 9:aa5d6636197b 295 pwmpin1 = 0;
aschut 6:14a9c4f30c86 296 }
aschut 4:99f7fdce608e 297
aschut 6:14a9c4f30c86 298 void ContinuousReader(void)
aschut 6:14a9c4f30c86 299 {
aschut 3:ac13255164cd 300 Pot2 = pot.read();
aschut 6:14a9c4f30c86 301 Pot1 = pot0.read();
aschut 7:9a1007e35bac 302 pwm2 =(Pot2*2)-1; //scaling naar -1 tot 1
aschut 9:aa5d6636197b 303 pwm1 =(Pot1*2)-1;
aschut 6:14a9c4f30c86 304 }
aschut 6:14a9c4f30c86 305
aschut 14:30d32f51dd9f 306
aschut 7:9a1007e35bac 307 void Kdcount (void) // Voor het testen van de PID waardes
aschut 7:9a1007e35bac 308 {
aschut 17:190c86f4828f 309 float ts = t.read();
aschut 17:190c86f4828f 310 A = 50;
aschut 17:190c86f4828f 311 PolsReference = A*sin(2*pi*ts*0.2);
aschut 17:190c86f4828f 312 //DA = sin(2/180*3.14)*0.5+0.5;
aschut 17:190c86f4828f 313 /*
aschut 17:190c86f4828f 314 if( i<360) {
aschut 17:190c86f4828f 315 DA = sin(i/180*3.14)*0.5+0.5;
aschut 17:190c86f4828f 316 i++;
aschut 17:190c86f4828f 317 }
aschut 17:190c86f4828f 318 */
aschut 17:190c86f4828f 319
aschut 7:9a1007e35bac 320 }
aschut 14:30d32f51dd9f 321
aschut 1:58f34947c674 322
aschut 6:14a9c4f30c86 323 int main()
aschut 6:14a9c4f30c86 324 {
aschut 17:190c86f4828f 325
aschut 6:14a9c4f30c86 326 t.start();
aschut 6:14a9c4f30c86 327 int counter = 0;
aschut 6:14a9c4f30c86 328 pwmpin2.period_us(60);
aschut 6:14a9c4f30c86 329 PotRead.attach(ContinuousReader,Ts);
aschut 17:190c86f4828f 330 Kdc.attach(Kdcount,0.01); //Voor PID waarde testen
aschut 17:190c86f4828f 331 //Kdcount();
aschut 2:926d56babb1a 332 pc.baud(115200);
aschut 6:14a9c4f30c86 333 //pc.printf("start\r\n");
aschut 4:99f7fdce608e 334 led = 1;
aschut 4:99f7fdce608e 335 led2 =1;
aschut 4:99f7fdce608e 336 led3 =1;
aschut 6:14a9c4f30c86 337
aschut 6:14a9c4f30c86 338 while (true) {
aschut 6:14a9c4f30c86 339 led3 = 0;
aschut 6:14a9c4f30c86 340 if (!button2) {
aschut 6:14a9c4f30c86 341 led3 = 1;
aschut 6:14a9c4f30c86 342 led = 0;
aschut 6:14a9c4f30c86 343 //pc.printf("MotorOn\r\n");
aschut 6:14a9c4f30c86 344 MotorOn();
aschut 6:14a9c4f30c86 345 }
aschut 6:14a9c4f30c86 346 if (!button3) {
aschut 6:14a9c4f30c86 347 //pc.printf("MotorOff\r\n");
aschut 6:14a9c4f30c86 348 PotRead.detach();
aschut 6:14a9c4f30c86 349 MotorOff();
aschut 6:14a9c4f30c86 350 }
aschut 4:99f7fdce608e 351 led = 0;
aschut 17:190c86f4828f 352
aschut 6:14a9c4f30c86 353 if(counter==10) {
aschut 6:14a9c4f30c86 354 float tmp = t.read();
aschut 9:aa5d6636197b 355 printf("%f,%f,%f,%f,%f,%f,%f\n\r",tmp,motor_position1,PolsReference,error1,Kp1,Ki1,Kd1);
aschut 6:14a9c4f30c86 356 counter = 0;
aschut 6:14a9c4f30c86 357 }
aschut 6:14a9c4f30c86 358 counter++;
aschut 6:14a9c4f30c86 359 wait(0.001);
aschut 1:58f34947c674 360 }
aschut 6:14a9c4f30c86 361 }
aschut 0:a9a42914138c 362
aschut 0:a9a42914138c 363
aschut 6:14a9c4f30c86 364