Jorn-Jan van de Beld / Mbed 2 deprecated kin_PD_project

begin van episch avontuur

Dependencies:   QEI mbed

main.cpp@9:182b33cabd45, 2017-11-01 (annotated)

Committer:
JornJan
Date:
Wed Nov 01 10:18:46 2017 +0000
Revision:
9:182b33cabd45
Parent:
8:668afaa63c96 
1 november;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JornJan 0:4c07bb5a9f9f 1 #include "mbed.h"
JornJan 0:4c07bb5a9f9f 2 #include "Serial.h"
JornJan 0:4c07bb5a9f9f 3 #include "math.h"
JornJan 0:4c07bb5a9f9f 4 #include "QEI.h"
JornJan 0:4c07bb5a9f9f 5
JornJan 0:4c07bb5a9f9f 6 // Connecties
JornJan 0:4c07bb5a9f9f 7 Serial pc(USBTX, USBRX); //Serial PC connectie
JornJan 2:821ae727bf8c 8 DigitalOut led_g(LED_GREEN); //Groene led op k64f bord
JornJan 0:4c07bb5a9f9f 9 DigitalOut motor1DirectionPin(D4); //Motorrichting op D4 (connected op het bord)
JornJan 0:4c07bb5a9f9f 10 PwmOut motor1MagnitudePin(D5); //Motorkracht op D5 (connected op het bord)
JornJan 2:821ae727bf8c 11 DigitalOut motor2DirectionPin(D7); //Motorrichting op D4 (connected op het bord)
JornJan 2:821ae727bf8c 12 PwmOut motor2MagnitudePin(D6); //Motorkracht op D5 (connected op het bord)
Happyfield 5:21e846902e3e 13 QEI q1_enc(D13, D12, NC, 32); //encoder motor 1 instellen
Happyfield 5:21e846902e3e 14 QEI q2_enc(D11, D10, NC, 32); // encoder motor 2 instellen
JornJan 0:4c07bb5a9f9f 15 const double pi = 3.1415926535897; // waarde voor pi aanmaken
JornJan 0:4c07bb5a9f9f 16
JornJan 9:182b33cabd45 17 double checkm1;
JornJan 9:182b33cabd45 18 double checkm2;
JornJan 9:182b33cabd45 19
JornJan 0:4c07bb5a9f9f 20 // globale gegevens
JornJan 0:4c07bb5a9f9f 21 Ticker tick_sample; // ticker voor aanroepen aansturing
JornJan 2:821ae727bf8c 22 Ticker tick_wasd; //ticker voor toetsenbord aansturing
JornJan 0:4c07bb5a9f9f 23 char key;
JornJan 0:4c07bb5a9f9f 24 double ts=0.001;
JornJan 9:182b33cabd45 25 int q1_puls;
JornJan 9:182b33cabd45 26 int q2_puls;
JornJan 2:821ae727bf8c 27 int n = 0;
Happyfield 8:668afaa63c96 28 double flex = 0;
JornJan 0:4c07bb5a9f9f 29
JornJan 0:4c07bb5a9f9f 30 // kinematica gegevens
JornJan 0:4c07bb5a9f9f 31 // lengte armen
JornJan 0:4c07bb5a9f9f 32 double L1 = 0.250;
JornJan 0:4c07bb5a9f9f 33 double L2 = 0.355;
JornJan 0:4c07bb5a9f9f 34 double L3 = 0.150;
JornJan 0:4c07bb5a9f9f 35
JornJan 0:4c07bb5a9f9f 36 // reference position
JornJan 3:329acce2487c 37 double q1=0; // positie q1 in radialen
JornJan 0:4c07bb5a9f9f 38 double q2=0; // positie q2 in radialen
JornJan 0:4c07bb5a9f9f 39 double q1_pos;
JornJan 0:4c07bb5a9f9f 40 double q2_pos;
JornJan 0:4c07bb5a9f9f 41
JornJan 0:4c07bb5a9f9f 42 // EMG Input_k
Happyfield 7:9e965efc430e 43 double vx;
Happyfield 7:9e965efc430e 44 double vy;
JornJan 0:4c07bb5a9f9f 45
JornJan 0:4c07bb5a9f9f 46
JornJan 0:4c07bb5a9f9f 47 // PID gegevens
Happyfield 8:668afaa63c96 48 double pulses2rad=(2*pi)/4200;
Happyfield 8:668afaa63c96 49 double rad2pulses=4200/(2*pi);
JornJan 0:4c07bb5a9f9f 50 double position;
JornJan 0:4c07bb5a9f9f 51 double ref1;
JornJan 0:4c07bb5a9f9f 52 double ref2;
Happyfield 6:c97264a28123 53 double PI1;
Happyfield 6:c97264a28123 54 double PI2;
Happyfield 5:21e846902e3e 55 double error1_1=0;
Happyfield 5:21e846902e3e 56 double error1_2=0;
Happyfield 5:21e846902e3e 57 double error2_1=0;
Happyfield 5:21e846902e3e 58 double error2_2=0;
Happyfield 5:21e846902e3e 59 double error_I_1=0;
Happyfield 5:21e846902e3e 60 double error_I2_1=0;
Happyfield 5:21e846902e3e 61 double error_I_2=0;
Happyfield 5:21e846902e3e 62 double error_I2_2=0;
JornJan 0:4c07bb5a9f9f 63
JornJan 9:182b33cabd45 64 double Kp1 = 0.2; // proportional coefficient motor 1
JornJan 9:182b33cabd45 65 double Ki1 = 0.02; // integrating coefficient motor 1
JornJan 0:4c07bb5a9f9f 66
JornJan 9:182b33cabd45 67 double Kp2 = 0.0015; // proportional coefficient motor 2
JornJan 9:182b33cabd45 68 double Ki2 = 0.035; // integrating coefficient motor 2
JornJan 9:182b33cabd45 69
Happyfield 7:9e965efc430e 70
JornJan 2:821ae727bf8c 71 void wasd()
JornJan 0:4c07bb5a9f9f 72 {
JornJan 2:821ae727bf8c 73 static char oldkey = 'p';
JornJan 2:821ae727bf8c 74 static double oldvx = 0;
JornJan 2:821ae727bf8c 75 static double oldvy = 0;
JornJan 2:821ae727bf8c 76
JornJan 0:4c07bb5a9f9f 77 if(pc.readable()==true)
JornJan 0:4c07bb5a9f9f 78 { key = pc.getc();
JornJan 2:821ae727bf8c 79
JornJan 0:4c07bb5a9f9f 80 if (key=='a')
JornJan 0:4c07bb5a9f9f 81 {
JornJan 9:182b33cabd45 82 vx = 0.02; //referentie snelheid m/s
Happyfield 8:668afaa63c96 83 vy = 0;
JornJan 0:4c07bb5a9f9f 84 }
JornJan 0:4c07bb5a9f9f 85 else if(key=='d')
JornJan 0:4c07bb5a9f9f 86 {
JornJan 9:182b33cabd45 87 vx = -0.02;
Happyfield 8:668afaa63c96 88 vy = 0;
JornJan 0:4c07bb5a9f9f 89 }
JornJan 0:4c07bb5a9f9f 90 else if(key=='w')
JornJan 0:4c07bb5a9f9f 91 {
Happyfield 8:668afaa63c96 92 vx = 0;
JornJan 9:182b33cabd45 93 vy = 0.02;
JornJan 0:4c07bb5a9f9f 94 }
JornJan 0:4c07bb5a9f9f 95 else if(key=='s')
JornJan 0:4c07bb5a9f9f 96 {
Happyfield 8:668afaa63c96 97 vx = 0;
JornJan 9:182b33cabd45 98 vy = -0.02;
JornJan 0:4c07bb5a9f9f 99 }
JornJan 2:821ae727bf8c 100 else
JornJan 0:4c07bb5a9f9f 101 {
JornJan 2:821ae727bf8c 102 key = oldkey;
JornJan 2:821ae727bf8c 103 vx = oldvx;
JornJan 2:821ae727bf8c 104 vy = oldvy;
JornJan 0:4c07bb5a9f9f 105 }
JornJan 2:821ae727bf8c 106
JornJan 2:821ae727bf8c 107 } //einde eerste if statemnet
JornJan 2:821ae727bf8c 108
JornJan 2:821ae727bf8c 109 else if (pc.readable()==false)
JornJan 0:4c07bb5a9f9f 110 {
JornJan 0:4c07bb5a9f9f 111 vx=0;
JornJan 2:821ae727bf8c 112 vy=0;
JornJan 2:821ae727bf8c 113 key='p';
JornJan 0:4c07bb5a9f9f 114 }
JornJan 2:821ae727bf8c 115 oldkey = key;
JornJan 2:821ae727bf8c 116 oldvx = vx;
JornJan 2:821ae727bf8c 117 oldvy = vy;
JornJan 2:821ae727bf8c 118
JornJan 0:4c07bb5a9f9f 119 }
JornJan 0:4c07bb5a9f9f 120
JornJan 2:821ae727bf8c 121 void kinematics()
JornJan 0:4c07bb5a9f9f 122 {
JornJan 0:4c07bb5a9f9f 123
Happyfield 5:21e846902e3e 124 q1 = ((-(L3 + L2*cos(q1_pos - q2_pos))/(L1*L2*cos(2*q1_pos - q2_pos) + L1*L3*cos(q1_pos)))*vx + ((L2*sin(q1_pos - q2_pos))/(L1*L2*cos(2*q1_pos - q2_pos) + L1*L3*cos(q1_pos)))*vy) * ts + q1_pos;
Happyfield 5:21e846902e3e 125 q2 = (((L3 - L1*sin(q1_pos) + L2*cos(q1_pos - q2_pos))/(L1*L2*cos(2*q1_pos - q2_pos) + L1*L3*cos(q1_pos))*vx) + ((L1*cos(q1_pos) - L2*sin(q1_pos - q2_pos))/(L1*L2*cos(2*q1_pos - q2_pos) + L1*L3*cos(q1_pos)))*vy) * ts + q2_pos;
Happyfield 5:21e846902e3e 126
JornJan 9:182b33cabd45 127 ref1 = q1*rad2pulses; // converteert de radialen weer terug naar pulses voor verwerking in PID
Happyfield 5:21e846902e3e 128 ref2 = q2*rad2pulses;
JornJan 0:4c07bb5a9f9f 129 }
JornJan 0:4c07bb5a9f9f 130
Happyfield 5:21e846902e3e 131 void controller()
Happyfield 8:668afaa63c96 132 {
Happyfield 5:21e846902e3e 133 //PID 1
Happyfield 5:21e846902e3e 134 error1_1 = ref1 - q1_puls;
JornJan 9:182b33cabd45 135 error_I_1 = error_I2_1 + ts*((error1_1 + error2_1)/2);
Happyfield 5:21e846902e3e 136
JornJan 9:182b33cabd45 137 PI1 = Kp1*error1_1 + Ki1*error_I_1;
Happyfield 5:21e846902e3e 138
JornJan 9:182b33cabd45 139 error2_1 = error1_1; // opslaan variabelen voor integraal onderdeel
Happyfield 5:21e846902e3e 140 error_I2_1 = error_I_1;
Happyfield 5:21e846902e3e 141
Happyfield 5:21e846902e3e 142 //PID 2
Happyfield 5:21e846902e3e 143 error1_2 = ref2 - q2_puls;
JornJan 9:182b33cabd45 144 error_I_2 = error_I2_2 + ts*((error1_2 + error2_2)/2);
Happyfield 5:21e846902e3e 145
Happyfield 5:21e846902e3e 146 PI2 = Kp2*error1_2 + Ki2*(error_I_2);
Happyfield 4:95bf97b44237 147
JornJan 9:182b33cabd45 148 error2_2 = error1_2; // opslaan variabelen voor integraal onderdeel
Happyfield 5:21e846902e3e 149 error_I2_2 = error_I_2;
Happyfield 5:21e846902e3e 150
Happyfield 6:c97264a28123 151 //Motor control
JornJan 9:182b33cabd45 152 if (PI1==0 || PI2==0)
Happyfield 5:21e846902e3e 153 {
JornJan 9:182b33cabd45 154 if (PI1<=0)
JornJan 9:182b33cabd45 155 {
JornJan 9:182b33cabd45 156 motor1DirectionPin = 0;
JornJan 9:182b33cabd45 157 motor1MagnitudePin = fabs(PI1);
JornJan 9:182b33cabd45 158 motor2MagnitudePin = 0;
JornJan 9:182b33cabd45 159 }
JornJan 9:182b33cabd45 160 else if (PI1>0)
JornJan 9:182b33cabd45 161 {
JornJan 9:182b33cabd45 162 motor1DirectionPin = 1;
JornJan 9:182b33cabd45 163 motor1MagnitudePin = fabs(PI1);
JornJan 9:182b33cabd45 164 motor2MagnitudePin = 0;
JornJan 9:182b33cabd45 165 }
JornJan 9:182b33cabd45 166 else if (PI2<=0)
JornJan 9:182b33cabd45 167 {
JornJan 9:182b33cabd45 168 motor2DirectionPin = 0;
JornJan 9:182b33cabd45 169 motor2MagnitudePin = fabs(PI2);
JornJan 9:182b33cabd45 170 motor1MagnitudePin = 0;
JornJan 9:182b33cabd45 171 }
JornJan 9:182b33cabd45 172 else if (PI2>0)
JornJan 9:182b33cabd45 173 {
JornJan 9:182b33cabd45 174 motor2DirectionPin = 1;
JornJan 9:182b33cabd45 175 motor2MagnitudePin = fabs(PI2);
JornJan 9:182b33cabd45 176 motor1MagnitudePin = 0;
JornJan 9:182b33cabd45 177 }
JornJan 9:182b33cabd45 178 }
JornJan 9:182b33cabd45 179
JornJan 9:182b33cabd45 180 else if (PI1<0 && PI2<0)
JornJan 9:182b33cabd45 181 {
JornJan 9:182b33cabd45 182 motor1DirectionPin = 1;
Happyfield 5:21e846902e3e 183 motor1MagnitudePin = fabs(PI1);
Happyfield 4:95bf97b44237 184 motor2DirectionPin = 0;
Happyfield 5:21e846902e3e 185 motor2MagnitudePin = fabs(PI2);
Happyfield 4:95bf97b44237 186 }
Happyfield 6:c97264a28123 187 else if (PI1>0 && PI2<0)
Happyfield 6:c97264a28123 188 {
Happyfield 6:c97264a28123 189 motor1DirectionPin = 1;
Happyfield 6:c97264a28123 190 motor1MagnitudePin = fabs(PI1);
Happyfield 6:c97264a28123 191 motor2DirectionPin = 0;
Happyfield 6:c97264a28123 192 motor2MagnitudePin = fabs(PI2);
Happyfield 6:c97264a28123 193 }
Happyfield 6:c97264a28123 194
Happyfield 6:c97264a28123 195 else if (PI1<0 && PI2>0)
Happyfield 4:95bf97b44237 196 {
Happyfield 6:c97264a28123 197 motor1DirectionPin = 0;
Happyfield 6:c97264a28123 198 motor1MagnitudePin = fabs(PI1);
Happyfield 6:c97264a28123 199 motor2DirectionPin = 1;
Happyfield 6:c97264a28123 200 motor2MagnitudePin = fabs(PI2);
Happyfield 6:c97264a28123 201 }
Happyfield 6:c97264a28123 202
Happyfield 6:c97264a28123 203 else if (PI1>0 && PI2>0)
Happyfield 6:c97264a28123 204 {
JornJan 9:182b33cabd45 205 motor1DirectionPin = 0;
Happyfield 6:c97264a28123 206 motor1MagnitudePin = fabs(PI1);
Happyfield 4:95bf97b44237 207 motor2DirectionPin = 1;
Happyfield 5:21e846902e3e 208 motor2MagnitudePin = fabs(PI2);
Happyfield 4:95bf97b44237 209 }
JornJan 9:182b33cabd45 210
JornJan 9:182b33cabd45 211 checkm1 = motor1MagnitudePin;
JornJan 9:182b33cabd45 212 checkm2 = motor2MagnitudePin;
Happyfield 4:95bf97b44237 213 }
JornJan 0:4c07bb5a9f9f 214
JornJan 0:4c07bb5a9f9f 215 void aansturing()
Happyfield 6:c97264a28123 216 {
JornJan 0:4c07bb5a9f9f 217 // encoders uitlezen
JornJan 0:4c07bb5a9f9f 218 q1_puls = q1_enc.getPulses();
JornJan 0:4c07bb5a9f9f 219 q1_pos = q1_puls*pulses2rad; // berekent positie q1 in radialen
JornJan 0:4c07bb5a9f9f 220 q2_puls = q2_enc.getPulses();
JornJan 0:4c07bb5a9f9f 221 q2_pos = q2_puls*pulses2rad; // berekent positie q2 in radialen
JornJan 2:821ae727bf8c 222
JornJan 0:4c07bb5a9f9f 223 // kinematica bepaald gewenste q1 en q2 referenties afhankelijk van ingegeven vx en vy
JornJan 2:821ae727bf8c 224 kinematics();
JornJan 2:821ae727bf8c 225
JornJan 0:4c07bb5a9f9f 226 // PD controller gebruikt PD control en stuurt motor aan
Happyfield 7:9e965efc430e 227 controller();
JornJan 9:182b33cabd45 228
Happyfield 7:9e965efc430e 229 if(n==500){
JornJan 9:182b33cabd45 230 printf("checkm1 = %f checkm2 = %f\n\r", ref1, ref2);
Happyfield 7:9e965efc430e 231 n=0;
Happyfield 7:9e965efc430e 232 }
Happyfield 7:9e965efc430e 233 else{
Happyfield 7:9e965efc430e 234 n=n+1;
Happyfield 7:9e965efc430e 235 }
Happyfield 6:c97264a28123 236 }
JornJan 0:4c07bb5a9f9f 237
JornJan 0:4c07bb5a9f9f 238
JornJan 0:4c07bb5a9f9f 239 int main()
JornJan 0:4c07bb5a9f9f 240 {
JornJan 0:4c07bb5a9f9f 241 pc.baud(115200);
Happyfield 8:668afaa63c96 242 tick_wasd.attach_us(&wasd, 100000);
JornJan 2:821ae727bf8c 243 tick_sample.attach_us(&aansturing, 1000); //sample frequency 1000 Hz;
Happyfield 8:668afaa63c96 244 led_g = 1;
JornJan 2:821ae727bf8c 245
JornJan 0:4c07bb5a9f9f 246 while(true)
JornJan 0:4c07bb5a9f9f 247 {
JornJan 2:821ae727bf8c 248
JornJan 0:4c07bb5a9f9f 249 }
JornJan 0:4c07bb5a9f9f 250 }