Fernon Eijkhoudt / Mbed 2 deprecated ROBOBIRDS_FINAL

EMG driven robot which shoots elastic bands

Dependencies:   QEI mbed

Fork of RoboBirdV1 by Fernon Eijkhoudt

main.cpp@38:eaf245d88d84, 2015-10-29 (annotated)

Committer:
Fernon
Date:
Thu Oct 29 09:34:20 2015 +0000
Revision:
38:eaf245d88d84
Parent:
37:0dbf536a95c8
Child:
39:e5bf4b1293fa
Working robot with buttons and all! Hidscope is still used for possible debugging.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Fernon 0:5a5f417fa1b2 1 #include "mbed.h"
Fernon 16:b0ec8e6a8ad4 2 #include "QEI.h"
Fernon 8:a2b725b502d8 3 #include "math.h"
yc238 12:dcf90cafb2a5 4 #include "HIDScope.h"
Fernon 0:5a5f417fa1b2 5
Fernon 25:230bd4e1f3ef 6
Fernon 25:230bd4e1f3ef 7
Fernon 25:230bd4e1f3ef 8 // Motor 1 & 2
Fernon 22:2e1713475f5f 9 DigitalOut Direction(D4); //1 = CCW - 0 = CW, moet nog omgezet worden naar up en down
Fernon 22:2e1713475f5f 10 PwmOut PowerMotor(D5); //van 0 tot 1
Fernon 25:230bd4e1f3ef 11 QEI Encoder(D10,D11,NC,32,QEI::X2_ENCODING); //Encoder
Fernon 28:b7d01a55530f 12 DigitalOut Direction2(D7);
Fernon 28:b7d01a55530f 13 PwmOut PowerMotor2(D6);
Fernon 29:7653adbbb101 14 QEI Encoder2(D12,D13,NC,32,QEI::X2_ENCODING);
Fernon 23:c97e206cf2a7 15 PwmOut PowerServo(D3);
Fernon 25:230bd4e1f3ef 16
Fernon 32:c2c80a2ca83d 17 // Buttons & EMG
Fernon 38:eaf245d88d84 18 AnalogIn Button(A3); // Calibration Button
Fernon 38:eaf245d88d84 19 AnalogIn Button2(A4); // Fire button
Fernon 32:c2c80a2ca83d 20 DigitalIn Button3(D9);
Fernon 36:5d1225d72bca 21 AnalogIn EMG(A0);
Fernon 36:5d1225d72bca 22 AnalogIn EMG2(A1);
Fernon 25:230bd4e1f3ef 23
Fernon 25:230bd4e1f3ef 24 // Tickers & timers
Fernon 31:85d3b4db5e2b 25
Fernon 31:85d3b4db5e2b 26 Ticker biquadTicker;
Fernon 22:2e1713475f5f 27 Ticker MotorWrite;
Fernon 22:2e1713475f5f 28 Ticker Sender;
Fernon 22:2e1713475f5f 29 Timer timer;
Fernon 31:85d3b4db5e2b 30 Timer TijdEMGCal;
Fernon 25:230bd4e1f3ef 31
Fernon 25:230bd4e1f3ef 32 // Debugging
Fernon 25:230bd4e1f3ef 33 Serial pc(USBTX, USBRX);
Fernon 29:7653adbbb101 34 HIDScope scope(6);
Fernon 38:eaf245d88d84 35 DigitalOut ledF(LED_RED);
Fernon 38:eaf245d88d84 36 DigitalOut ledC(LED_GREEN);
Fernon 0:5a5f417fa1b2 37
yc238 15:f7e2b20f4dca 38
Fernon 25:230bd4e1f3ef 39
Fernon 25:230bd4e1f3ef 40 // Waardes
Fernon 25:230bd4e1f3ef 41 const double twopi = 6.2831853071795;
Fernon 25:230bd4e1f3ef 42 const double Fs=100;
Fernon 25:230bd4e1f3ef 43 int Fired = 0;
Fernon 25:230bd4e1f3ef 44
Fernon 25:230bd4e1f3ef 45 // EMG
Fernon 32:c2c80a2ca83d 46 double T1 = 0;
Fernon 32:c2c80a2ca83d 47 double T2 = 0;
Fernon 32:c2c80a2ca83d 48 double T3 = 0;
Fernon 32:c2c80a2ca83d 49 double T4 = 0;
Fernon 25:230bd4e1f3ef 50
Fernon 25:230bd4e1f3ef 51 // Motor 1 (Translatie)
Fernon 38:eaf245d88d84 52 const double n1 = 3.3; // Aantal rondjes dat ons apparaat maximaal mag draaien (omhoog)
Fernon 22:2e1713475f5f 53 int Pulses;
Fernon 22:2e1713475f5f 54 double Rotatie = 0; //aantal graden dat de motor is gedraaid
Fernon 25:230bd4e1f3ef 55 double Goal; //initele waarde goal waar de motor naar toe moet, dit wordt gedaan
Fernon 22:2e1713475f5f 56 double Error = 0;
Fernon 22:2e1713475f5f 57 double Errord = 0;
Fernon 22:2e1713475f5f 58 double Errori = 0;
Fernon 22:2e1713475f5f 59 double Errorp = 0;
Fernon 22:2e1713475f5f 60 const double Kp = 0.2; //Moet berekend worden aan de hand van Control concept slides
Fernon 22:2e1713475f5f 61 const double Kd = 10;
Fernon 22:2e1713475f5f 62 const double Ki = 0.2;
Fernon 32:c2c80a2ca83d 63 double v = 0;
Fernon 25:230bd4e1f3ef 64 double upperlimit; //max aantal rotaties omhoog
Fernon 33:4e3870ab4e17 65 const double downlimit = 0.8;
Fernon 33:4e3870ab4e17 66 const double margin = 0.8;
Fernon 25:230bd4e1f3ef 67 bool OutRange = false;
Fernon 25:230bd4e1f3ef 68
Fernon 25:230bd4e1f3ef 69 // Motor 2 (Rotatie)
Fernon 38:eaf245d88d84 70 const double n2 = 0.4; // Aantal rondjes dat dat ons apparaat maximaal mag draaien (rotatie)
Fernon 25:230bd4e1f3ef 71 int Pulses2;
Fernon 25:230bd4e1f3ef 72 double Rotatie2 = 0;
Fernon 25:230bd4e1f3ef 73 double Goal2;
Fernon 25:230bd4e1f3ef 74 double Error2 = 0;
Fernon 25:230bd4e1f3ef 75 double Errord2 = 0;
Fernon 25:230bd4e1f3ef 76 double Errori2 = 0;
Fernon 25:230bd4e1f3ef 77 double Errorp2 = 0;
Fernon 25:230bd4e1f3ef 78 const double Kp2 = 0.2;
Fernon 25:230bd4e1f3ef 79 const double Kd2 = 10;
Fernon 25:230bd4e1f3ef 80 const double Ki2 = 0.2;
Fernon 25:230bd4e1f3ef 81 double v2 = 0;
Fernon 28:b7d01a55530f 82 double turnlimit = 0.4; // max aantal rotaties voor roteren hele robot
Fernon 25:230bd4e1f3ef 83 // Margin 2 is in ons geval 0
Fernon 25:230bd4e1f3ef 84 bool OutRange2 = false;
Fernon 25:230bd4e1f3ef 85
Fernon 25:230bd4e1f3ef 86 // Activatie tussen het schietgedeelte en terugkeergedeelte
Fernon 22:2e1713475f5f 87 bool Excecute = false;
Fernon 22:2e1713475f5f 88 bool Home = false;
Fernon 25:230bd4e1f3ef 89
Fernon 31:85d3b4db5e2b 90 // Filter
Fernon 33:4e3870ab4e17 91 double Fs2 = 500; // in Hz
Fernon 31:85d3b4db5e2b 92 const double TijdCal = 5;
Fernon 31:85d3b4db5e2b 93 double Max = 0;
Fernon 31:85d3b4db5e2b 94 double Max2 = 0;
Fernon 31:85d3b4db5e2b 95 bool Cali = false;
Fernon 31:85d3b4db5e2b 96
Fernon 31:85d3b4db5e2b 97 double u1;
Fernon 31:85d3b4db5e2b 98 double y1;
Fernon 31:85d3b4db5e2b 99 double y2;
Fernon 31:85d3b4db5e2b 100 double y3;
Fernon 31:85d3b4db5e2b 101 double y4;
Fernon 31:85d3b4db5e2b 102 double y5;
Fernon 31:85d3b4db5e2b 103 double y6;
Fernon 31:85d3b4db5e2b 104 double y7;
Fernon 31:85d3b4db5e2b 105 double y8;
Fernon 36:5d1225d72bca 106 double y9;
Fernon 36:5d1225d72bca 107 double y10;
Fernon 31:85d3b4db5e2b 108
Fernon 31:85d3b4db5e2b 109 double u10;
Fernon 36:5d1225d72bca 110 double y11;
Fernon 31:85d3b4db5e2b 111 double y12;
Fernon 31:85d3b4db5e2b 112 double y13;
Fernon 31:85d3b4db5e2b 113 double y14;
Fernon 31:85d3b4db5e2b 114 double y15;
Fernon 31:85d3b4db5e2b 115 double y16;
Fernon 31:85d3b4db5e2b 116 double y17;
Fernon 31:85d3b4db5e2b 117 double y18;
Fernon 31:85d3b4db5e2b 118
Fernon 31:85d3b4db5e2b 119 double f1_v1=0,f1_v2=0;
Fernon 31:85d3b4db5e2b 120 double f2_v1=0,f2_v2=0;
Fernon 31:85d3b4db5e2b 121 double f3_v1=0,f3_v2=0;
Fernon 31:85d3b4db5e2b 122 double f4_v1=0,f4_v2=0;
Fernon 31:85d3b4db5e2b 123 double f5_v1=0,f5_v2=0;
Fernon 31:85d3b4db5e2b 124 double f6_v1=0,f6_v2=0;
Fernon 31:85d3b4db5e2b 125 double f7_v1=0,f7_v2=0;
Fernon 31:85d3b4db5e2b 126
Fernon 31:85d3b4db5e2b 127 double f1_v3=0,f1_v4=0;
Fernon 31:85d3b4db5e2b 128 double f2_v3=0,f2_v4=0;
Fernon 31:85d3b4db5e2b 129 double f3_v3=0,f3_v4=0;
Fernon 31:85d3b4db5e2b 130 double f4_v3=0,f4_v4=0;
Fernon 31:85d3b4db5e2b 131 double f5_v3=0,f5_v4=0;
Fernon 31:85d3b4db5e2b 132 double f6_v3=0,f6_v4=0;
Fernon 31:85d3b4db5e2b 133 double f7_v3=0,f7_v4=0;
Fernon 31:85d3b4db5e2b 134
Fernon 31:85d3b4db5e2b 135 // Notch
Fernon 31:85d3b4db5e2b 136 const double gainNotch3=0.969922;
Fernon 31:85d3b4db5e2b 137 const double f3_a1=-1.56143694016, f3_a2=0.93984421899, f3_b0=1.00000000000, f3_b1=-1.60985807508, f3_b2=1.00000000000;
Fernon 31:85d3b4db5e2b 138 const double gainNotch4=0.975183;
Fernon 31:85d3b4db5e2b 139 const double f4_a1=-1.55188488157,f4_a2=0.96839115647,f4_b0=1.000000000,f4_b1=-1.60985807508,f4_b2=1.00000000;
Fernon 31:85d3b4db5e2b 140 const double gainNotch5=0.993678;
Fernon 31:85d3b4db5e2b 141 const double f5_a1=-1.61645491476,f5_a2=0.97057916088,f5_b0=1.000000000,f5_b1=-1.60985807508,f5_b2=1.00000000;
Fernon 31:85d3b4db5e2b 142
Fernon 31:85d3b4db5e2b 143 // High pass
Fernon 36:5d1225d72bca 144 const double gainHP1=0.96860379641660377;
Fernon 36:5d1225d72bca 145 const double f1_a1=-1.9352943868599917,f1_a2=0.96960379641660377,f1_b0=1.0000000,f1_b1=-2.00000000,f1_b2=1.000000000;
Fernon 31:85d3b4db5e2b 146 const double gainHP2=0.935820;
Fernon 36:5d1225d72bca 147 const double f2_a1=-0.939062508174924,f2_a2=0,f2_b0=1.0000000000,f2_b1=-1.0000000,f2_b2=0.0000000;
Fernon 31:85d3b4db5e2b 148
Fernon 31:85d3b4db5e2b 149 // Low pass
Fernon 31:85d3b4db5e2b 150 const double gainLP6=0.000048;
Fernon 31:85d3b4db5e2b 151 const double f6_a1=-1.97326192076 , f6_a2=0.97345330126 , f6_b0=1.0000000 , f6_b1=2.0000000 , f6_b2=1.0000000;
Fernon 31:85d3b4db5e2b 152 const double gainLP7=0.000048;
Fernon 31:85d3b4db5e2b 153 const double f7_a1=-1.98030504048 , f7_a2=0.98049710408 , f7_b0=1.0000000 , f7_b1=2.0000000 , f7_b2=1.0000000;
Fernon 31:85d3b4db5e2b 154
Fernon 10:e210675cbe71 155
Fernon 25:230bd4e1f3ef 156
Fernon 25:230bd4e1f3ef 157 // Voids voor berekeningen in het hoofdprogramma
Fernon 2:f0e9ffc5df09 158
Fernon 31:85d3b4db5e2b 159 double biquad(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2)
Fernon 31:85d3b4db5e2b 160 {
Fernon 31:85d3b4db5e2b 161 double v = u-a1*v1-a2*v2;
Fernon 31:85d3b4db5e2b 162 double y=b0*v+b1*v1+b2*v2;
Fernon 31:85d3b4db5e2b 163 v2=v1;
Fernon 31:85d3b4db5e2b 164 v1=v;
Fernon 31:85d3b4db5e2b 165 return y;
Fernon 31:85d3b4db5e2b 166 }
Fernon 31:85d3b4db5e2b 167
Fernon 31:85d3b4db5e2b 168 void myController()
Fernon 31:85d3b4db5e2b 169 {
Fernon 31:85d3b4db5e2b 170 // EMG 1
Fernon 31:85d3b4db5e2b 171 u1 = EMG.read();
Fernon 31:85d3b4db5e2b 172 // Notch
Fernon 32:c2c80a2ca83d 173 y1 = biquad(u1,f3_v1,f3_v2,f3_a1,f3_a2,f3_b0*gainNotch3,f3_b1*gainNotch3,f3_b2*gainNotch3);
Fernon 32:c2c80a2ca83d 174 y2 = biquad(y1,f4_v1,f4_v2,f4_a1,f4_a2,f4_b0*gainNotch4,f4_b1*gainNotch4,f4_b2*gainNotch4);
Fernon 32:c2c80a2ca83d 175 y3 = biquad(y2,f5_v1,f5_v2,f5_a1,f5_a2,f5_b0*gainNotch5,f5_b1*gainNotch5,f5_b2*gainNotch5);
Fernon 31:85d3b4db5e2b 176
Fernon 31:85d3b4db5e2b 177 // HP
Fernon 32:c2c80a2ca83d 178 y4 = biquad(y3,f1_v1,f1_v2,f1_a1,f1_a2,f1_b0*gainHP1,f1_b1*gainHP1,f1_b2*gainHP1);
Fernon 32:c2c80a2ca83d 179 y5 = biquad(y4,f2_v1,f2_v2,f2_a1,f2_a2,f2_b0*gainHP2,f2_b1*gainHP2,f2_b2*gainHP2);
Fernon 31:85d3b4db5e2b 180
Fernon 31:85d3b4db5e2b 181 // LP
Fernon 31:85d3b4db5e2b 182 y6 = fabs(y5);
Fernon 32:c2c80a2ca83d 183 y7 = biquad(y6,f6_v1,f6_v2,f6_a1,f6_a2,gainLP6*f6_b0,gainLP6*f6_b1,gainLP6*f6_b2);
Fernon 32:c2c80a2ca83d 184 y8 = biquad(y7,f7_v1,f7_v2,f7_a1,f7_a2,gainLP7*f7_b0,gainLP7*f7_b1,gainLP7*f7_b2);
Fernon 35:b71140a46b9c 185
Fernon 35:b71140a46b9c 186
Fernon 31:85d3b4db5e2b 187 // EMG 2
Fernon 35:b71140a46b9c 188 u10 = EMG2.read();
Fernon 31:85d3b4db5e2b 189 // Notch
Fernon 36:5d1225d72bca 190 y11 = biquad(u10,f3_v3,f3_v4,f3_a1,f3_a2,f3_b0*gainNotch3,f3_b1*gainNotch3,f3_b2*gainNotch3);
Fernon 36:5d1225d72bca 191 y12 = biquad(y11,f4_v3,f4_v4,f4_a1,f4_a2,f4_b0*gainNotch4,f4_b1*gainNotch4,f4_b2*gainNotch4);
Fernon 32:c2c80a2ca83d 192 y13 = biquad(y12,f5_v3,f5_v4,f5_a1,f5_a2,f5_b0*gainNotch5,f5_b1*gainNotch5,f5_b2*gainNotch5);
Fernon 31:85d3b4db5e2b 193
Fernon 31:85d3b4db5e2b 194 // HP
Fernon 32:c2c80a2ca83d 195 y14 = biquad(y13,f1_v3,f1_v4,f1_a1,f1_a2,f1_b0*gainHP1,f1_b1*gainHP1,f1_b2*gainHP1);
Fernon 32:c2c80a2ca83d 196 y15 = biquad(y14,f2_v3,f2_v4,f2_a1,f2_a2,f2_b0*gainHP2,f2_b1*gainHP2,f2_b2*gainHP2);
Fernon 31:85d3b4db5e2b 197
Fernon 31:85d3b4db5e2b 198 // LP
Fernon 31:85d3b4db5e2b 199 y16 = fabs(y15);
Fernon 32:c2c80a2ca83d 200 y17 = biquad(y16,f6_v3,f6_v4,f6_a1,f6_a2,gainLP6*f6_b0,gainLP6*f6_b1,gainLP6*f6_b2);
Fernon 32:c2c80a2ca83d 201 y18 = biquad(y17,f7_v3,f7_v4,f7_a1,f7_a2,gainLP7*f7_b0,gainLP7*f7_b1,gainLP7*f7_b2);
Fernon 31:85d3b4db5e2b 202
Fernon 31:85d3b4db5e2b 203 if (Cali == true) {
Fernon 31:85d3b4db5e2b 204 if (y8 >= Max) {
Fernon 31:85d3b4db5e2b 205 Max = y8;
Fernon 31:85d3b4db5e2b 206 }
Fernon 31:85d3b4db5e2b 207 if (y18 >= Max2) {
Fernon 31:85d3b4db5e2b 208 Max2 = y18;
Fernon 31:85d3b4db5e2b 209 }
Fernon 31:85d3b4db5e2b 210 }
Fernon 31:85d3b4db5e2b 211 }
Fernon 31:85d3b4db5e2b 212
yc238 14:b9c925a8176a 213 void MotorSet()
Fernon 2:f0e9ffc5df09 214 {
Fernon 27:9cca2ad74ec0 215 // Eerst motor 1 (translatie)
Fernon 23:c97e206cf2a7 216 if (OutRange) {
Fernon 23:c97e206cf2a7 217 Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal
Fernon 23:c97e206cf2a7 218 Errord = (Error-Errorp)/Fs;
Fernon 23:c97e206cf2a7 219 Errorp = Error;
Fernon 27:9cca2ad74ec0 220 if (fabs(Error) <= 0.5) {
Fernon 23:c97e206cf2a7 221 Errori = Errori + Error*1/Fs;
Fernon 23:c97e206cf2a7 222 } else {
Fernon 23:c97e206cf2a7 223 Errori = 0;
Fernon 23:c97e206cf2a7 224 }
Fernon 23:c97e206cf2a7 225 if (Error>=0) {
Fernon 23:c97e206cf2a7 226 Direction=1;
Fernon 23:c97e206cf2a7 227 } else {
Fernon 23:c97e206cf2a7 228 Direction=0;
Fernon 23:c97e206cf2a7 229 }
Fernon 23:c97e206cf2a7 230 v=Kp*Error + Kd*Errord + Ki*Errori;
Fernon 33:4e3870ab4e17 231 if (Home == true) {
Fernon 33:4e3870ab4e17 232 if (v >0.15) {
Fernon 33:4e3870ab4e17 233 v = 0.15;
Fernon 33:4e3870ab4e17 234 }
Fernon 33:4e3870ab4e17 235 }
Fernon 22:2e1713475f5f 236 }
yc238 14:b9c925a8176a 237 PowerMotor.write(fabs(v));
Fernon 25:230bd4e1f3ef 238
Fernon 27:9cca2ad74ec0 239 // Dan motor 2 (rotatie)
Fernon 27:9cca2ad74ec0 240 if (OutRange2) {
Fernon 27:9cca2ad74ec0 241 Error2 = Goal2-Rotatie2; // De error die het motortje maakt ten opzichte van je Goal
Fernon 27:9cca2ad74ec0 242 Errord2 = (Error2-Errorp2)/Fs;
Fernon 27:9cca2ad74ec0 243 Errorp2 = Error2;
Fernon 27:9cca2ad74ec0 244 if (fabs(Error2) <= 0.5) {
Fernon 27:9cca2ad74ec0 245 Errori2 = Errori2 + Error2*1/Fs;
Fernon 27:9cca2ad74ec0 246 } else {
Fernon 27:9cca2ad74ec0 247 Errori2 = 0;
Fernon 27:9cca2ad74ec0 248 }
Fernon 27:9cca2ad74ec0 249 if (Error2>=0) {
Fernon 29:7653adbbb101 250 Direction2 = 0;
Fernon 27:9cca2ad74ec0 251 } else {
Fernon 29:7653adbbb101 252 Direction2 = 1;
Fernon 27:9cca2ad74ec0 253 }
Fernon 27:9cca2ad74ec0 254 v2=Kp2*Error2 + Kd2*Errord2 + Ki2*Errori2;
Fernon 27:9cca2ad74ec0 255 }
Fernon 27:9cca2ad74ec0 256 PowerMotor2.write(fabs(v2));
Fernon 2:f0e9ffc5df09 257 }
yc238 15:f7e2b20f4dca 258 void Send()
yc238 15:f7e2b20f4dca 259 {
Fernon 22:2e1713475f5f 260 Pulses = Encoder.getPulses();
Fernon 31:85d3b4db5e2b 261 Rotatie = (Pulses*twopi)/4200;
Fernon 29:7653adbbb101 262 Pulses2 = Encoder2.getPulses();
Fernon 26:5b6c577fb3c1 263 Rotatie2 = (Pulses2*twopi)/4200;
Fernon 33:4e3870ab4e17 264 scope.set(0,Rotatie);
Fernon 33:4e3870ab4e17 265 scope.set(1,y8);
Fernon 33:4e3870ab4e17 266 scope.set(2,Rotatie2);
Fernon 33:4e3870ab4e17 267 scope.set(3,y18);
Fernon 16:b0ec8e6a8ad4 268 scope.send();
Fernon 16:b0ec8e6a8ad4 269 }
Fernon 22:2e1713475f5f 270
Fernon 32:c2c80a2ca83d 271 void Calibration ()
Fernon 32:c2c80a2ca83d 272 {
Fernon 32:c2c80a2ca83d 273 if (OutRange == false && OutRange2 == false) {
Fernon 38:eaf245d88d84 274 if (Button >= 0.8) {
Fernon 38:eaf245d88d84 275 wait (0.2);
Fernon 32:c2c80a2ca83d 276 Cali = true;
Fernon 32:c2c80a2ca83d 277 TijdEMGCal.start();
Fernon 32:c2c80a2ca83d 278 Excecute = false;
Fernon 38:eaf245d88d84 279 ledC = 0;
Fernon 32:c2c80a2ca83d 280 v = 0;
Fernon 32:c2c80a2ca83d 281 v2 = 0;
Fernon 32:c2c80a2ca83d 282 }
Fernon 32:c2c80a2ca83d 283 }
Fernon 32:c2c80a2ca83d 284 if (TijdEMGCal.read() >= TijdCal) {
Fernon 32:c2c80a2ca83d 285 Cali = false;
Fernon 38:eaf245d88d84 286 ledC = 1;
Fernon 32:c2c80a2ca83d 287 TijdEMGCal.stop();
Fernon 32:c2c80a2ca83d 288 TijdEMGCal.reset();
Fernon 36:5d1225d72bca 289 T1 = 0.35*Max;
Fernon 37:0dbf536a95c8 290 T2 = 0.5*Max;
Fernon 36:5d1225d72bca 291 T3 = 0.35*Max2;
Fernon 37:0dbf536a95c8 292 T4 = 0.5*Max2;
Fernon 36:5d1225d72bca 293 //pc.printf("T1 = %f\nT2 = %f\nT3 = %f\nT4 = %f\n", T1, T2, T3, T4);
Fernon 32:c2c80a2ca83d 294 wait (3);
Fernon 32:c2c80a2ca83d 295 Excecute = true;
Fernon 32:c2c80a2ca83d 296 }
Fernon 32:c2c80a2ca83d 297 }
Fernon 32:c2c80a2ca83d 298
Fernon 0:5a5f417fa1b2 299 int main()
Fernon 0:5a5f417fa1b2 300 {
Fernon 38:eaf245d88d84 301 pc.printf("Welkom\n");
Fernon 25:230bd4e1f3ef 302 upperlimit = n1*twopi;
Fernon 25:230bd4e1f3ef 303 turnlimit = n2*twopi;
Fernon 38:eaf245d88d84 304 ledF = 1;
Fernon 38:eaf245d88d84 305 ledC = 1;
Fernon 2:f0e9ffc5df09 306 pc.baud(115200);
Fernon 2:f0e9ffc5df09 307 PowerMotor.write(0);
Fernon 28:b7d01a55530f 308 PowerMotor2.write(0);
Fernon 31:85d3b4db5e2b 309 Sender.attach(Send,1/Fs);
Fernon 31:85d3b4db5e2b 310 MotorWrite.attach(MotorSet,1/Fs);
Fernon 31:85d3b4db5e2b 311 biquadTicker.attach(myController,1/Fs2);
Fernon 23:c97e206cf2a7 312 PowerServo.period_ms(20);
Fernon 0:5a5f417fa1b2 313 while (true) {
Fernon 38:eaf245d88d84 314 pc.printf("grote While Loop\n");
Fernon 32:c2c80a2ca83d 315 Calibration();
Fernon 31:85d3b4db5e2b 316 while (Excecute) {
Fernon 33:4e3870ab4e17 317 pc.printf("Rotatie = %f \nRotatie2 = %f \n", Rotatie, Rotatie2);
Fernon 25:230bd4e1f3ef 318 // Eerst wordt motor 1 aangestuurd
Fernon 25:230bd4e1f3ef 319 if (Rotatie >= upperlimit) { //Als hij buiten bereik is
Fernon 38:eaf245d88d84 320 Goal = upperlimit - margin-0.2;
Fernon 23:c97e206cf2a7 321 OutRange = true;
Fernon 22:2e1713475f5f 322 }
Fernon 25:230bd4e1f3ef 323 if (Rotatie <= downlimit) { //Als hij buiten bereik is
Fernon 38:eaf245d88d84 324 Goal = 0 + margin +0.2;
Fernon 23:c97e206cf2a7 325 OutRange = true;
Fernon 22:2e1713475f5f 326 }
Fernon 25:230bd4e1f3ef 327 if (Rotatie >= margin && Rotatie <= upperlimit - margin) { // Voor als hij voor het eerst weer binnen bereik is
Fernon 23:c97e206cf2a7 328 OutRange = false;
Fernon 24:711c7c388e57 329 }
Fernon 25:230bd4e1f3ef 330 if (Rotatie >= downlimit && Rotatie <= upperlimit && OutRange == false) { //EMG aansturing
Fernon 32:c2c80a2ca83d 331 if (y8 >= T2 ) {
Fernon 22:2e1713475f5f 332 Direction = 1;
Fernon 38:eaf245d88d84 333 v = 0.13;
Fernon 22:2e1713475f5f 334 }
Fernon 32:c2c80a2ca83d 335 if (y8 > T1 && y8 < T2) {
Fernon 22:2e1713475f5f 336 Direction = 0;
Fernon 38:eaf245d88d84 337 v = 0.07;
Fernon 24:711c7c388e57 338 }
Fernon 32:c2c80a2ca83d 339 if (y8 <= T1) {
Fernon 23:c97e206cf2a7 340 Direction = 0;
Fernon 23:c97e206cf2a7 341 v = 0;
Fernon 23:c97e206cf2a7 342 }
Fernon 23:c97e206cf2a7 343 }
Fernon 24:711c7c388e57 344
Fernon 28:b7d01a55530f 345 // Vanaf hier wordt motor 2 aangestuurd
Fernon 28:b7d01a55530f 346 if (Rotatie2 >= turnlimit) { //Als hij buiten bereik is
Fernon 38:eaf245d88d84 347 Goal2 = turnlimit-0.1;
Fernon 28:b7d01a55530f 348 OutRange2 = true;
Fernon 28:b7d01a55530f 349 }
Fernon 28:b7d01a55530f 350 if (Rotatie2 <= -turnlimit) { //Als hij buiten bereik is
Fernon 38:eaf245d88d84 351 Goal2 = -turnlimit+0.1;
Fernon 28:b7d01a55530f 352 OutRange2 = true;
Fernon 28:b7d01a55530f 353 }
Fernon 28:b7d01a55530f 354 if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit) { // Voor als hij voor het eerst weer binnen bereik is
Fernon 28:b7d01a55530f 355 OutRange2 = false;
Fernon 28:b7d01a55530f 356 }
Fernon 28:b7d01a55530f 357 if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit && OutRange2 == false) { // EMG aansturing
Fernon 32:c2c80a2ca83d 358 if (y18 >= T4 ) {
Fernon 28:b7d01a55530f 359 Direction2 = 1;
Fernon 38:eaf245d88d84 360 v2 = 0.07;
Fernon 28:b7d01a55530f 361 }
Fernon 32:c2c80a2ca83d 362 if (y18 > T3 && y18 < T4) {
Fernon 28:b7d01a55530f 363 Direction2 = 0;
Fernon 38:eaf245d88d84 364 v2 = 0.07;
Fernon 28:b7d01a55530f 365 }
Fernon 32:c2c80a2ca83d 366 if (y18 <= T3) {
Fernon 28:b7d01a55530f 367 Direction2 = 0;
Fernon 28:b7d01a55530f 368 v2 = 0;
Fernon 28:b7d01a55530f 369 }
Fernon 28:b7d01a55530f 370 }
Fernon 38:eaf245d88d84 371 if (Button2 >= 0.8) { //Afvuren van de RBG
Fernon 38:eaf245d88d84 372 wait(0.2);
Fernon 38:eaf245d88d84 373 ledF = 0;
Fernon 31:85d3b4db5e2b 374 PowerServo.write(0.27);
Fernon 31:85d3b4db5e2b 375 wait (1);
Fernon 31:85d3b4db5e2b 376 PowerServo.write(0.04);
Fernon 38:eaf245d88d84 377 ledF = 1;
Fernon 31:85d3b4db5e2b 378 Fired=Fired+1;
Fernon 31:85d3b4db5e2b 379 pc.printf("Fire = %i", Fired);
Fernon 36:5d1225d72bca 380 if (Fired == 4) {
Fernon 31:85d3b4db5e2b 381 wait (1);
Fernon 31:85d3b4db5e2b 382 Home = true;
Fernon 31:85d3b4db5e2b 383 Excecute = false;
Fernon 31:85d3b4db5e2b 384 }
Fernon 27:9cca2ad74ec0 385 }
Fernon 32:c2c80a2ca83d 386 if (Button3 == 0) {
Fernon 32:c2c80a2ca83d 387 Excecute = false;
Fernon 32:c2c80a2ca83d 388 Home = true;
Fernon 32:c2c80a2ca83d 389 }
Fernon 32:c2c80a2ca83d 390 Calibration();
Fernon 22:2e1713475f5f 391 }
Fernon 22:2e1713475f5f 392
Fernon 25:230bd4e1f3ef 393 while (Home) { //Terugkeren naar vaste positie
Fernon 30:37e778f27fce 394 pc.printf("Home\n");
Fernon 25:230bd4e1f3ef 395 OutRange = true; //Hiermee wordt het PID gedeelte van de motor control aangestuurd.
Fernon 36:5d1225d72bca 396 OutRange2 = true;
Fernon 33:4e3870ab4e17 397 Goal = margin;
Fernon 25:230bd4e1f3ef 398 Goal2 = 0;
Fernon 36:5d1225d72bca 399 if (fabs(Error)<=0.1 && fabs(Error2)<=0.1) {
Fernon 21:d0156eadcbfe 400 timer.start();
Fernon 21:d0156eadcbfe 401 } else {
Fernon 21:d0156eadcbfe 402 timer.stop();
Fernon 21:d0156eadcbfe 403 timer.reset();
Fernon 16:b0ec8e6a8ad4 404 }
Fernon 22:2e1713475f5f 405 if (timer.read() >= 3) {
Fernon 18:0f7f57228901 406 Errori = 0;
Fernon 18:0f7f57228901 407 Errord = 0;
Fernon 25:230bd4e1f3ef 408 Errorp = 0;
Fernon 25:230bd4e1f3ef 409 Errori2 = 0;
Fernon 25:230bd4e1f3ef 410 Errord2 = 0;
Fernon 25:230bd4e1f3ef 411 Errorp2 = 0;
Fernon 30:37e778f27fce 412 Fired = 0;
Fernon 38:eaf245d88d84 413 wait (5);
Fernon 32:c2c80a2ca83d 414 Excecute = true;
Fernon 37:0dbf536a95c8 415 Home = false;
Fernon 11:a9a23042fc9e 416 }
Fernon 8:a2b725b502d8 417 }
Fernon 0:5a5f417fa1b2 418 }
Fernon 22:2e1713475f5f 419 }