Facking kut filter werkt EINDELIJK !311!!111!!1!!!!!!!!!!!!!!!!!!!
Dependencies: HIDScope MODSERIAL Matrix QEI biquadFilter mbed
Fork of Turning_Motor_V6 by
main.cpp@9:355babe55589, 2018-10-31 (annotated)
- Committer:
- Brighton_the_robot
- Date:
- Wed Oct 31 19:53:52 2018 +0000
- Revision:
- 9:355babe55589
- Parent:
- 8:b6b09226a421
Inclusief filter
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
JurrienBos | 0:4591ba678a39 | 1 | #include "mbed.h" |
JurrienBos | 0:4591ba678a39 | 2 | #include "MODSERIAL.h" |
ThomBMT | 2:dc9766657afb | 3 | #include "HIDScope.h" |
ThomBMT | 2:dc9766657afb | 4 | #include "QEI.h" |
JurrienBos | 7:c5c648898881 | 5 | #include "BiQuad.h" |
ThomBMT | 2:dc9766657afb | 6 | |
JurrienBos | 0:4591ba678a39 | 7 | MODSERIAL pc(USBTX, USBRX); |
JurrienBos | 0:4591ba678a39 | 8 | DigitalOut DirectionPin1(D4); |
JurrienBos | 1:4bf64d003f3a | 9 | DigitalOut DirectionPin2(D7); |
JurrienBos | 0:4591ba678a39 | 10 | PwmOut PwmPin1(D5); |
JurrienBos | 1:4bf64d003f3a | 11 | PwmOut PwmPin2(D6); |
JurrienBos | 0:4591ba678a39 | 12 | DigitalIn Knop1(D2); |
ThomBMT | 4:8f67b8327300 | 13 | DigitalIn Knop2(D3); |
JurrienBos | 0:4591ba678a39 | 14 | AnalogIn pot1 (A5); |
JurrienBos | 1:4bf64d003f3a | 15 | AnalogIn pot2 (A4); |
ThomBMT | 2:dc9766657afb | 16 | AnalogIn emg0( A0 ); |
ThomBMT | 2:dc9766657afb | 17 | AnalogIn emg1( A1 ); |
ThomBMT | 2:dc9766657afb | 18 | AnalogIn emg2( A2 ); |
ThomBMT | 2:dc9766657afb | 19 | AnalogIn emg3( A3 ); |
JurrienBos | 0:4591ba678a39 | 20 | |
JurrienBos | 7:c5c648898881 | 21 | QEI Encoder1(D12,D13,NC,64,QEI::X4_ENCODING); |
ThomBMT | 5:312186a0604d | 22 | QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING); |
ThomBMT | 5:312186a0604d | 23 | |
ThomBMT | 5:312186a0604d | 24 | //DigitalOut LED(LED_RED); |
ThomBMT | 5:312186a0604d | 25 | |
ThomBMT | 4:8f67b8327300 | 26 | Ticker StateTicker; |
ThomBMT | 2:dc9766657afb | 27 | Ticker printTicker; |
ThomBMT | 5:312186a0604d | 28 | |
JurrienBos | 7:c5c648898881 | 29 | HIDScope scope(4); |
JurrienBos | 7:c5c648898881 | 30 | |
JurrienBos | 7:c5c648898881 | 31 | BiQuadChain bqc1; |
JurrienBos | 7:c5c648898881 | 32 | BiQuadChain bqc2; |
JurrienBos | 7:c5c648898881 | 33 | BiQuadChain bqc3; |
JurrienBos | 7:c5c648898881 | 34 | BiQuadChain bqc4; |
JurrienBos | 7:c5c648898881 | 35 | BiQuadChain bqc5; |
JurrienBos | 7:c5c648898881 | 36 | BiQuadChain bqc6; |
JurrienBos | 7:c5c648898881 | 37 | BiQuadChain bqc7; |
JurrienBos | 7:c5c648898881 | 38 | BiQuadChain bqc8; |
Brighton_the_robot | 8:b6b09226a421 | 39 | BiQuad BqNotch1_1( 9.65081e-01, -1.56203e+00, 9.65081e-01,-1.56858e+00, 9.64241e-01 ); |
Brighton_the_robot | 8:b6b09226a421 | 40 | BiQuad BqNotch2_1( 1.00000e+00, -1.61855e+00, 1.00000e+00 ,-1.61100e+00, 9.65922e-01); |
Brighton_the_robot | 8:b6b09226a421 | 41 | BiQuad BqNotch1_2( 9.65081e-01, -1.56203e+00, 9.65081e-01,-1.56858e+00, 9.64241e-01 ); |
Brighton_the_robot | 8:b6b09226a421 | 42 | BiQuad BqNotch2_2( 1.00000e+00, -1.61855e+00, 1.00000e+00 ,-1.61100e+00, 9.65922e-01); |
Brighton_the_robot | 8:b6b09226a421 | 43 | BiQuad BqNotch1_3( 9.65081e-01, -1.56203e+00, 9.65081e-01,-1.56858e+00, 9.64241e-01 ); |
Brighton_the_robot | 8:b6b09226a421 | 44 | BiQuad BqNotch2_3( 1.00000e+00, -1.61855e+00, 1.00000e+00 ,-1.61100e+00, 9.65922e-01); |
Brighton_the_robot | 8:b6b09226a421 | 45 | BiQuad BqNotch1_4( 9.65081e-01, -1.56203e+00, 9.65081e-01,-1.56858e+00, 9.64241e-01 ); |
Brighton_the_robot | 8:b6b09226a421 | 46 | BiQuad BqNotch2_4( 1.00000e+00, -1.61855e+00, 1.00000e+00 ,-1.61100e+00, 9.65922e-01); |
Brighton_the_robot | 8:b6b09226a421 | 47 | BiQuad BqHP1( 9.86760e-01, -1.97352e+00, 9.86760e-01, -1.97334e+00, 9.73695e-01 ); |
Brighton_the_robot | 8:b6b09226a421 | 48 | BiQuad BqHP2( 9.86760e-01, -1.97352e+00, 9.86760e-01, -1.97334e+00, 9.73695e-01 ); |
Brighton_the_robot | 8:b6b09226a421 | 49 | BiQuad BqHP3( 9.86760e-01, -1.97352e+00, 9.86760e-01, -1.97334e+00, 9.73695e-01 ); |
Brighton_the_robot | 8:b6b09226a421 | 50 | BiQuad BqHP4( 9.86760e-01, -1.97352e+00, 9.86760e-01, -1.97334e+00, 9.73695e-01 ); |
Brighton_the_robot | 9:355babe55589 | 51 | |
Brighton_the_robot | 9:355babe55589 | 52 | BiQuad BqLP1( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 ); |
Brighton_the_robot | 9:355babe55589 | 53 | BiQuad BqLP2( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 ); |
Brighton_the_robot | 9:355babe55589 | 54 | BiQuad BqLP3( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 ); |
Brighton_the_robot | 9:355babe55589 | 55 | BiQuad BqLP4( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 ); |
JurrienBos | 0:4591ba678a39 | 56 | |
ThomBMT | 3:c8f0fc045505 | 57 | volatile float Bicep_Right = 0.0; |
ThomBMT | 3:c8f0fc045505 | 58 | volatile float Bicep_Left = 0.0; |
ThomBMT | 3:c8f0fc045505 | 59 | volatile float Tricep_Right = 0.0; |
ThomBMT | 3:c8f0fc045505 | 60 | volatile float Tricep_Left = 0.0; |
ThomBMT | 3:c8f0fc045505 | 61 | volatile const float maxVelocity = 8.4; // in rad/s |
ThomBMT | 4:8f67b8327300 | 62 | volatile const double pi = 3.14159265358979; |
ThomBMT | 3:c8f0fc045505 | 63 | volatile float referenceVelocity1 = 0.5; //dit is de gecentreerde waarde en dus de nulstand |
ThomBMT | 3:c8f0fc045505 | 64 | volatile float referenceVelocity2 = 0.5; |
ThomBMT | 2:dc9766657afb | 65 | |
ThomBMT | 4:8f67b8327300 | 66 | enum states{Calibration, Homing, Function}; |
ThomBMT | 4:8f67b8327300 | 67 | |
ThomBMT | 4:8f67b8327300 | 68 | volatile states Active_State = Calibration; |
ThomBMT | 4:8f67b8327300 | 69 | |
ThomBMT | 5:312186a0604d | 70 | volatile int counts1 ; |
ThomBMT | 5:312186a0604d | 71 | volatile int counts2 ; |
ThomBMT | 5:312186a0604d | 72 | volatile float rad_m1; |
ThomBMT | 5:312186a0604d | 73 | volatile float rad_m2; |
JurrienBos | 7:c5c648898881 | 74 | volatile float q_1; |
JurrienBos | 7:c5c648898881 | 75 | volatile float q_2; |
JurrienBos | 7:c5c648898881 | 76 | volatile float r_1; |
JurrienBos | 7:c5c648898881 | 77 | volatile float r_2; |
JurrienBos | 7:c5c648898881 | 78 | volatile const float r_3 = 2.0; // |
JurrienBos | 7:c5c648898881 | 79 | |
JurrienBos | 7:c5c648898881 | 80 | volatile float FilterHP_Bi_R; |
JurrienBos | 7:c5c648898881 | 81 | volatile float Filterabs_Bi_R; |
JurrienBos | 7:c5c648898881 | 82 | volatile float Filtered_Bi_R; |
JurrienBos | 7:c5c648898881 | 83 | volatile float FilterHP_Bi_L; |
JurrienBos | 7:c5c648898881 | 84 | volatile float Filterabs_Bi_L; |
JurrienBos | 7:c5c648898881 | 85 | volatile float Filtered_Bi_L; |
JurrienBos | 7:c5c648898881 | 86 | volatile float FilterHP_Tri_R; |
JurrienBos | 7:c5c648898881 | 87 | volatile float Filterabs_Tri_R; |
JurrienBos | 7:c5c648898881 | 88 | volatile float Filtered_Tri_R; |
JurrienBos | 7:c5c648898881 | 89 | volatile float FilterHP_Tri_L; |
JurrienBos | 7:c5c648898881 | 90 | volatile float Filterabs_Tri_L; |
JurrienBos | 7:c5c648898881 | 91 | volatile float Filtered_Tri_L; |
ThomBMT | 4:8f67b8327300 | 92 | |
JurrienBos | 7:c5c648898881 | 93 | void filter() |
JurrienBos | 7:c5c648898881 | 94 | { |
JurrienBos | 7:c5c648898881 | 95 | FilterHP_Bi_R = bqc1.step( emg0.read() ); |
JurrienBos | 7:c5c648898881 | 96 | Filterabs_Bi_R = fabs(FilterHP_Bi_R); |
JurrienBos | 7:c5c648898881 | 97 | Filtered_Bi_R = bqc2.step( Filterabs_Bi_R ); |
JurrienBos | 7:c5c648898881 | 98 | |
JurrienBos | 7:c5c648898881 | 99 | FilterHP_Bi_L = bqc3.step( emg1.read() ); |
JurrienBos | 7:c5c648898881 | 100 | Filterabs_Bi_L = fabs(FilterHP_Bi_L); |
JurrienBos | 7:c5c648898881 | 101 | Filtered_Bi_L = bqc4.step( Filterabs_Bi_L ); |
JurrienBos | 7:c5c648898881 | 102 | |
JurrienBos | 7:c5c648898881 | 103 | FilterHP_Tri_R = bqc5.step( emg2.read() ); |
JurrienBos | 7:c5c648898881 | 104 | Filterabs_Tri_R = fabs(FilterHP_Tri_R); |
JurrienBos | 7:c5c648898881 | 105 | Filtered_Tri_R = bqc6.step( Filterabs_Tri_R ); |
JurrienBos | 7:c5c648898881 | 106 | |
JurrienBos | 7:c5c648898881 | 107 | FilterHP_Tri_L = bqc7.step( emg3.read() ); |
JurrienBos | 7:c5c648898881 | 108 | Filterabs_Tri_L = fabs(FilterHP_Tri_L); |
JurrienBos | 7:c5c648898881 | 109 | Filtered_Tri_L = bqc8.step( Filterabs_Tri_L ); |
JurrienBos | 7:c5c648898881 | 110 | } |
JurrienBos | 7:c5c648898881 | 111 | |
ThomBMT | 3:c8f0fc045505 | 112 | void Encoding() |
ThomBMT | 3:c8f0fc045505 | 113 | { |
ThomBMT | 5:312186a0604d | 114 | |
ThomBMT | 3:c8f0fc045505 | 115 | counts1 = Encoder1.getPulses(); |
ThomBMT | 3:c8f0fc045505 | 116 | counts2 = Encoder2.getPulses(); |
ThomBMT | 5:312186a0604d | 117 | // Hier gaat iets fout waardoor het 0 wordt!!! |
JurrienBos | 7:c5c648898881 | 118 | rad_m1 = ((2.0*pi)/32.0)* (float)counts1; |
JurrienBos | 7:c5c648898881 | 119 | rad_m2 = ((2.0*pi)/32.0)* (float)counts2; |
ThomBMT | 4:8f67b8327300 | 120 | |
ThomBMT | 4:8f67b8327300 | 121 | // pc.printf("%f & %f ....\n",rad_m1, rad_m2); |
ThomBMT | 3:c8f0fc045505 | 122 | } |
ThomBMT | 2:dc9766657afb | 123 | |
ThomBMT | 2:dc9766657afb | 124 | void EMG_Read() |
ThomBMT | 2:dc9766657afb | 125 | { |
ThomBMT | 3:c8f0fc045505 | 126 | Bicep_Right = emg0.read(); |
ThomBMT | 3:c8f0fc045505 | 127 | Bicep_Left = emg1.read(); |
ThomBMT | 3:c8f0fc045505 | 128 | Tricep_Right = emg2.read(); |
ThomBMT | 3:c8f0fc045505 | 129 | Tricep_Left = emg3.read(); |
ThomBMT | 2:dc9766657afb | 130 | } |
ThomBMT | 2:dc9766657afb | 131 | |
ThomBMT | 2:dc9766657afb | 132 | void sample() |
ThomBMT | 2:dc9766657afb | 133 | { |
ThomBMT | 2:dc9766657afb | 134 | |
Brighton_the_robot | 9:355babe55589 | 135 | scope.set(0, Filtered_Bi_R*10.0f ); |
Brighton_the_robot | 9:355babe55589 | 136 | scope.set(1, Filtered_Bi_L*10.0f ); |
Brighton_the_robot | 9:355babe55589 | 137 | scope.set(2, Filtered_Tri_R*10.0f ); |
Brighton_the_robot | 9:355babe55589 | 138 | scope.set(3, Filtered_Tri_L*10.0f ); |
ThomBMT | 2:dc9766657afb | 139 | |
ThomBMT | 2:dc9766657afb | 140 | scope.send(); |
ThomBMT | 2:dc9766657afb | 141 | } |
ThomBMT | 2:dc9766657afb | 142 | |
JurrienBos | 1:4bf64d003f3a | 143 | |
JurrienBos | 1:4bf64d003f3a | 144 | void velocity1() |
JurrienBos | 0:4591ba678a39 | 145 | { |
ThomBMT | 3:c8f0fc045505 | 146 | if (pot1.read()>0.5f) |
JurrienBos | 0:4591ba678a39 | 147 | { |
JurrienBos | 0:4591ba678a39 | 148 | // Clockwise rotation |
JurrienBos | 1:4bf64d003f3a | 149 | referenceVelocity1 = (pot1.read()-0.5f) * 2.0f; |
JurrienBos | 0:4591ba678a39 | 150 | } |
JurrienBos | 0:4591ba678a39 | 151 | |
ThomBMT | 5:312186a0604d | 152 | else if (pot1.read() == 0.5f) |
JurrienBos | 0:4591ba678a39 | 153 | { |
JurrienBos | 1:4bf64d003f3a | 154 | referenceVelocity1 = pot1.read() * 0.0f; |
JurrienBos | 0:4591ba678a39 | 155 | } |
JurrienBos | 0:4591ba678a39 | 156 | |
JurrienBos | 0:4591ba678a39 | 157 | else if (pot1.read() < 0.5f) |
JurrienBos | 0:4591ba678a39 | 158 | { |
JurrienBos | 0:4591ba678a39 | 159 | // Counterclockwise rotation |
JurrienBos | 1:4bf64d003f3a | 160 | referenceVelocity1 = 2.0f * (pot1.read()-0.5f) ; |
JurrienBos | 0:4591ba678a39 | 161 | } |
JurrienBos | 0:4591ba678a39 | 162 | } |
JurrienBos | 0:4591ba678a39 | 163 | |
JurrienBos | 1:4bf64d003f3a | 164 | void velocity2() |
JurrienBos | 1:4bf64d003f3a | 165 | { |
JurrienBos | 1:4bf64d003f3a | 166 | if (pot2.read()>0.5f) |
JurrienBos | 1:4bf64d003f3a | 167 | { |
JurrienBos | 1:4bf64d003f3a | 168 | // Clockwise rotation |
JurrienBos | 1:4bf64d003f3a | 169 | referenceVelocity2 = (pot2.read()-0.5f) * 2.0f; |
JurrienBos | 1:4bf64d003f3a | 170 | } |
JurrienBos | 1:4bf64d003f3a | 171 | |
JurrienBos | 1:4bf64d003f3a | 172 | else if (pot2.read() == 0.5f) |
JurrienBos | 1:4bf64d003f3a | 173 | { |
JurrienBos | 1:4bf64d003f3a | 174 | referenceVelocity2 = pot2.read() * 0.0f; |
JurrienBos | 1:4bf64d003f3a | 175 | } |
JurrienBos | 1:4bf64d003f3a | 176 | |
JurrienBos | 1:4bf64d003f3a | 177 | else if (pot2.read() < 0.5f) |
JurrienBos | 1:4bf64d003f3a | 178 | { |
JurrienBos | 1:4bf64d003f3a | 179 | // Counterclockwise rotation |
JurrienBos | 1:4bf64d003f3a | 180 | referenceVelocity2 = 2.0f * (pot2.read()-0.5f) ; |
JurrienBos | 1:4bf64d003f3a | 181 | } |
JurrienBos | 1:4bf64d003f3a | 182 | } |
JurrienBos | 1:4bf64d003f3a | 183 | |
JurrienBos | 1:4bf64d003f3a | 184 | void motor1() |
JurrienBos | 0:4591ba678a39 | 185 | { |
JurrienBos | 1:4bf64d003f3a | 186 | float u = referenceVelocity1; |
JurrienBos | 0:4591ba678a39 | 187 | DirectionPin1 = u < 0.0f; |
JurrienBos | 0:4591ba678a39 | 188 | PwmPin1 = fabs(u); |
JurrienBos | 0:4591ba678a39 | 189 | } |
JurrienBos | 0:4591ba678a39 | 190 | |
JurrienBos | 1:4bf64d003f3a | 191 | void motor2() |
JurrienBos | 1:4bf64d003f3a | 192 | { |
JurrienBos | 1:4bf64d003f3a | 193 | float u = referenceVelocity2; |
JurrienBos | 1:4bf64d003f3a | 194 | DirectionPin2 = u > 0.0f; |
JurrienBos | 1:4bf64d003f3a | 195 | PwmPin2 = fabs(u); |
JurrienBos | 1:4bf64d003f3a | 196 | } |
JurrienBos | 0:4591ba678a39 | 197 | |
ThomBMT | 2:dc9766657afb | 198 | void Printing() |
ThomBMT | 2:dc9766657afb | 199 | { |
ThomBMT | 2:dc9766657afb | 200 | float v1 = fabs(referenceVelocity1) * maxVelocity; |
ThomBMT | 2:dc9766657afb | 201 | float v2 = fabs(referenceVelocity2) * maxVelocity; |
ThomBMT | 2:dc9766657afb | 202 | |
ThomBMT | 2:dc9766657afb | 203 | //eventueel nog counts -> rad/s |
ThomBMT | 2:dc9766657afb | 204 | |
ThomBMT | 2:dc9766657afb | 205 | //pc.printf("%f \n %f snelheid Motor1 \n %f snelheid Motor2 \n", Bicep_Right,v1,v2); |
ThomBMT | 5:312186a0604d | 206 | pc.printf("%i %i \n",counts1,counts2); |
ThomBMT | 2:dc9766657afb | 207 | } |
ThomBMT | 2:dc9766657afb | 208 | |
JurrienBos | 7:c5c648898881 | 209 | void inverse_kinematics() |
JurrienBos | 7:c5c648898881 | 210 | { |
JurrienBos | 7:c5c648898881 | 211 | float JacPs [2][2]; |
JurrienBos | 7:c5c648898881 | 212 | JacPs[0][0] = 2.0; |
JurrienBos | 7:c5c648898881 | 213 | JacPs[0][1] = 3.0; |
JurrienBos | 7:c5c648898881 | 214 | JacPs[1][0] = 4.0; |
JurrienBos | 7:c5c648898881 | 215 | JacPs[1][1] = 5.0; |
JurrienBos | 7:c5c648898881 | 216 | pc.printf("%f ", JacPs[0][0]); |
JurrienBos | 7:c5c648898881 | 217 | } |
JurrienBos | 7:c5c648898881 | 218 | |
JurrienBos | 7:c5c648898881 | 219 | |
JurrienBos | 7:c5c648898881 | 220 | |
ThomBMT | 4:8f67b8327300 | 221 | void StateMachine() |
ThomBMT | 4:8f67b8327300 | 222 | { |
ThomBMT | 4:8f67b8327300 | 223 | switch (Active_State) |
ThomBMT | 4:8f67b8327300 | 224 | { |
ThomBMT | 4:8f67b8327300 | 225 | case Calibration: |
ThomBMT | 5:312186a0604d | 226 | //calibration actions |
ThomBMT | 5:312186a0604d | 227 | //pc.printf("Calibration State"); |
ThomBMT | 5:312186a0604d | 228 | if (Knop1==false) |
ThomBMT | 5:312186a0604d | 229 | { |
ThomBMT | 5:312186a0604d | 230 | pc.printf("Entering Homing state \n"); |
ThomBMT | 5:312186a0604d | 231 | Active_State = Homing; |
ThomBMT | 5:312186a0604d | 232 | } |
JurrienBos | 7:c5c648898881 | 233 | filter(); |
ThomBMT | 5:312186a0604d | 234 | sample(); |
JurrienBos | 7:c5c648898881 | 235 | //EMG_Read(); |
JurrienBos | 7:c5c648898881 | 236 | Encoding(); |
ThomBMT | 5:312186a0604d | 237 | |
ThomBMT | 4:8f67b8327300 | 238 | break; |
ThomBMT | 4:8f67b8327300 | 239 | |
ThomBMT | 4:8f67b8327300 | 240 | case Homing: |
ThomBMT | 5:312186a0604d | 241 | //Homing actions |
ThomBMT | 5:312186a0604d | 242 | //pc.printf("Homing State"); |
ThomBMT | 5:312186a0604d | 243 | if (Knop2==false) |
ThomBMT | 5:312186a0604d | 244 | { |
ThomBMT | 5:312186a0604d | 245 | pc.printf("Entering Funtioning State \n"); |
ThomBMT | 5:312186a0604d | 246 | Active_State = Function; |
ThomBMT | 5:312186a0604d | 247 | } |
JurrienBos | 7:c5c648898881 | 248 | filter(); |
ThomBMT | 5:312186a0604d | 249 | sample(); |
JurrienBos | 7:c5c648898881 | 250 | //EMG_Read(); |
ThomBMT | 5:312186a0604d | 251 | Encoding(); |
ThomBMT | 4:8f67b8327300 | 252 | break; |
ThomBMT | 4:8f67b8327300 | 253 | |
ThomBMT | 4:8f67b8327300 | 254 | case Function: |
ThomBMT | 5:312186a0604d | 255 | //pc.printf("Funtioning State"); |
ThomBMT | 5:312186a0604d | 256 | |
ThomBMT | 5:312186a0604d | 257 | if (Knop2==false) |
ThomBMT | 5:312186a0604d | 258 | { |
ThomBMT | 5:312186a0604d | 259 | pc.printf("Re-entering Homing State \n"); |
ThomBMT | 5:312186a0604d | 260 | Active_State = Homing; |
ThomBMT | 5:312186a0604d | 261 | } |
ThomBMT | 5:312186a0604d | 262 | else if (Knop1==false) |
ThomBMT | 5:312186a0604d | 263 | { |
ThomBMT | 5:312186a0604d | 264 | pc.printf("Re-entering Calibration State \n"); |
ThomBMT | 5:312186a0604d | 265 | Active_State = Calibration; |
ThomBMT | 5:312186a0604d | 266 | } |
ThomBMT | 5:312186a0604d | 267 | |
JurrienBos | 7:c5c648898881 | 268 | filter(); |
ThomBMT | 5:312186a0604d | 269 | sample(); |
JurrienBos | 7:c5c648898881 | 270 | //EMG_Read(); |
ThomBMT | 5:312186a0604d | 271 | Encoding(); |
ThomBMT | 5:312186a0604d | 272 | velocity1(); |
ThomBMT | 5:312186a0604d | 273 | velocity2(); |
ThomBMT | 5:312186a0604d | 274 | motor1(); |
ThomBMT | 5:312186a0604d | 275 | motor2(); |
ThomBMT | 5:312186a0604d | 276 | |
ThomBMT | 4:8f67b8327300 | 277 | break; |
ThomBMT | 4:8f67b8327300 | 278 | |
ThomBMT | 4:8f67b8327300 | 279 | default: |
ThomBMT | 4:8f67b8327300 | 280 | pc.printf("UNKNOWN COMMAND"); |
ThomBMT | 4:8f67b8327300 | 281 | } |
ThomBMT | 4:8f67b8327300 | 282 | } |
ThomBMT | 4:8f67b8327300 | 283 | |
JurrienBos | 0:4591ba678a39 | 284 | int main() |
JurrienBos | 0:4591ba678a39 | 285 | { |
JurrienBos | 0:4591ba678a39 | 286 | pc.baud(115200); |
ThomBMT | 4:8f67b8327300 | 287 | PwmPin1.period_us(30); //60 microseconds pwm period, 16.7 kHz |
Brighton_the_robot | 8:b6b09226a421 | 288 | bqc1.add( &BqNotch1_1 ).add( &BqNotch2_1 ).add( &BqHP1 ); |
Brighton_the_robot | 8:b6b09226a421 | 289 | bqc2.add(&BqLP1); |
Brighton_the_robot | 8:b6b09226a421 | 290 | bqc3.add( &BqNotch1_2 ).add( &BqNotch2_2 ).add( &BqHP2 ); |
Brighton_the_robot | 8:b6b09226a421 | 291 | bqc4.add(&BqLP2); |
Brighton_the_robot | 8:b6b09226a421 | 292 | bqc5.add( &BqNotch1_3 ).add( &BqNotch2_3 ).add( &BqHP3 ); |
Brighton_the_robot | 8:b6b09226a421 | 293 | bqc6.add(&BqLP3); |
Brighton_the_robot | 8:b6b09226a421 | 294 | bqc7.add( &BqNotch1_4 ).add( &BqNotch2_4 ).add( &BqHP4 ); |
Brighton_the_robot | 8:b6b09226a421 | 295 | bqc8.add(&BqLP4); |
ThomBMT | 4:8f67b8327300 | 296 | StateTicker.attach(StateMachine, 0.002); |
ThomBMT | 4:8f67b8327300 | 297 | |
ThomBMT | 5:312186a0604d | 298 | printTicker.attach(&Printing, 2.0); |
ThomBMT | 2:dc9766657afb | 299 | |
JurrienBos | 0:4591ba678a39 | 300 | while(true) |
ThomBMT | 5:312186a0604d | 301 | { |
JurrienBos | 0:4591ba678a39 | 302 | } |
JurrienBos | 1:4bf64d003f3a | 303 | } |