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@8:bf5192a22c64, 2019-03-21 (annotated)

Committer:
aschut
Date:
Thu Mar 21 10:12:46 2019 +0000
Revision:
8:bf5192a22c64
Parent:
7:9a1007e35bac 
Poging tot toevoegen tweede motor

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 4:99f7fdce608e 5
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 8:bf5192a22c64 17 DigitalOut direction1(D4); //Motor 1 = rotatie gewricht
aschut 0:a9a42914138c 18 PwmOut pwmpin1(D5);
aschut 8:bf5192a22c64 19 PwmOut pwmpin2(D6); //Motor 2 = elleboog gewricht
aschut 0:a9a42914138c 20 DigitalOut direction2(D7);
aschut 4:99f7fdce608e 21 volatile float PWM1;
aschut 4:99f7fdce608e 22 volatile float PWM2;
aschut 4:99f7fdce608e 23 volatile float pwm2;
aschut 8:bf5192a22c64 24 volatile float pwm1;
aschut 4:99f7fdce608e 25
aschut 4:99f7fdce608e 26 //Encoder
aschut 4:99f7fdce608e 27 DigitalIn EncoderA(D13);
aschut 4:99f7fdce608e 28 DigitalIn EncoderB(D12);
aschut 4:99f7fdce608e 29 QEI encoder2 (D13, D12, NC, 8400, QEI::X4_ENCODING);
aschut 8:bf5192a22c64 30 QEI encoder1 (D10, D9, NC, 1200, QEI::X4_ENCODING);
aschut 8:bf5192a22c64 31 double Pulses1;
aschut 8:bf5192a22c64 32 double motor_position1;
aschut 6:14a9c4f30c86 33 double Pulses2;
aschut 6:14a9c4f30c86 34 double motor_position2;
aschut 2:926d56babb1a 35
aschut 2:926d56babb1a 36 //Pot meter
aschut 1:58f34947c674 37 AnalogIn pot(A1);
aschut 6:14a9c4f30c86 38 AnalogIn pot0(A0);
aschut 4:99f7fdce608e 39 float Pot2;
aschut 6:14a9c4f30c86 40 float Pot1;
aschut 4:99f7fdce608e 41
aschut 4:99f7fdce608e 42 //Ticker
aschut 4:99f7fdce608e 43 Ticker Pwm;
aschut 2:926d56babb1a 44 Ticker PotRead;
aschut 7:9a1007e35bac 45 Ticker Kdc;
aschut 6:14a9c4f30c86 46
aschut 0:a9a42914138c 47
aschut 5:4b25551aeb6e 48 //Kinematica
aschut 6:14a9c4f30c86 49 double stap;
aschut 6:14a9c4f30c86 50 double KPot;
aschut 5:4b25551aeb6e 51 float KPotabs;
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 5:4b25551aeb6e 57 float Hoeknieuw;
aschut 5:4b25551aeb6e 58
aschut 5:4b25551aeb6e 59 //Limiet in graden
aschut 5:4b25551aeb6e 60 float lowerlim = 0;
aschut 5:4b25551aeb6e 61 float upperlim = 748.8; //40% van 1 ronde van het grote tandwiel is 2,08 rondes van de motor
aschut 2:926d56babb1a 62
aschut 6:14a9c4f30c86 63 // VARIABLES PID CONTROLLER
aschut 8:bf5192a22c64 64 double Kp = 10 * Pot1; // Zonder arm: 6,0,1
aschut 8:bf5192a22c64 65 double Ki = 10 * Pot2; //
aschut 7:9a1007e35bac 66 double Kd = 1; //
aschut 7:9a1007e35bac 67 double Ts = 0.001; // Sample time in seconds
aschut 6:14a9c4f30c86 68
aschut 5:4b25551aeb6e 69 float Kinematics(float KPot)
aschut 6:14a9c4f30c86 70 {
aschut 6:14a9c4f30c86 71
aschut 6:14a9c4f30c86 72 if (KPot > 0.45f) {
aschut 6:14a9c4f30c86 73 stap = KPot*150*Ts; // 144 graden van de arm in 5 seconden
aschut 6:14a9c4f30c86 74 Hoeknieuw = ElbowReference + stap;
aschut 6:14a9c4f30c86 75 return Hoeknieuw;
aschut 5:4b25551aeb6e 76 }
aschut 4:99f7fdce608e 77
aschut 6:14a9c4f30c86 78 else if (KPot < -0.45f) {
aschut 6:14a9c4f30c86 79 stap = KPot*150*Ts;
aschut 6:14a9c4f30c86 80 Hoeknieuw = ElbowReference + stap;
aschut 6:14a9c4f30c86 81 return Hoeknieuw;
aschut 6:14a9c4f30c86 82 }
aschut 6:14a9c4f30c86 83
aschut 6:14a9c4f30c86 84 else {
aschut 6:14a9c4f30c86 85 return ElbowReference;
aschut 6:14a9c4f30c86 86 }
aschut 6:14a9c4f30c86 87 }
aschut 6:14a9c4f30c86 88
aschut 6:14a9c4f30c86 89 float Limits(float Ellebooghoek2)
aschut 6:14a9c4f30c86 90 {
aschut 6:14a9c4f30c86 91
aschut 5:4b25551aeb6e 92 if (Ellebooghoek2 <= upperlim && Ellebooghoek2 >= lowerlim) { //Binnen de limieten
aschut 5:4b25551aeb6e 93 Ellebooghoek3 = Ellebooghoek2;
aschut 6:14a9c4f30c86 94 }
aschut 6:14a9c4f30c86 95
aschut 5:4b25551aeb6e 96 else {
aschut 6:14a9c4f30c86 97 if (Ellebooghoek2 >= upperlim) { //Boven de limiet
aschut 5:4b25551aeb6e 98 Ellebooghoek3 = upperlim;
aschut 6:14a9c4f30c86 99 } else { //Onder de limiet
aschut 5:4b25551aeb6e 100 Ellebooghoek3 = lowerlim;
aschut 5:4b25551aeb6e 101 }
aschut 5:4b25551aeb6e 102 }
aschut 6:14a9c4f30c86 103
aschut 5:4b25551aeb6e 104 return Ellebooghoek3;
aschut 6:14a9c4f30c86 105 }
aschut 6:14a9c4f30c86 106
aschut 6:14a9c4f30c86 107
aschut 4:99f7fdce608e 108
aschut 0:a9a42914138c 109
aschut 6:14a9c4f30c86 110 // ~~~~~~~~~~~~~~PID CONTROLLER~~~~~~~~~~~~~~~~~~
aschut 6:14a9c4f30c86 111
aschut 6:14a9c4f30c86 112 double PID_controller(double error)
aschut 6:14a9c4f30c86 113 {
aschut 6:14a9c4f30c86 114 static double error_integral = 0;
aschut 6:14a9c4f30c86 115 static double error_prev = error; // initialization with this value only done once!
aschut 6:14a9c4f30c86 116 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 117
aschut 8:bf5192a22c64 118 // PID testing
aschut 7:9a1007e35bac 119 Kp = 10 * Pot2;
aschut 7:9a1007e35bac 120 Ki = 10 * Pot1;
aschut 7:9a1007e35bac 121
aschut 7:9a1007e35bac 122 if (!But2) {
aschut 7:9a1007e35bac 123 Kd = Kd + 0.01;
aschut 7:9a1007e35bac 124 }
aschut 7:9a1007e35bac 125 if (!But1){
aschut 7:9a1007e35bac 126 Kd = Kd - 0.01;
aschut 7:9a1007e35bac 127 }
aschut 8:bf5192a22c64 128
aschut 7:9a1007e35bac 129
aschut 6:14a9c4f30c86 130 // Proportional part:
aschut 6:14a9c4f30c86 131 double u_k = Kp * error;
aschut 6:14a9c4f30c86 132
aschut 6:14a9c4f30c86 133 // Integral part
aschut 6:14a9c4f30c86 134 error_integral = error_integral + error * Ts;
aschut 6:14a9c4f30c86 135 double u_i = Ki * error_integral;
aschut 6:14a9c4f30c86 136
aschut 6:14a9c4f30c86 137 // Derivative part
aschut 6:14a9c4f30c86 138 double error_derivative = (error - error_prev)/Ts;
aschut 6:14a9c4f30c86 139 double filtered_error_derivative = LowPassFilter.step(error_derivative);
aschut 6:14a9c4f30c86 140 double u_d = Kd * filtered_error_derivative;
aschut 6:14a9c4f30c86 141 error_prev = error;
aschut 6:14a9c4f30c86 142
aschut 6:14a9c4f30c86 143 // Sum all parts and return it
aschut 6:14a9c4f30c86 144 return u_k + u_i + u_d;
aschut 6:14a9c4f30c86 145 }
aschut 6:14a9c4f30c86 146
aschut 8:bf5192a22c64 147 void moter1_control(double u1)
aschut 6:14a9c4f30c86 148 {
aschut 8:bf5192a22c64 149 direction1= u1 < 0.0f; //positief = CW
aschut 8:bf5192a22c64 150 if (fabs(u1)> 0.7f) {
aschut 8:bf5192a22c64 151 u1 = 0.7f;
aschut 6:14a9c4f30c86 152 } else {
aschut 8:bf5192a22c64 153 u1= u1;
aschut 0:a9a42914138c 154 }
aschut 8:bf5192a22c64 155 pwmpin1= fabs(u1); //pwmduty cycle canonlybepositive, floatingpoint absolute value
aschut 6:14a9c4f30c86 156 }
aschut 6:14a9c4f30c86 157
aschut 4:99f7fdce608e 158 void PwmMotor(void)
aschut 6:14a9c4f30c86 159 {
aschut 7:9a1007e35bac 160 // Reference hoek berekenen, in graden
aschut 5:4b25551aeb6e 161 float Ellebooghoek1 = Kinematics(pwm2);
aschut 5:4b25551aeb6e 162 float Ellebooghoek4 = Limits(Ellebooghoek1);
aschut 5:4b25551aeb6e 163 ElbowReference = Ellebooghoek4;
aschut 6:14a9c4f30c86 164
aschut 7:9a1007e35bac 165 // Positie motor berekenen, in graden
aschut 8:bf5192a22c64 166 Pulses1 = encoder1.getPulses();
aschut 6:14a9c4f30c86 167 Pulses2 = encoder2.getPulses();
aschut 8:bf5192a22c64 168 motor_position1 = -(Pulses1/1200)*360;
aschut 6:14a9c4f30c86 169 motor_position2 = -(Pulses2/8400)*360;
aschut 2:926d56babb1a 170
aschut 8:bf5192a22c64 171 //double error = ElbowReference - motor_position2;
aschut 8:bf5192a22c64 172 double error1 = ElbowReference - motor_position1;
aschut 8:bf5192a22c64 173
aschut 8:bf5192a22c64 174 //double u = PID_controller(error);
aschut 8:bf5192a22c64 175 double u1 = PID_controller(error1);
aschut 8:bf5192a22c64 176 moter1_control(u1);
aschut 8:bf5192a22c64 177 //moter2_control(u);
aschut 6:14a9c4f30c86 178
aschut 6:14a9c4f30c86 179 }
aschut 6:14a9c4f30c86 180
aschut 1:58f34947c674 181 void MotorOn(void)
aschut 6:14a9c4f30c86 182 {
aschut 8:bf5192a22c64 183 pwmpin1 = 0;
aschut 4:99f7fdce608e 184 pwmpin2 = 0;
aschut 6:14a9c4f30c86 185 Pwm.attach (PwmMotor, Ts);
aschut 6:14a9c4f30c86 186
aschut 6:14a9c4f30c86 187 }
aschut 1:58f34947c674 188 void MotorOff(void)
aschut 6:14a9c4f30c86 189 {
aschut 6:14a9c4f30c86 190 Pwm.detach ();
aschut 8:bf5192a22c64 191 pwmpin1 = 0;
aschut 1:58f34947c674 192 pwmpin2 = 0;
aschut 6:14a9c4f30c86 193 }
aschut 4:99f7fdce608e 194
aschut 6:14a9c4f30c86 195 void ContinuousReader(void)
aschut 6:14a9c4f30c86 196 {
aschut 3:ac13255164cd 197 Pot2 = pot.read();
aschut 6:14a9c4f30c86 198 Pot1 = pot0.read();
aschut 8:bf5192a22c64 199 pwm1 =(Pot1*2)-1;
aschut 7:9a1007e35bac 200 pwm2 =(Pot2*2)-1; //scaling naar -1 tot 1
aschut 6:14a9c4f30c86 201 }
aschut 6:14a9c4f30c86 202
aschut 8:bf5192a22c64 203
aschut 7:9a1007e35bac 204 void Kdcount (void) // Voor het testen van de PID waardes
aschut 7:9a1007e35bac 205 {
aschut 7:9a1007e35bac 206 int count = 0;
aschut 7:9a1007e35bac 207 ElbowReference = ElbowReference + 10;
aschut 7:9a1007e35bac 208 if (count == 7) {
aschut 7:9a1007e35bac 209 ElbowReference = 0;
aschut 7:9a1007e35bac 210 count = 0;
aschut 7:9a1007e35bac 211 }
aschut 7:9a1007e35bac 212 count ++;
aschut 7:9a1007e35bac 213 }
aschut 8:bf5192a22c64 214
aschut 1:58f34947c674 215
aschut 6:14a9c4f30c86 216 int main()
aschut 6:14a9c4f30c86 217 {
aschut 6:14a9c4f30c86 218 Timer t;
aschut 6:14a9c4f30c86 219 t.start();
aschut 6:14a9c4f30c86 220 int counter = 0;
aschut 8:bf5192a22c64 221 pwmpin1.period_us(60);
aschut 6:14a9c4f30c86 222 pwmpin2.period_us(60);
aschut 6:14a9c4f30c86 223 PotRead.attach(ContinuousReader,Ts);
aschut 8:bf5192a22c64 224 Kdc.attach(Kdcount,5); //Voor PID waarde testen
aschut 2:926d56babb1a 225 pc.baud(115200);
aschut 6:14a9c4f30c86 226 //pc.printf("start\r\n");
aschut 4:99f7fdce608e 227 led = 1;
aschut 4:99f7fdce608e 228 led2 =1;
aschut 4:99f7fdce608e 229 led3 =1;
aschut 6:14a9c4f30c86 230
aschut 6:14a9c4f30c86 231 while (true) {
aschut 6:14a9c4f30c86 232 led3 = 0;
aschut 6:14a9c4f30c86 233 if (!button2) {
aschut 6:14a9c4f30c86 234 led3 = 1;
aschut 6:14a9c4f30c86 235 led = 0;
aschut 6:14a9c4f30c86 236 //pc.printf("MotorOn\r\n");
aschut 6:14a9c4f30c86 237 MotorOn();
aschut 6:14a9c4f30c86 238 }
aschut 6:14a9c4f30c86 239 if (!button3) {
aschut 6:14a9c4f30c86 240 //pc.printf("MotorOff\r\n");
aschut 6:14a9c4f30c86 241 PotRead.detach();
aschut 6:14a9c4f30c86 242 MotorOff();
aschut 6:14a9c4f30c86 243 }
aschut 4:99f7fdce608e 244 led = 0;
aschut 6:14a9c4f30c86 245 if(counter==10) {
aschut 6:14a9c4f30c86 246 float tmp = t.read();
aschut 8:bf5192a22c64 247 printf("%f,%f,%f,%f,%f,%f\n\r",tmp,motor_position1,ElbowReference,Kp,Ki,Kd);
aschut 6:14a9c4f30c86 248 counter = 0;
aschut 6:14a9c4f30c86 249 }
aschut 6:14a9c4f30c86 250 counter++;
aschut 6:14a9c4f30c86 251 wait(0.001);
aschut 1:58f34947c674 252 }
aschut 6:14a9c4f30c86 253 }
aschut 0:a9a42914138c 254
aschut 0:a9a42914138c 255
aschut 6:14a9c4f30c86 256