EMG driven robot which shoots elastic bands
Fork of ROBOBIRDS_FINAL by
main.cpp@35:b71140a46b9c, 2015-10-27 (annotated)
- Committer:
- Fernon
- Date:
- Tue Oct 27 16:38:47 2015 +0000
- Revision:
- 35:b71140a46b9c
- Parent:
- 34:67f211c08e74
- Child:
- 36:5d1225d72bca
Met de Potmeter lijkt het weer te werken
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 | 34:67f211c08e74 | 21 | //AnalogIn EMG(A0); |
Fernon | 34:67f211c08e74 | 22 | //AnalogIn EMG2(A1); |
Fernon | 34:67f211c08e74 | 23 | AnalogIn EMG(A2); |
Fernon | 34:67f211c08e74 | 24 | AnalogIn EMG2(A3); |
Fernon | 25:230bd4e1f3ef | 25 | |
Fernon | 25:230bd4e1f3ef | 26 | // Tickers & timers |
Fernon | 31:85d3b4db5e2b | 27 | |
Fernon | 31:85d3b4db5e2b | 28 | Ticker biquadTicker; |
Fernon | 22:2e1713475f5f | 29 | Ticker MotorWrite; |
Fernon | 22:2e1713475f5f | 30 | Ticker Sender; |
Fernon | 22:2e1713475f5f | 31 | Timer timer; |
Fernon | 31:85d3b4db5e2b | 32 | Timer TijdEMGCal; |
Fernon | 25:230bd4e1f3ef | 33 | |
Fernon | 25:230bd4e1f3ef | 34 | // Debugging |
Fernon | 25:230bd4e1f3ef | 35 | Serial pc(USBTX, USBRX); |
Fernon | 29:7653adbbb101 | 36 | HIDScope scope(6); |
Fernon | 32:c2c80a2ca83d | 37 | DigitalOut led(LED_RED); |
Fernon | 0:5a5f417fa1b2 | 38 | |
yc238 | 15:f7e2b20f4dca | 39 | |
Fernon | 25:230bd4e1f3ef | 40 | |
Fernon | 25:230bd4e1f3ef | 41 | // Waardes |
Fernon | 25:230bd4e1f3ef | 42 | const double twopi = 6.2831853071795; |
Fernon | 25:230bd4e1f3ef | 43 | const double Fs=100; |
Fernon | 25:230bd4e1f3ef | 44 | int Fired = 0; |
Fernon | 25:230bd4e1f3ef | 45 | |
Fernon | 25:230bd4e1f3ef | 46 | // EMG |
Fernon | 32:c2c80a2ca83d | 47 | double T1 = 0; |
Fernon | 32:c2c80a2ca83d | 48 | double T2 = 0; |
Fernon | 32:c2c80a2ca83d | 49 | double T3 = 0; |
Fernon | 32:c2c80a2ca83d | 50 | double T4 = 0; |
Fernon | 25:230bd4e1f3ef | 51 | |
Fernon | 25:230bd4e1f3ef | 52 | // Motor 1 (Translatie) |
Fernon | 34:67f211c08e74 | 53 | const double n1 = 3.861464193; // Aantal rondjes dat ons apparaat maximaal mag draaien (omhoog) |
Fernon | 22:2e1713475f5f | 54 | int Pulses; |
Fernon | 22:2e1713475f5f | 55 | double Rotatie = 0; //aantal graden dat de motor is gedraaid |
Fernon | 25:230bd4e1f3ef | 56 | double Goal; //initele waarde goal waar de motor naar toe moet, dit wordt gedaan |
Fernon | 22:2e1713475f5f | 57 | double Error = 0; |
Fernon | 22:2e1713475f5f | 58 | double Errord = 0; |
Fernon | 22:2e1713475f5f | 59 | double Errori = 0; |
Fernon | 22:2e1713475f5f | 60 | double Errorp = 0; |
Fernon | 22:2e1713475f5f | 61 | const double Kp = 0.2; //Moet berekend worden aan de hand van Control concept slides |
Fernon | 22:2e1713475f5f | 62 | const double Kd = 10; |
Fernon | 22:2e1713475f5f | 63 | const double Ki = 0.2; |
Fernon | 32:c2c80a2ca83d | 64 | double v = 0; |
Fernon | 25:230bd4e1f3ef | 65 | double upperlimit; //max aantal rotaties omhoog |
Fernon | 33:4e3870ab4e17 | 66 | const double downlimit = 0.8; |
Fernon | 33:4e3870ab4e17 | 67 | const double margin = 0.8; |
Fernon | 25:230bd4e1f3ef | 68 | bool OutRange = false; |
Fernon | 25:230bd4e1f3ef | 69 | |
Fernon | 25:230bd4e1f3ef | 70 | // Motor 2 (Rotatie) |
Fernon | 34:67f211c08e74 | 71 | const double n2 = 0.3125; // Aantal rondjes dat dat ons apparaat maximaal mag draaien (rotatie) |
Fernon | 25:230bd4e1f3ef | 72 | int Pulses2; |
Fernon | 25:230bd4e1f3ef | 73 | double Rotatie2 = 0; |
Fernon | 25:230bd4e1f3ef | 74 | double Goal2; |
Fernon | 25:230bd4e1f3ef | 75 | double Error2 = 0; |
Fernon | 25:230bd4e1f3ef | 76 | double Errord2 = 0; |
Fernon | 25:230bd4e1f3ef | 77 | double Errori2 = 0; |
Fernon | 25:230bd4e1f3ef | 78 | double Errorp2 = 0; |
Fernon | 25:230bd4e1f3ef | 79 | const double Kp2 = 0.2; |
Fernon | 25:230bd4e1f3ef | 80 | const double Kd2 = 10; |
Fernon | 25:230bd4e1f3ef | 81 | const double Ki2 = 0.2; |
Fernon | 25:230bd4e1f3ef | 82 | double v2 = 0; |
Fernon | 28:b7d01a55530f | 83 | double turnlimit = 0.4; // max aantal rotaties voor roteren hele robot |
Fernon | 25:230bd4e1f3ef | 84 | // Margin 2 is in ons geval 0 |
Fernon | 25:230bd4e1f3ef | 85 | bool OutRange2 = false; |
Fernon | 25:230bd4e1f3ef | 86 | |
Fernon | 25:230bd4e1f3ef | 87 | // Activatie tussen het schietgedeelte en terugkeergedeelte |
Fernon | 22:2e1713475f5f | 88 | bool Excecute = false; |
Fernon | 22:2e1713475f5f | 89 | bool Home = false; |
Fernon | 25:230bd4e1f3ef | 90 | |
Fernon | 31:85d3b4db5e2b | 91 | // Filter |
Fernon | 33:4e3870ab4e17 | 92 | double Fs2 = 500; // in Hz |
Fernon | 31:85d3b4db5e2b | 93 | const double TijdCal = 5; |
Fernon | 31:85d3b4db5e2b | 94 | double Max = 0; |
Fernon | 31:85d3b4db5e2b | 95 | double Max2 = 0; |
Fernon | 31:85d3b4db5e2b | 96 | bool Cali = false; |
Fernon | 31:85d3b4db5e2b | 97 | |
Fernon | 31:85d3b4db5e2b | 98 | double u1; |
Fernon | 31:85d3b4db5e2b | 99 | double y1; |
Fernon | 31:85d3b4db5e2b | 100 | double y2; |
Fernon | 31:85d3b4db5e2b | 101 | double y3; |
Fernon | 31:85d3b4db5e2b | 102 | double y4; |
Fernon | 31:85d3b4db5e2b | 103 | double y5; |
Fernon | 31:85d3b4db5e2b | 104 | double y6; |
Fernon | 31:85d3b4db5e2b | 105 | double y7; |
Fernon | 31:85d3b4db5e2b | 106 | double y8; |
Fernon | 31:85d3b4db5e2b | 107 | |
Fernon | 31:85d3b4db5e2b | 108 | double u10; |
Fernon | 31:85d3b4db5e2b | 109 | double y10; |
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 | 31:85d3b4db5e2b | 143 | const double gainHP1=0.939472; |
Fernon | 31:85d3b4db5e2b | 144 | const double f1_a1=-0.87894202296,f1_a2=0.00000000,f1_b0=1.0000000,f1_b1=-1.00000000,f1_b2=0.000000000; |
Fernon | 31:85d3b4db5e2b | 145 | const double gainHP2=0.935820; |
Fernon | 31:85d3b4db5e2b | 146 | const double f2_a1=-1.86387364983,f2_a2=0.87941229211,f2_b0=1.0000000000,f2_b1=-2.0000000,f2_b2=1.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 | 32:c2c80a2ca83d | 189 | y10 = biquad(u10,f3_v3,f3_v4,f3_a1,f3_a2,f3_b0*gainNotch3,f3_b1*gainNotch3,f3_b2*gainNotch3); |
Fernon | 32:c2c80a2ca83d | 190 | y12 = biquad(y10,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 | 35:b71140a46b9c | 201 | y18 = EMG.read(); |
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 | } |
Fernon | 35:b71140a46b9c | 237 | v = 0; |
yc238 | 14:b9c925a8176a | 238 | PowerMotor.write(fabs(v)); |
Fernon | 25:230bd4e1f3ef | 239 | |
Fernon | 27:9cca2ad74ec0 | 240 | // Dan motor 2 (rotatie) |
Fernon | 27:9cca2ad74ec0 | 241 | if (OutRange2) { |
Fernon | 27:9cca2ad74ec0 | 242 | Error2 = Goal2-Rotatie2; // De error die het motortje maakt ten opzichte van je Goal |
Fernon | 27:9cca2ad74ec0 | 243 | Errord2 = (Error2-Errorp2)/Fs; |
Fernon | 27:9cca2ad74ec0 | 244 | Errorp2 = Error2; |
Fernon | 27:9cca2ad74ec0 | 245 | if (fabs(Error2) <= 0.5) { |
Fernon | 27:9cca2ad74ec0 | 246 | Errori2 = Errori2 + Error2*1/Fs; |
Fernon | 27:9cca2ad74ec0 | 247 | } else { |
Fernon | 27:9cca2ad74ec0 | 248 | Errori2 = 0; |
Fernon | 27:9cca2ad74ec0 | 249 | } |
Fernon | 27:9cca2ad74ec0 | 250 | if (Error2>=0) { |
Fernon | 29:7653adbbb101 | 251 | Direction2 = 0; |
Fernon | 27:9cca2ad74ec0 | 252 | } else { |
Fernon | 29:7653adbbb101 | 253 | Direction2 = 1; |
Fernon | 27:9cca2ad74ec0 | 254 | } |
Fernon | 27:9cca2ad74ec0 | 255 | v2=Kp2*Error2 + Kd2*Errord2 + Ki2*Errori2; |
Fernon | 27:9cca2ad74ec0 | 256 | } |
Fernon | 27:9cca2ad74ec0 | 257 | PowerMotor2.write(fabs(v2)); |
Fernon | 2:f0e9ffc5df09 | 258 | } |
yc238 | 15:f7e2b20f4dca | 259 | void Send() |
yc238 | 15:f7e2b20f4dca | 260 | { |
Fernon | 22:2e1713475f5f | 261 | Pulses = Encoder.getPulses(); |
Fernon | 31:85d3b4db5e2b | 262 | Rotatie = (Pulses*twopi)/4200; |
Fernon | 29:7653adbbb101 | 263 | Pulses2 = Encoder2.getPulses(); |
Fernon | 26:5b6c577fb3c1 | 264 | Rotatie2 = (Pulses2*twopi)/4200; |
Fernon | 33:4e3870ab4e17 | 265 | scope.set(0,Rotatie); |
Fernon | 33:4e3870ab4e17 | 266 | scope.set(1,y8); |
Fernon | 33:4e3870ab4e17 | 267 | scope.set(2,Rotatie2); |
Fernon | 33:4e3870ab4e17 | 268 | scope.set(3,y18); |
Fernon | 16:b0ec8e6a8ad4 | 269 | scope.send(); |
Fernon | 16:b0ec8e6a8ad4 | 270 | } |
Fernon | 22:2e1713475f5f | 271 | |
Fernon | 32:c2c80a2ca83d | 272 | void Calibration () |
Fernon | 32:c2c80a2ca83d | 273 | { |
Fernon | 32:c2c80a2ca83d | 274 | if (OutRange == false && OutRange2 == false) { |
Fernon | 32:c2c80a2ca83d | 275 | if (Button == 0) { |
Fernon | 32:c2c80a2ca83d | 276 | Cali = true; |
Fernon | 32:c2c80a2ca83d | 277 | TijdEMGCal.start(); |
Fernon | 32:c2c80a2ca83d | 278 | Excecute = false; |
Fernon | 32:c2c80a2ca83d | 279 | led = 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 | 32:c2c80a2ca83d | 286 | led = 1; |
Fernon | 32:c2c80a2ca83d | 287 | TijdEMGCal.stop(); |
Fernon | 32:c2c80a2ca83d | 288 | TijdEMGCal.reset(); |
Fernon | 33:4e3870ab4e17 | 289 | T1 = 0.2*Max; |
Fernon | 33:4e3870ab4e17 | 290 | T2 = 0.4*Max; |
Fernon | 33:4e3870ab4e17 | 291 | T3 = 0.2*Max2; |
Fernon | 33:4e3870ab4e17 | 292 | T4 = 0.4*Max2; |
Fernon | 33:4e3870ab4e17 | 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 | 25:230bd4e1f3ef | 301 | upperlimit = n1*twopi; |
Fernon | 25:230bd4e1f3ef | 302 | turnlimit = n2*twopi; |
Fernon | 2:f0e9ffc5df09 | 303 | pc.baud(115200); |
Fernon | 2:f0e9ffc5df09 | 304 | PowerMotor.write(0); |
Fernon | 28:b7d01a55530f | 305 | PowerMotor2.write(0); |
Fernon | 31:85d3b4db5e2b | 306 | Sender.attach(Send,1/Fs); |
Fernon | 31:85d3b4db5e2b | 307 | MotorWrite.attach(MotorSet,1/Fs); |
Fernon | 31:85d3b4db5e2b | 308 | biquadTicker.attach(myController,1/Fs2); |
Fernon | 23:c97e206cf2a7 | 309 | PowerServo.period_ms(20); |
Fernon | 32:c2c80a2ca83d | 310 | led = 1; |
Fernon | 0:5a5f417fa1b2 | 311 | while (true) { |
Fernon | 32:c2c80a2ca83d | 312 | Calibration(); |
Fernon | 31:85d3b4db5e2b | 313 | while (Excecute) { |
Fernon | 33:4e3870ab4e17 | 314 | pc.printf("Rotatie = %f \nRotatie2 = %f \n", Rotatie, Rotatie2); |
Fernon | 25:230bd4e1f3ef | 315 | // Eerst wordt motor 1 aangestuurd |
Fernon | 25:230bd4e1f3ef | 316 | if (Rotatie >= upperlimit) { //Als hij buiten bereik is |
Fernon | 27:9cca2ad74ec0 | 317 | Goal = upperlimit - margin; |
Fernon | 23:c97e206cf2a7 | 318 | OutRange = true; |
Fernon | 22:2e1713475f5f | 319 | } |
Fernon | 25:230bd4e1f3ef | 320 | if (Rotatie <= downlimit) { //Als hij buiten bereik is |
Fernon | 27:9cca2ad74ec0 | 321 | Goal = 0 + margin; |
Fernon | 23:c97e206cf2a7 | 322 | OutRange = true; |
Fernon | 22:2e1713475f5f | 323 | } |
Fernon | 25:230bd4e1f3ef | 324 | if (Rotatie >= margin && Rotatie <= upperlimit - margin) { // Voor als hij voor het eerst weer binnen bereik is |
Fernon | 23:c97e206cf2a7 | 325 | OutRange = false; |
Fernon | 24:711c7c388e57 | 326 | } |
Fernon | 25:230bd4e1f3ef | 327 | if (Rotatie >= downlimit && Rotatie <= upperlimit && OutRange == false) { //EMG aansturing |
Fernon | 32:c2c80a2ca83d | 328 | if (y8 >= T2 ) { |
Fernon | 22:2e1713475f5f | 329 | Direction = 1; |
Fernon | 33:4e3870ab4e17 | 330 | v = 0.15; |
Fernon | 22:2e1713475f5f | 331 | } |
Fernon | 32:c2c80a2ca83d | 332 | if (y8 > T1 && y8 < T2) { |
Fernon | 22:2e1713475f5f | 333 | Direction = 0; |
Fernon | 33:4e3870ab4e17 | 334 | v = 0.15; |
Fernon | 24:711c7c388e57 | 335 | } |
Fernon | 32:c2c80a2ca83d | 336 | if (y8 <= T1) { |
Fernon | 23:c97e206cf2a7 | 337 | Direction = 0; |
Fernon | 23:c97e206cf2a7 | 338 | v = 0; |
Fernon | 23:c97e206cf2a7 | 339 | } |
Fernon | 23:c97e206cf2a7 | 340 | } |
Fernon | 24:711c7c388e57 | 341 | |
Fernon | 28:b7d01a55530f | 342 | // Vanaf hier wordt motor 2 aangestuurd |
Fernon | 28:b7d01a55530f | 343 | if (Rotatie2 >= turnlimit) { //Als hij buiten bereik is |
Fernon | 28:b7d01a55530f | 344 | Goal2 = turnlimit; |
Fernon | 28:b7d01a55530f | 345 | OutRange2 = true; |
Fernon | 28:b7d01a55530f | 346 | } |
Fernon | 28:b7d01a55530f | 347 | if (Rotatie2 <= -turnlimit) { //Als hij buiten bereik is |
Fernon | 28:b7d01a55530f | 348 | Goal2 = -turnlimit; |
Fernon | 28:b7d01a55530f | 349 | OutRange2 = true; |
Fernon | 28:b7d01a55530f | 350 | } |
Fernon | 28:b7d01a55530f | 351 | if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit) { // Voor als hij voor het eerst weer binnen bereik is |
Fernon | 28:b7d01a55530f | 352 | OutRange2 = false; |
Fernon | 28:b7d01a55530f | 353 | } |
Fernon | 28:b7d01a55530f | 354 | if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit && OutRange2 == false) { // EMG aansturing |
Fernon | 32:c2c80a2ca83d | 355 | if (y18 >= T4 ) { |
Fernon | 28:b7d01a55530f | 356 | Direction2 = 1; |
Fernon | 35:b71140a46b9c | 357 | v2 = 0.08; |
Fernon | 28:b7d01a55530f | 358 | } |
Fernon | 32:c2c80a2ca83d | 359 | if (y18 > T3 && y18 < T4) { |
Fernon | 28:b7d01a55530f | 360 | Direction2 = 0; |
Fernon | 35:b71140a46b9c | 361 | v2 = 0.08; |
Fernon | 28:b7d01a55530f | 362 | } |
Fernon | 32:c2c80a2ca83d | 363 | if (y18 <= T3) { |
Fernon | 28:b7d01a55530f | 364 | Direction2 = 0; |
Fernon | 28:b7d01a55530f | 365 | v2 = 0; |
Fernon | 28:b7d01a55530f | 366 | } |
Fernon | 28:b7d01a55530f | 367 | } |
Fernon | 27:9cca2ad74ec0 | 368 | if (Button2 == 0) { //Afvuren van de RBG |
Fernon | 31:85d3b4db5e2b | 369 | PowerServo.write(0.27); |
Fernon | 31:85d3b4db5e2b | 370 | wait (1); |
Fernon | 31:85d3b4db5e2b | 371 | PowerServo.write(0.04); |
Fernon | 31:85d3b4db5e2b | 372 | Fired=Fired+1; |
Fernon | 31:85d3b4db5e2b | 373 | pc.printf("Fire = %i", Fired); |
Fernon | 31:85d3b4db5e2b | 374 | if (Fired == 3) { |
Fernon | 31:85d3b4db5e2b | 375 | wait (1); |
Fernon | 31:85d3b4db5e2b | 376 | Home = true; |
Fernon | 31:85d3b4db5e2b | 377 | Excecute = false; |
Fernon | 31:85d3b4db5e2b | 378 | } |
Fernon | 27:9cca2ad74ec0 | 379 | } |
Fernon | 32:c2c80a2ca83d | 380 | if (Button3 == 0) { |
Fernon | 32:c2c80a2ca83d | 381 | Excecute = false; |
Fernon | 32:c2c80a2ca83d | 382 | Home = true; |
Fernon | 32:c2c80a2ca83d | 383 | } |
Fernon | 32:c2c80a2ca83d | 384 | Calibration(); |
Fernon | 22:2e1713475f5f | 385 | } |
Fernon | 22:2e1713475f5f | 386 | |
Fernon | 25:230bd4e1f3ef | 387 | while (Home) { //Terugkeren naar vaste positie |
Fernon | 30:37e778f27fce | 388 | pc.printf("Home\n"); |
Fernon | 25:230bd4e1f3ef | 389 | OutRange = true; //Hiermee wordt het PID gedeelte van de motor control aangestuurd. |
Fernon | 33:4e3870ab4e17 | 390 | Goal = margin; |
Fernon | 25:230bd4e1f3ef | 391 | Goal2 = 0; |
Fernon | 30:37e778f27fce | 392 | if (fabs(Error)<=0.015 && fabs(Error2)<=0.015) { |
Fernon | 21:d0156eadcbfe | 393 | timer.start(); |
Fernon | 21:d0156eadcbfe | 394 | } else { |
Fernon | 21:d0156eadcbfe | 395 | timer.stop(); |
Fernon | 21:d0156eadcbfe | 396 | timer.reset(); |
Fernon | 16:b0ec8e6a8ad4 | 397 | } |
Fernon | 22:2e1713475f5f | 398 | if (timer.read() >= 3) { |
Fernon | 22:2e1713475f5f | 399 | Home = false; |
Fernon | 18:0f7f57228901 | 400 | Errori = 0; |
Fernon | 18:0f7f57228901 | 401 | Errord = 0; |
Fernon | 25:230bd4e1f3ef | 402 | Errorp = 0; |
Fernon | 25:230bd4e1f3ef | 403 | Errori2 = 0; |
Fernon | 25:230bd4e1f3ef | 404 | Errord2 = 0; |
Fernon | 25:230bd4e1f3ef | 405 | Errorp2 = 0; |
Fernon | 30:37e778f27fce | 406 | Fired = 0; |
Fernon | 32:c2c80a2ca83d | 407 | wait (3); |
Fernon | 32:c2c80a2ca83d | 408 | Encoder.reset(); |
Fernon | 32:c2c80a2ca83d | 409 | Encoder2.reset(); |
Fernon | 32:c2c80a2ca83d | 410 | Excecute = true; |
Fernon | 11:a9a23042fc9e | 411 | } |
Fernon | 8:a2b725b502d8 | 412 | } |
Fernon | 0:5a5f417fa1b2 | 413 | } |
Fernon | 22:2e1713475f5f | 414 | } |