Fernon Eijkhoudt
/
ROBOBIRDS_FINAL
EMG driven robot which shoots elastic bands
Fork of RoboBirdV1 by
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?
User | Revision | Line number | New 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 | } |