Jorn-Jan van de Beld / Mbed 2 deprecated kin_PD_project

begin van episch avontuur

Dependencies:   QEI mbed

main.cpp@8:668afaa63c96, 2017-11-01 (annotated)

Committer:
Happyfield
Date:
Wed Nov 01 00:00:23 2017 +0000
Revision:
8:668afaa63c96
Parent:
7:9e965efc430e
Child:
9:182b33cabd45
We zijn super moe nu, klaar dr mee

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