EMG driven robot which shoots elastic bands
Fork of ROBOBIRDS_FINAL by
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?
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" |
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 | } |