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@37:0dbf536a95c8, 2015-10-28 (annotated)

Committer:
Fernon
Date:
Wed Oct 28 14:22:23 2015 +0000
Revision:
37:0dbf536a95c8
Parent:
36:5d1225d72bca
Child:
38:eaf245d88d84
Translatie werkt nu ook, nu tegelijk testen. Home is ook aangepast en de encoder.reset eruit gehaald! De thresholds zijn naar elsbeths waarde afgesteld!

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