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@40:cac08f589732, 2015-10-29 (annotated)

Committer:
Fernon
Date:
Thu Oct 29 15:15:45 2015 +0000
Revision:
40:cac08f589732
Parent:
39:e5bf4b1293fa 
Works well

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"
Fernon 25:230bd4e1f3ef 4
Fernon 25:230bd4e1f3ef 5
Fernon 25:230bd4e1f3ef 6 // Motor 1 & 2
Fernon 40:cac08f589732 7 DigitalOut Direction(D4);
Fernon 40:cac08f589732 8 PwmOut PowerMotor(D5);
Fernon 40:cac08f589732 9 QEI Encoder(D10,D11,NC,32,QEI::X2_ENCODING);
Fernon 28:b7d01a55530f 10 DigitalOut Direction2(D7);
Fernon 28:b7d01a55530f 11 PwmOut PowerMotor2(D6);
Fernon 29:7653adbbb101 12 QEI Encoder2(D12,D13,NC,32,QEI::X2_ENCODING);
Fernon 23:c97e206cf2a7 13 PwmOut PowerServo(D3);
Fernon 25:230bd4e1f3ef 14
Fernon 32:c2c80a2ca83d 15 // Buttons & EMG
Fernon 40:cac08f589732 16 AnalogIn Button(A3); // Calibration button
Fernon 38:eaf245d88d84 17 AnalogIn Button2(A4); // Fire button
Fernon 40:cac08f589732 18 DigitalIn Button3(D9); // Home button
Fernon 36:5d1225d72bca 19 AnalogIn EMG(A0);
Fernon 36:5d1225d72bca 20 AnalogIn EMG2(A1);
Fernon 25:230bd4e1f3ef 21
Fernon 25:230bd4e1f3ef 22 // Tickers & timers
Fernon 31:85d3b4db5e2b 23
Fernon 31:85d3b4db5e2b 24 Ticker biquadTicker;
Fernon 22:2e1713475f5f 25 Ticker MotorWrite;
Fernon 22:2e1713475f5f 26 Timer timer;
Fernon 31:85d3b4db5e2b 27 Timer TijdEMGCal;
Fernon 25:230bd4e1f3ef 28
Fernon 39:e5bf4b1293fa 29 // LED
Fernon 38:eaf245d88d84 30 DigitalOut ledF(LED_RED);
Fernon 38:eaf245d88d84 31 DigitalOut ledC(LED_GREEN);
Fernon 0:5a5f417fa1b2 32
yc238 15:f7e2b20f4dca 33
Fernon 25:230bd4e1f3ef 34
Fernon 40:cac08f589732 35 // Values
Fernon 25:230bd4e1f3ef 36 const double twopi = 6.2831853071795;
Fernon 40:cac08f589732 37 const double Fs=100; // Sample Frequency
Fernon 25:230bd4e1f3ef 38 int Fired = 0;
Fernon 25:230bd4e1f3ef 39
Fernon 40:cac08f589732 40 // Motor 1 (Translation)
Fernon 40:cac08f589732 41 const double n1 = 3.3; // The amount of the rotations that is allowed
Fernon 22:2e1713475f5f 42 int Pulses;
Fernon 40:cac08f589732 43 double Rotatie = 0; //Rotation in radial
Fernon 40:cac08f589732 44 double Goal; // Setpoint for Motor
Fernon 40:cac08f589732 45 double Error = 0; // Setting initial values
Fernon 22:2e1713475f5f 46 double Errord = 0;
Fernon 22:2e1713475f5f 47 double Errori = 0;
Fernon 22:2e1713475f5f 48 double Errorp = 0;
Fernon 40:cac08f589732 49 const double Kp = 0.2; // PID controll parameters
Fernon 22:2e1713475f5f 50 const double Kd = 10;
Fernon 22:2e1713475f5f 51 const double Ki = 0.2;
Fernon 32:c2c80a2ca83d 52 double v = 0;
Fernon 40:cac08f589732 53 double upperlimit; //Maximum rounds up
Fernon 40:cac08f589732 54 const double downlimit = 0.8; //Lowest limit of the robot
Fernon 40:cac08f589732 55 const double margin = 0.8; //Margin for safe range
Fernon 40:cac08f589732 56 bool OutRange = false; //In Range or not
Fernon 25:230bd4e1f3ef 57
Fernon 40:cac08f589732 58 // Motor 2 (Rotation)
Fernon 40:cac08f589732 59 const double n2 = 0.334;
Fernon 25:230bd4e1f3ef 60 int Pulses2;
Fernon 25:230bd4e1f3ef 61 double Rotatie2 = 0;
Fernon 25:230bd4e1f3ef 62 double Goal2;
Fernon 25:230bd4e1f3ef 63 double Error2 = 0;
Fernon 25:230bd4e1f3ef 64 double Errord2 = 0;
Fernon 25:230bd4e1f3ef 65 double Errori2 = 0;
Fernon 25:230bd4e1f3ef 66 double Errorp2 = 0;
Fernon 25:230bd4e1f3ef 67 const double Kp2 = 0.2;
Fernon 25:230bd4e1f3ef 68 const double Kd2 = 10;
Fernon 25:230bd4e1f3ef 69 const double Ki2 = 0.2;
Fernon 25:230bd4e1f3ef 70 double v2 = 0;
Fernon 40:cac08f589732 71 double turnlimit = 0.4;
Fernon 40:cac08f589732 72 // Margin 2 = 0
Fernon 25:230bd4e1f3ef 73 bool OutRange2 = false;
Fernon 25:230bd4e1f3ef 74
Fernon 40:cac08f589732 75 // Switch between firing mode (Excecute) and return to Home mode
Fernon 22:2e1713475f5f 76 bool Excecute = false;
Fernon 22:2e1713475f5f 77 bool Home = false;
Fernon 25:230bd4e1f3ef 78
Fernon 31:85d3b4db5e2b 79 // Filter
Fernon 40:cac08f589732 80 double Fs2 = 500; // Frequency in Hz
Fernon 40:cac08f589732 81 const double TijdCal = 5; // Time for calibration
Fernon 40:cac08f589732 82 double Max = 0; // Max value for EMG
Fernon 40:cac08f589732 83 double Max2 = 0; // Max value for EMG2
Fernon 40:cac08f589732 84 bool Cali = false; // Switch for Calibration
Fernon 40:cac08f589732 85 double T1 = 0; // Thresholds for EMG
Fernon 40:cac08f589732 86 double T2 = 0;
Fernon 40:cac08f589732 87 double T3 = 0;
Fernon 40:cac08f589732 88 double T4 = 0;
Fernon 31:85d3b4db5e2b 89
Fernon 40:cac08f589732 90 double u1; //All variables for filtering
Fernon 31:85d3b4db5e2b 91 double y1;
Fernon 31:85d3b4db5e2b 92 double y2;
Fernon 31:85d3b4db5e2b 93 double y3;
Fernon 31:85d3b4db5e2b 94 double y4;
Fernon 31:85d3b4db5e2b 95 double y5;
Fernon 31:85d3b4db5e2b 96 double y6;
Fernon 31:85d3b4db5e2b 97 double y7;
Fernon 31:85d3b4db5e2b 98 double y8;
Fernon 36:5d1225d72bca 99 double y9;
Fernon 36:5d1225d72bca 100 double y10;
Fernon 31:85d3b4db5e2b 101
Fernon 31:85d3b4db5e2b 102 double u10;
Fernon 36:5d1225d72bca 103 double y11;
Fernon 31:85d3b4db5e2b 104 double y12;
Fernon 31:85d3b4db5e2b 105 double y13;
Fernon 31:85d3b4db5e2b 106 double y14;
Fernon 31:85d3b4db5e2b 107 double y15;
Fernon 31:85d3b4db5e2b 108 double y16;
Fernon 31:85d3b4db5e2b 109 double y17;
Fernon 31:85d3b4db5e2b 110 double y18;
Fernon 31:85d3b4db5e2b 111
Fernon 31:85d3b4db5e2b 112 double f1_v1=0,f1_v2=0;
Fernon 31:85d3b4db5e2b 113 double f2_v1=0,f2_v2=0;
Fernon 31:85d3b4db5e2b 114 double f3_v1=0,f3_v2=0;
Fernon 31:85d3b4db5e2b 115 double f4_v1=0,f4_v2=0;
Fernon 31:85d3b4db5e2b 116 double f5_v1=0,f5_v2=0;
Fernon 31:85d3b4db5e2b 117 double f6_v1=0,f6_v2=0;
Fernon 31:85d3b4db5e2b 118 double f7_v1=0,f7_v2=0;
Fernon 31:85d3b4db5e2b 119
Fernon 31:85d3b4db5e2b 120 double f1_v3=0,f1_v4=0;
Fernon 31:85d3b4db5e2b 121 double f2_v3=0,f2_v4=0;
Fernon 31:85d3b4db5e2b 122 double f3_v3=0,f3_v4=0;
Fernon 31:85d3b4db5e2b 123 double f4_v3=0,f4_v4=0;
Fernon 31:85d3b4db5e2b 124 double f5_v3=0,f5_v4=0;
Fernon 31:85d3b4db5e2b 125 double f6_v3=0,f6_v4=0;
Fernon 31:85d3b4db5e2b 126 double f7_v3=0,f7_v4=0;
Fernon 31:85d3b4db5e2b 127
Fernon 31:85d3b4db5e2b 128 // Notch
Fernon 31:85d3b4db5e2b 129 const double gainNotch3=0.969922;
Fernon 31:85d3b4db5e2b 130 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 131 const double gainNotch4=0.975183;
Fernon 31:85d3b4db5e2b 132 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 133 const double gainNotch5=0.993678;
Fernon 31:85d3b4db5e2b 134 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 135
Fernon 31:85d3b4db5e2b 136 // High pass
Fernon 36:5d1225d72bca 137 const double gainHP1=0.96860379641660377;
Fernon 36:5d1225d72bca 138 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 139 const double gainHP2=0.935820;
Fernon 36:5d1225d72bca 140 const double f2_a1=-0.939062508174924,f2_a2=0,f2_b0=1.0000000000,f2_b1=-1.0000000,f2_b2=0.0000000;
Fernon 31:85d3b4db5e2b 141
Fernon 31:85d3b4db5e2b 142 // Low pass
Fernon 31:85d3b4db5e2b 143 const double gainLP6=0.000048;
Fernon 31:85d3b4db5e2b 144 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 145 const double gainLP7=0.000048;
Fernon 31:85d3b4db5e2b 146 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 147
Fernon 10:e210675cbe71 148
Fernon 25:230bd4e1f3ef 149
Fernon 40:cac08f589732 150 // Voids for calculations in the main programme
Fernon 2:f0e9ffc5df09 151
Fernon 31:85d3b4db5e2b 152 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 153 {
Fernon 31:85d3b4db5e2b 154 double v = u-a1*v1-a2*v2;
Fernon 31:85d3b4db5e2b 155 double y=b0*v+b1*v1+b2*v2;
Fernon 31:85d3b4db5e2b 156 v2=v1;
Fernon 31:85d3b4db5e2b 157 v1=v;
Fernon 31:85d3b4db5e2b 158 return y;
Fernon 31:85d3b4db5e2b 159 }
Fernon 31:85d3b4db5e2b 160
Fernon 31:85d3b4db5e2b 161 void myController()
Fernon 31:85d3b4db5e2b 162 {
Fernon 31:85d3b4db5e2b 163 // EMG 1
Fernon 31:85d3b4db5e2b 164 u1 = EMG.read();
Fernon 31:85d3b4db5e2b 165 // Notch
Fernon 32:c2c80a2ca83d 166 y1 = biquad(u1,f3_v1,f3_v2,f3_a1,f3_a2,f3_b0*gainNotch3,f3_b1*gainNotch3,f3_b2*gainNotch3);
Fernon 32:c2c80a2ca83d 167 y2 = biquad(y1,f4_v1,f4_v2,f4_a1,f4_a2,f4_b0*gainNotch4,f4_b1*gainNotch4,f4_b2*gainNotch4);
Fernon 32:c2c80a2ca83d 168 y3 = biquad(y2,f5_v1,f5_v2,f5_a1,f5_a2,f5_b0*gainNotch5,f5_b1*gainNotch5,f5_b2*gainNotch5);
Fernon 31:85d3b4db5e2b 169
Fernon 31:85d3b4db5e2b 170 // HP
Fernon 32:c2c80a2ca83d 171 y4 = biquad(y3,f1_v1,f1_v2,f1_a1,f1_a2,f1_b0*gainHP1,f1_b1*gainHP1,f1_b2*gainHP1);
Fernon 32:c2c80a2ca83d 172 y5 = biquad(y4,f2_v1,f2_v2,f2_a1,f2_a2,f2_b0*gainHP2,f2_b1*gainHP2,f2_b2*gainHP2);
Fernon 31:85d3b4db5e2b 173
Fernon 31:85d3b4db5e2b 174 // LP
Fernon 31:85d3b4db5e2b 175 y6 = fabs(y5);
Fernon 32:c2c80a2ca83d 176 y7 = biquad(y6,f6_v1,f6_v2,f6_a1,f6_a2,gainLP6*f6_b0,gainLP6*f6_b1,gainLP6*f6_b2);
Fernon 32:c2c80a2ca83d 177 y8 = biquad(y7,f7_v1,f7_v2,f7_a1,f7_a2,gainLP7*f7_b0,gainLP7*f7_b1,gainLP7*f7_b2);
Fernon 35:b71140a46b9c 178
Fernon 35:b71140a46b9c 179
Fernon 31:85d3b4db5e2b 180 // EMG 2
Fernon 35:b71140a46b9c 181 u10 = EMG2.read();
Fernon 31:85d3b4db5e2b 182 // Notch
Fernon 36:5d1225d72bca 183 y11 = biquad(u10,f3_v3,f3_v4,f3_a1,f3_a2,f3_b0*gainNotch3,f3_b1*gainNotch3,f3_b2*gainNotch3);
Fernon 36:5d1225d72bca 184 y12 = biquad(y11,f4_v3,f4_v4,f4_a1,f4_a2,f4_b0*gainNotch4,f4_b1*gainNotch4,f4_b2*gainNotch4);
Fernon 32:c2c80a2ca83d 185 y13 = biquad(y12,f5_v3,f5_v4,f5_a1,f5_a2,f5_b0*gainNotch5,f5_b1*gainNotch5,f5_b2*gainNotch5);
Fernon 31:85d3b4db5e2b 186
Fernon 31:85d3b4db5e2b 187 // HP
Fernon 32:c2c80a2ca83d 188 y14 = biquad(y13,f1_v3,f1_v4,f1_a1,f1_a2,f1_b0*gainHP1,f1_b1*gainHP1,f1_b2*gainHP1);
Fernon 32:c2c80a2ca83d 189 y15 = biquad(y14,f2_v3,f2_v4,f2_a1,f2_a2,f2_b0*gainHP2,f2_b1*gainHP2,f2_b2*gainHP2);
Fernon 31:85d3b4db5e2b 190
Fernon 31:85d3b4db5e2b 191 // LP
Fernon 31:85d3b4db5e2b 192 y16 = fabs(y15);
Fernon 32:c2c80a2ca83d 193 y17 = biquad(y16,f6_v3,f6_v4,f6_a1,f6_a2,gainLP6*f6_b0,gainLP6*f6_b1,gainLP6*f6_b2);
Fernon 32:c2c80a2ca83d 194 y18 = biquad(y17,f7_v3,f7_v4,f7_a1,f7_a2,gainLP7*f7_b0,gainLP7*f7_b1,gainLP7*f7_b2);
Fernon 31:85d3b4db5e2b 195
Fernon 31:85d3b4db5e2b 196 if (Cali == true) {
Fernon 31:85d3b4db5e2b 197 if (y8 >= Max) {
Fernon 31:85d3b4db5e2b 198 Max = y8;
Fernon 31:85d3b4db5e2b 199 }
Fernon 31:85d3b4db5e2b 200 if (y18 >= Max2) {
Fernon 31:85d3b4db5e2b 201 Max2 = y18;
Fernon 31:85d3b4db5e2b 202 }
Fernon 31:85d3b4db5e2b 203 }
Fernon 31:85d3b4db5e2b 204 }
Fernon 31:85d3b4db5e2b 205
yc238 14:b9c925a8176a 206 void MotorSet()
Fernon 2:f0e9ffc5df09 207 {
Fernon 40:cac08f589732 208 // Calculation for position of motor
Fernon 39:e5bf4b1293fa 209 Pulses = Encoder.getPulses();
Fernon 39:e5bf4b1293fa 210 Rotatie = (Pulses*twopi)/4200;
Fernon 39:e5bf4b1293fa 211 Pulses2 = Encoder2.getPulses();
Fernon 39:e5bf4b1293fa 212 Rotatie2 = (Pulses2*twopi)/4200;
Fernon 40:cac08f589732 213 // Motor 1 (translation)
Fernon 40:cac08f589732 214 if (OutRange) { //If the Robot is OutRange or is going Home
Fernon 40:cac08f589732 215 Error = Goal-Rotatie;
Fernon 23:c97e206cf2a7 216 Errord = (Error-Errorp)/Fs;
Fernon 23:c97e206cf2a7 217 Errorp = Error;
Fernon 40:cac08f589732 218 if (fabs(Error) <= 0.5) { // Only activate the I part of PID when the Error is small
Fernon 23:c97e206cf2a7 219 Errori = Errori + Error*1/Fs;
Fernon 23:c97e206cf2a7 220 } else {
Fernon 23:c97e206cf2a7 221 Errori = 0;
Fernon 23:c97e206cf2a7 222 }
Fernon 40:cac08f589732 223 if (Error>=0) { // Determine the direction
Fernon 23:c97e206cf2a7 224 Direction=1;
Fernon 23:c97e206cf2a7 225 } else {
Fernon 23:c97e206cf2a7 226 Direction=0;
Fernon 23:c97e206cf2a7 227 }
Fernon 40:cac08f589732 228 v=Kp*Error + Kd*Errord + Ki*Errori; //Calculation of speed
Fernon 40:cac08f589732 229 if (Home == true) { //Maximum speed when OutRange
Fernon 33:4e3870ab4e17 230 if (v >0.15) {
Fernon 33:4e3870ab4e17 231 v = 0.15;
Fernon 33:4e3870ab4e17 232 }
Fernon 33:4e3870ab4e17 233 }
Fernon 22:2e1713475f5f 234 }
yc238 14:b9c925a8176a 235 PowerMotor.write(fabs(v));
Fernon 25:230bd4e1f3ef 236
Fernon 27:9cca2ad74ec0 237 // Dan motor 2 (rotatie)
Fernon 27:9cca2ad74ec0 238 if (OutRange2) {
Fernon 40:cac08f589732 239 Error2 = Goal2-Rotatie2;
Fernon 27:9cca2ad74ec0 240 Errord2 = (Error2-Errorp2)/Fs;
Fernon 27:9cca2ad74ec0 241 Errorp2 = Error2;
Fernon 27:9cca2ad74ec0 242 if (fabs(Error2) <= 0.5) {
Fernon 27:9cca2ad74ec0 243 Errori2 = Errori2 + Error2*1/Fs;
Fernon 27:9cca2ad74ec0 244 } else {
Fernon 27:9cca2ad74ec0 245 Errori2 = 0;
Fernon 27:9cca2ad74ec0 246 }
Fernon 27:9cca2ad74ec0 247 if (Error2>=0) {
Fernon 29:7653adbbb101 248 Direction2 = 0;
Fernon 27:9cca2ad74ec0 249 } else {
Fernon 29:7653adbbb101 250 Direction2 = 1;
Fernon 27:9cca2ad74ec0 251 }
Fernon 27:9cca2ad74ec0 252 v2=Kp2*Error2 + Kd2*Errord2 + Ki2*Errori2;
Fernon 27:9cca2ad74ec0 253 }
Fernon 27:9cca2ad74ec0 254 PowerMotor2.write(fabs(v2));
Fernon 2:f0e9ffc5df09 255 }
Fernon 22:2e1713475f5f 256
Fernon 32:c2c80a2ca83d 257 void Calibration ()
Fernon 32:c2c80a2ca83d 258 {
Fernon 40:cac08f589732 259 if (OutRange == false && OutRange2 == false) { //Only allow callibration during firing mode and when in Range
Fernon 38:eaf245d88d84 260 if (Button >= 0.8) {
Fernon 38:eaf245d88d84 261 wait (0.2);
Fernon 40:cac08f589732 262 Cali = true; // Calibration part in the EMG void is being activated
Fernon 32:c2c80a2ca83d 263 TijdEMGCal.start();
Fernon 32:c2c80a2ca83d 264 Excecute = false;
Fernon 38:eaf245d88d84 265 ledC = 0;
Fernon 32:c2c80a2ca83d 266 v = 0;
Fernon 32:c2c80a2ca83d 267 v2 = 0;
Fernon 32:c2c80a2ca83d 268 }
Fernon 32:c2c80a2ca83d 269 }
Fernon 32:c2c80a2ca83d 270 if (TijdEMGCal.read() >= TijdCal) {
Fernon 40:cac08f589732 271 Cali = false; // Calibration part in the EMG void is stopped
Fernon 38:eaf245d88d84 272 ledC = 1;
Fernon 32:c2c80a2ca83d 273 TijdEMGCal.stop();
Fernon 32:c2c80a2ca83d 274 TijdEMGCal.reset();
Fernon 40:cac08f589732 275 T1 = 0.35*Max; // Determination of thresholds
Fernon 37:0dbf536a95c8 276 T2 = 0.5*Max;
Fernon 36:5d1225d72bca 277 T3 = 0.35*Max2;
Fernon 37:0dbf536a95c8 278 T4 = 0.5*Max2;
Fernon 32:c2c80a2ca83d 279 wait (3);
Fernon 32:c2c80a2ca83d 280 Excecute = true;
Fernon 32:c2c80a2ca83d 281 }
Fernon 32:c2c80a2ca83d 282 }
Fernon 32:c2c80a2ca83d 283
Fernon 0:5a5f417fa1b2 284 int main()
Fernon 0:5a5f417fa1b2 285 {
Fernon 40:cac08f589732 286 upperlimit = n1*twopi; // Calculation for limits of the robot
Fernon 25:230bd4e1f3ef 287 turnlimit = n2*twopi;
Fernon 38:eaf245d88d84 288 ledF = 1;
Fernon 38:eaf245d88d84 289 ledC = 1;
Fernon 2:f0e9ffc5df09 290 PowerMotor.write(0);
Fernon 28:b7d01a55530f 291 PowerMotor2.write(0);
Fernon 40:cac08f589732 292 MotorWrite.attach(MotorSet,1/Fs); // Activate the tickers
Fernon 31:85d3b4db5e2b 293 biquadTicker.attach(myController,1/Fs2);
Fernon 23:c97e206cf2a7 294 PowerServo.period_ms(20);
Fernon 0:5a5f417fa1b2 295 while (true) {
Fernon 40:cac08f589732 296 Calibration(); // Makes it able to calibrate during firing mode
Fernon 31:85d3b4db5e2b 297 while (Excecute) {
Fernon 40:cac08f589732 298 // Motor 1
Fernon 40:cac08f589732 299 if (Rotatie >= upperlimit) { // If OutRange
Fernon 38:eaf245d88d84 300 Goal = upperlimit - margin-0.2;
Fernon 23:c97e206cf2a7 301 OutRange = true;
Fernon 22:2e1713475f5f 302 }
Fernon 40:cac08f589732 303 if (Rotatie <= downlimit) { // If OutRange
Fernon 38:eaf245d88d84 304 Goal = 0 + margin +0.2;
Fernon 23:c97e206cf2a7 305 OutRange = true;
Fernon 22:2e1713475f5f 306 }
Fernon 40:cac08f589732 307 if (Rotatie >= margin && Rotatie <= upperlimit - margin) { // If in range / not OutRange
Fernon 23:c97e206cf2a7 308 OutRange = false;
Fernon 24:711c7c388e57 309 }
Fernon 40:cac08f589732 310 if (Rotatie >= downlimit && Rotatie <= upperlimit && OutRange == false) { //EMG aansturing when in range / not OutRange
Fernon 32:c2c80a2ca83d 311 if (y8 >= T2 ) {
Fernon 22:2e1713475f5f 312 Direction = 1;
Fernon 38:eaf245d88d84 313 v = 0.13;
Fernon 22:2e1713475f5f 314 }
Fernon 32:c2c80a2ca83d 315 if (y8 > T1 && y8 < T2) {
Fernon 22:2e1713475f5f 316 Direction = 0;
Fernon 38:eaf245d88d84 317 v = 0.07;
Fernon 24:711c7c388e57 318 }
Fernon 32:c2c80a2ca83d 319 if (y8 <= T1) {
Fernon 23:c97e206cf2a7 320 Direction = 0;
Fernon 23:c97e206cf2a7 321 v = 0;
Fernon 23:c97e206cf2a7 322 }
Fernon 23:c97e206cf2a7 323 }
Fernon 24:711c7c388e57 324
Fernon 40:cac08f589732 325 // Motor
Fernon 40:cac08f589732 326 if (Rotatie2 >= turnlimit) { // If OutRange
Fernon 38:eaf245d88d84 327 Goal2 = turnlimit-0.1;
Fernon 28:b7d01a55530f 328 OutRange2 = true;
Fernon 28:b7d01a55530f 329 }
Fernon 40:cac08f589732 330 if (Rotatie2 <= -turnlimit) { // If OutRange
Fernon 38:eaf245d88d84 331 Goal2 = -turnlimit+0.1;
Fernon 28:b7d01a55530f 332 OutRange2 = true;
Fernon 28:b7d01a55530f 333 }
Fernon 40:cac08f589732 334 if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit) { // If in range / not OutRange
Fernon 28:b7d01a55530f 335 OutRange2 = false;
Fernon 28:b7d01a55530f 336 }
Fernon 40:cac08f589732 337 if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit && OutRange2 == false) { //EMG aansturing when in range / not OutRange
Fernon 32:c2c80a2ca83d 338 if (y18 >= T4 ) {
Fernon 28:b7d01a55530f 339 Direction2 = 1;
Fernon 40:cac08f589732 340 v2 = 0.06;
Fernon 28:b7d01a55530f 341 }
Fernon 32:c2c80a2ca83d 342 if (y18 > T3 && y18 < T4) {
Fernon 28:b7d01a55530f 343 Direction2 = 0;
Fernon 40:cac08f589732 344 v2 = 0.06;
Fernon 28:b7d01a55530f 345 }
Fernon 32:c2c80a2ca83d 346 if (y18 <= T3) {
Fernon 28:b7d01a55530f 347 Direction2 = 0;
Fernon 28:b7d01a55530f 348 v2 = 0;
Fernon 28:b7d01a55530f 349 }
Fernon 28:b7d01a55530f 350 }
Fernon 40:cac08f589732 351 if (Button2 >= 0.8) { // Firing Rubber Band Gun
Fernon 38:eaf245d88d84 352 wait(0.2);
Fernon 38:eaf245d88d84 353 ledF = 0;
Fernon 31:85d3b4db5e2b 354 PowerServo.write(0.27);
Fernon 31:85d3b4db5e2b 355 wait (1);
Fernon 31:85d3b4db5e2b 356 PowerServo.write(0.04);
Fernon 40:cac08f589732 357 wait (1);
Fernon 40:cac08f589732 358 PowerServo.write(0.27);
Fernon 40:cac08f589732 359 wait (1);
Fernon 40:cac08f589732 360 PowerServo.write(0.04);
Fernon 38:eaf245d88d84 361 ledF = 1;
Fernon 31:85d3b4db5e2b 362 Fired=Fired+1;
Fernon 40:cac08f589732 363 if (Fired == 3) {
Fernon 31:85d3b4db5e2b 364 wait (1);
Fernon 31:85d3b4db5e2b 365 Home = true;
Fernon 31:85d3b4db5e2b 366 Excecute = false;
Fernon 31:85d3b4db5e2b 367 }
Fernon 27:9cca2ad74ec0 368 }
Fernon 32:c2c80a2ca83d 369 if (Button3 == 0) {
Fernon 32:c2c80a2ca83d 370 Excecute = false;
Fernon 32:c2c80a2ca83d 371 Home = true;
Fernon 32:c2c80a2ca83d 372 }
Fernon 32:c2c80a2ca83d 373 Calibration();
Fernon 22:2e1713475f5f 374 }
Fernon 22:2e1713475f5f 375
Fernon 40:cac08f589732 376 while (Home) { // Return to base position
Fernon 40:cac08f589732 377 OutRange = true; // Activation of PID control part
Fernon 36:5d1225d72bca 378 OutRange2 = true;
Fernon 33:4e3870ab4e17 379 Goal = margin;
Fernon 25:230bd4e1f3ef 380 Goal2 = 0;
Fernon 40:cac08f589732 381 if (fabs(Error)<=0.1 && fabs(Error2)<=0.1) { // If error is small enough, then firing mode will be enabled again
Fernon 21:d0156eadcbfe 382 timer.start();
Fernon 21:d0156eadcbfe 383 } else {
Fernon 21:d0156eadcbfe 384 timer.stop();
Fernon 21:d0156eadcbfe 385 timer.reset();
Fernon 16:b0ec8e6a8ad4 386 }
Fernon 40:cac08f589732 387 if (timer.read() >= 3) { // Firing mode is being enabled again and values are reset.
Fernon 18:0f7f57228901 388 Errori = 0;
Fernon 18:0f7f57228901 389 Errord = 0;
Fernon 25:230bd4e1f3ef 390 Errorp = 0;
Fernon 25:230bd4e1f3ef 391 Errori2 = 0;
Fernon 25:230bd4e1f3ef 392 Errord2 = 0;
Fernon 25:230bd4e1f3ef 393 Errorp2 = 0;
Fernon 30:37e778f27fce 394 Fired = 0;
Fernon 38:eaf245d88d84 395 wait (5);
Fernon 32:c2c80a2ca83d 396 Excecute = true;
Fernon 37:0dbf536a95c8 397 Home = false;
Fernon 11:a9a23042fc9e 398 }
Fernon 8:a2b725b502d8 399 }
Fernon 0:5a5f417fa1b2 400 }
Fernon 22:2e1713475f5f 401 }