Jorn-Jan van de Beld / Mbed 2 deprecated kin_PD_project

begin van episch avontuur

Dependencies:   QEI mbed

main.cpp@5:21e846902e3e, 2017-10-31 (annotated)

Committer:
Happyfield
Date:
Tue Oct 31 21:09:34 2017 +0000
Revision:
5:21e846902e3e
Parent:
4:95bf97b44237
Child:
6:c97264a28123
Bijna klaar met samenvoegen

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