Astrid Schut / Mbed 2 deprecated PWMmotor

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

Dependencies:   mbed QEI MODSERIAL FastPWM biquadFilter

main.cpp@15:65c86c3c656d, 2019-04-04 (annotated)

Committer:
aschut
Date:
Thu Apr 04 12:35:02 2019 +0000
Revision:
15:65c86c3c656d
Parent:
13:b7a1a4245f37 
Motor 2 PID testing met fastpwm

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