![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Filter werkt eindelijk, echter zijn alle kanalen hetzelfde
Dependencies: HIDScope MODSERIAL Matrix QEI biquadFilter mbed
Fork of Turning_Motor_V3 by
main.cpp@5:312186a0604d, 2018-10-24 (annotated)
- Committer:
- ThomBMT
- Date:
- Wed Oct 24 10:30:47 2018 +0000
- Revision:
- 5:312186a0604d
- Parent:
- 4:8f67b8327300
- Child:
- 6:056ad27636ff
Counts zijn nu werkend. Omrekenen naar rad is nog niet kloppend
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" |
ThomBMT | 2:dc9766657afb | 5 | |
JurrienBos | 0:4591ba678a39 | 6 | MODSERIAL pc(USBTX, USBRX); |
JurrienBos | 0:4591ba678a39 | 7 | DigitalOut DirectionPin1(D4); |
JurrienBos | 1:4bf64d003f3a | 8 | DigitalOut DirectionPin2(D7); |
JurrienBos | 0:4591ba678a39 | 9 | PwmOut PwmPin1(D5); |
JurrienBos | 1:4bf64d003f3a | 10 | PwmOut PwmPin2(D6); |
JurrienBos | 0:4591ba678a39 | 11 | DigitalIn Knop1(D2); |
ThomBMT | 4:8f67b8327300 | 12 | DigitalIn Knop2(D3); |
JurrienBos | 0:4591ba678a39 | 13 | AnalogIn pot1 (A5); |
JurrienBos | 1:4bf64d003f3a | 14 | AnalogIn pot2 (A4); |
ThomBMT | 2:dc9766657afb | 15 | AnalogIn emg0( A0 ); |
ThomBMT | 2:dc9766657afb | 16 | AnalogIn emg1( A1 ); |
ThomBMT | 2:dc9766657afb | 17 | AnalogIn emg2( A2 ); |
ThomBMT | 2:dc9766657afb | 18 | AnalogIn emg3( A3 ); |
JurrienBos | 0:4591ba678a39 | 19 | |
ThomBMT | 5:312186a0604d | 20 | QEI Encoder1(D12,D13,NC,64); |
ThomBMT | 5:312186a0604d | 21 | QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING); |
ThomBMT | 5:312186a0604d | 22 | |
ThomBMT | 5:312186a0604d | 23 | //DigitalOut LED(LED_RED); |
ThomBMT | 5:312186a0604d | 24 | |
ThomBMT | 4:8f67b8327300 | 25 | Ticker StateTicker; |
ThomBMT | 2:dc9766657afb | 26 | Ticker printTicker; |
ThomBMT | 5:312186a0604d | 27 | |
ThomBMT | 2:dc9766657afb | 28 | HIDScope scope( 4 ); |
JurrienBos | 0:4591ba678a39 | 29 | |
ThomBMT | 3:c8f0fc045505 | 30 | volatile float Bicep_Right = 0.0; |
ThomBMT | 3:c8f0fc045505 | 31 | volatile float Bicep_Left = 0.0; |
ThomBMT | 3:c8f0fc045505 | 32 | volatile float Tricep_Right = 0.0; |
ThomBMT | 3:c8f0fc045505 | 33 | volatile float Tricep_Left = 0.0; |
ThomBMT | 3:c8f0fc045505 | 34 | volatile const float maxVelocity = 8.4; // in rad/s |
ThomBMT | 4:8f67b8327300 | 35 | volatile const double pi = 3.14159265358979; |
ThomBMT | 3:c8f0fc045505 | 36 | volatile float referenceVelocity1 = 0.5; //dit is de gecentreerde waarde en dus de nulstand |
ThomBMT | 3:c8f0fc045505 | 37 | volatile float referenceVelocity2 = 0.5; |
ThomBMT | 2:dc9766657afb | 38 | |
ThomBMT | 4:8f67b8327300 | 39 | enum states{Calibration, Homing, Function}; |
ThomBMT | 4:8f67b8327300 | 40 | |
ThomBMT | 4:8f67b8327300 | 41 | volatile states Active_State = Calibration; |
ThomBMT | 4:8f67b8327300 | 42 | |
ThomBMT | 5:312186a0604d | 43 | volatile int counts1 ; |
ThomBMT | 5:312186a0604d | 44 | volatile int counts2 ; |
ThomBMT | 5:312186a0604d | 45 | volatile float rad_m1; |
ThomBMT | 5:312186a0604d | 46 | volatile float rad_m2; |
ThomBMT | 4:8f67b8327300 | 47 | |
ThomBMT | 3:c8f0fc045505 | 48 | void Encoding() |
ThomBMT | 3:c8f0fc045505 | 49 | { |
ThomBMT | 5:312186a0604d | 50 | |
ThomBMT | 3:c8f0fc045505 | 51 | counts1 = Encoder1.getPulses(); |
ThomBMT | 3:c8f0fc045505 | 52 | counts2 = Encoder2.getPulses(); |
ThomBMT | 5:312186a0604d | 53 | // Hier gaat iets fout waardoor het 0 wordt!!! |
ThomBMT | 4:8f67b8327300 | 54 | float rad_m1 = ((2*pi)/32.0)* (float)counts1; |
ThomBMT | 4:8f67b8327300 | 55 | float rad_m2 = ((2*pi)/32.0)* (float)counts2; |
ThomBMT | 4:8f67b8327300 | 56 | |
ThomBMT | 4:8f67b8327300 | 57 | // pc.printf("%f & %f ....\n",rad_m1, rad_m2); |
ThomBMT | 3:c8f0fc045505 | 58 | } |
ThomBMT | 2:dc9766657afb | 59 | |
ThomBMT | 2:dc9766657afb | 60 | void EMG_Read() |
ThomBMT | 2:dc9766657afb | 61 | { |
ThomBMT | 3:c8f0fc045505 | 62 | Bicep_Right = emg0.read(); |
ThomBMT | 3:c8f0fc045505 | 63 | Bicep_Left = emg1.read(); |
ThomBMT | 3:c8f0fc045505 | 64 | Tricep_Right = emg2.read(); |
ThomBMT | 3:c8f0fc045505 | 65 | Tricep_Left = emg3.read(); |
ThomBMT | 2:dc9766657afb | 66 | } |
ThomBMT | 2:dc9766657afb | 67 | |
ThomBMT | 2:dc9766657afb | 68 | void sample() |
ThomBMT | 2:dc9766657afb | 69 | { |
ThomBMT | 2:dc9766657afb | 70 | |
ThomBMT | 2:dc9766657afb | 71 | scope.set(0, emg0.read() ); |
ThomBMT | 2:dc9766657afb | 72 | scope.set(1, emg1.read() ); |
ThomBMT | 2:dc9766657afb | 73 | scope.set(2, emg2.read() ); |
ThomBMT | 2:dc9766657afb | 74 | scope.set(3, emg3.read() ); |
ThomBMT | 2:dc9766657afb | 75 | |
ThomBMT | 2:dc9766657afb | 76 | scope.send(); |
ThomBMT | 2:dc9766657afb | 77 | } |
ThomBMT | 2:dc9766657afb | 78 | |
JurrienBos | 1:4bf64d003f3a | 79 | |
JurrienBos | 1:4bf64d003f3a | 80 | void velocity1() |
JurrienBos | 0:4591ba678a39 | 81 | { |
ThomBMT | 3:c8f0fc045505 | 82 | if (pot1.read()>0.5f) |
JurrienBos | 0:4591ba678a39 | 83 | { |
JurrienBos | 0:4591ba678a39 | 84 | // Clockwise rotation |
JurrienBos | 1:4bf64d003f3a | 85 | referenceVelocity1 = (pot1.read()-0.5f) * 2.0f; |
JurrienBos | 0:4591ba678a39 | 86 | } |
JurrienBos | 0:4591ba678a39 | 87 | |
ThomBMT | 5:312186a0604d | 88 | else if (pot1.read() == 0.5f) |
JurrienBos | 0:4591ba678a39 | 89 | { |
JurrienBos | 1:4bf64d003f3a | 90 | referenceVelocity1 = pot1.read() * 0.0f; |
JurrienBos | 0:4591ba678a39 | 91 | } |
JurrienBos | 0:4591ba678a39 | 92 | |
JurrienBos | 0:4591ba678a39 | 93 | else if (pot1.read() < 0.5f) |
JurrienBos | 0:4591ba678a39 | 94 | { |
JurrienBos | 0:4591ba678a39 | 95 | // Counterclockwise rotation |
JurrienBos | 1:4bf64d003f3a | 96 | referenceVelocity1 = 2.0f * (pot1.read()-0.5f) ; |
JurrienBos | 0:4591ba678a39 | 97 | } |
JurrienBos | 0:4591ba678a39 | 98 | } |
JurrienBos | 0:4591ba678a39 | 99 | |
JurrienBos | 1:4bf64d003f3a | 100 | void velocity2() |
JurrienBos | 1:4bf64d003f3a | 101 | { |
JurrienBos | 1:4bf64d003f3a | 102 | if (pot2.read()>0.5f) |
JurrienBos | 1:4bf64d003f3a | 103 | { |
JurrienBos | 1:4bf64d003f3a | 104 | // Clockwise rotation |
JurrienBos | 1:4bf64d003f3a | 105 | referenceVelocity2 = (pot2.read()-0.5f) * 2.0f; |
JurrienBos | 1:4bf64d003f3a | 106 | } |
JurrienBos | 1:4bf64d003f3a | 107 | |
JurrienBos | 1:4bf64d003f3a | 108 | else if (pot2.read() == 0.5f) |
JurrienBos | 1:4bf64d003f3a | 109 | { |
JurrienBos | 1:4bf64d003f3a | 110 | referenceVelocity2 = pot2.read() * 0.0f; |
JurrienBos | 1:4bf64d003f3a | 111 | } |
JurrienBos | 1:4bf64d003f3a | 112 | |
JurrienBos | 1:4bf64d003f3a | 113 | else if (pot2.read() < 0.5f) |
JurrienBos | 1:4bf64d003f3a | 114 | { |
JurrienBos | 1:4bf64d003f3a | 115 | // Counterclockwise rotation |
JurrienBos | 1:4bf64d003f3a | 116 | referenceVelocity2 = 2.0f * (pot2.read()-0.5f) ; |
JurrienBos | 1:4bf64d003f3a | 117 | } |
JurrienBos | 1:4bf64d003f3a | 118 | } |
JurrienBos | 1:4bf64d003f3a | 119 | |
JurrienBos | 1:4bf64d003f3a | 120 | void motor1() |
JurrienBos | 0:4591ba678a39 | 121 | { |
JurrienBos | 1:4bf64d003f3a | 122 | float u = referenceVelocity1; |
JurrienBos | 0:4591ba678a39 | 123 | DirectionPin1 = u < 0.0f; |
JurrienBos | 0:4591ba678a39 | 124 | PwmPin1 = fabs(u); |
JurrienBos | 0:4591ba678a39 | 125 | } |
JurrienBos | 0:4591ba678a39 | 126 | |
JurrienBos | 1:4bf64d003f3a | 127 | void motor2() |
JurrienBos | 1:4bf64d003f3a | 128 | { |
JurrienBos | 1:4bf64d003f3a | 129 | float u = referenceVelocity2; |
JurrienBos | 1:4bf64d003f3a | 130 | DirectionPin2 = u > 0.0f; |
JurrienBos | 1:4bf64d003f3a | 131 | PwmPin2 = fabs(u); |
JurrienBos | 1:4bf64d003f3a | 132 | } |
JurrienBos | 0:4591ba678a39 | 133 | |
ThomBMT | 2:dc9766657afb | 134 | void Printing() |
ThomBMT | 2:dc9766657afb | 135 | { |
ThomBMT | 2:dc9766657afb | 136 | float v1 = fabs(referenceVelocity1) * maxVelocity; |
ThomBMT | 2:dc9766657afb | 137 | float v2 = fabs(referenceVelocity2) * maxVelocity; |
ThomBMT | 2:dc9766657afb | 138 | |
ThomBMT | 2:dc9766657afb | 139 | //eventueel nog counts -> rad/s |
ThomBMT | 2:dc9766657afb | 140 | |
ThomBMT | 2:dc9766657afb | 141 | //pc.printf("%f \n %f snelheid Motor1 \n %f snelheid Motor2 \n", Bicep_Right,v1,v2); |
ThomBMT | 5:312186a0604d | 142 | pc.printf("%i %i \n",counts1,counts2); |
ThomBMT | 2:dc9766657afb | 143 | } |
ThomBMT | 2:dc9766657afb | 144 | |
ThomBMT | 4:8f67b8327300 | 145 | void StateMachine() |
ThomBMT | 4:8f67b8327300 | 146 | { |
ThomBMT | 4:8f67b8327300 | 147 | switch (Active_State) |
ThomBMT | 4:8f67b8327300 | 148 | { |
ThomBMT | 4:8f67b8327300 | 149 | case Calibration: |
ThomBMT | 5:312186a0604d | 150 | //calibration actions |
ThomBMT | 5:312186a0604d | 151 | //pc.printf("Calibration State"); |
ThomBMT | 5:312186a0604d | 152 | if (Knop1==false) |
ThomBMT | 5:312186a0604d | 153 | { |
ThomBMT | 5:312186a0604d | 154 | pc.printf("Entering Homing state \n"); |
ThomBMT | 5:312186a0604d | 155 | Active_State = Homing; |
ThomBMT | 5:312186a0604d | 156 | } |
ThomBMT | 5:312186a0604d | 157 | |
ThomBMT | 5:312186a0604d | 158 | sample(); |
ThomBMT | 5:312186a0604d | 159 | EMG_Read(); |
ThomBMT | 5:312186a0604d | 160 | Encoding(); |
ThomBMT | 5:312186a0604d | 161 | |
ThomBMT | 4:8f67b8327300 | 162 | break; |
ThomBMT | 4:8f67b8327300 | 163 | |
ThomBMT | 4:8f67b8327300 | 164 | case Homing: |
ThomBMT | 5:312186a0604d | 165 | //Homing actions |
ThomBMT | 5:312186a0604d | 166 | //pc.printf("Homing State"); |
ThomBMT | 5:312186a0604d | 167 | if (Knop2==false) |
ThomBMT | 5:312186a0604d | 168 | { |
ThomBMT | 5:312186a0604d | 169 | pc.printf("Entering Funtioning State \n"); |
ThomBMT | 5:312186a0604d | 170 | Active_State = Function; |
ThomBMT | 5:312186a0604d | 171 | } |
ThomBMT | 5:312186a0604d | 172 | |
ThomBMT | 5:312186a0604d | 173 | sample(); |
ThomBMT | 5:312186a0604d | 174 | EMG_Read(); |
ThomBMT | 5:312186a0604d | 175 | Encoding(); |
ThomBMT | 4:8f67b8327300 | 176 | break; |
ThomBMT | 4:8f67b8327300 | 177 | |
ThomBMT | 4:8f67b8327300 | 178 | case Function: |
ThomBMT | 5:312186a0604d | 179 | //pc.printf("Funtioning State"); |
ThomBMT | 5:312186a0604d | 180 | |
ThomBMT | 5:312186a0604d | 181 | if (Knop2==false) |
ThomBMT | 5:312186a0604d | 182 | { |
ThomBMT | 5:312186a0604d | 183 | pc.printf("Re-entering Homing State \n"); |
ThomBMT | 5:312186a0604d | 184 | Active_State = Homing; |
ThomBMT | 5:312186a0604d | 185 | } |
ThomBMT | 5:312186a0604d | 186 | else if (Knop1==false) |
ThomBMT | 5:312186a0604d | 187 | { |
ThomBMT | 5:312186a0604d | 188 | pc.printf("Re-entering Calibration State \n"); |
ThomBMT | 5:312186a0604d | 189 | Active_State = Calibration; |
ThomBMT | 5:312186a0604d | 190 | } |
ThomBMT | 5:312186a0604d | 191 | |
ThomBMT | 5:312186a0604d | 192 | |
ThomBMT | 5:312186a0604d | 193 | sample(); |
ThomBMT | 5:312186a0604d | 194 | EMG_Read(); |
ThomBMT | 5:312186a0604d | 195 | Encoding(); |
ThomBMT | 5:312186a0604d | 196 | velocity1(); |
ThomBMT | 5:312186a0604d | 197 | velocity2(); |
ThomBMT | 5:312186a0604d | 198 | motor1(); |
ThomBMT | 5:312186a0604d | 199 | motor2(); |
ThomBMT | 5:312186a0604d | 200 | |
ThomBMT | 4:8f67b8327300 | 201 | break; |
ThomBMT | 4:8f67b8327300 | 202 | |
ThomBMT | 4:8f67b8327300 | 203 | default: |
ThomBMT | 4:8f67b8327300 | 204 | pc.printf("UNKNOWN COMMAND"); |
ThomBMT | 4:8f67b8327300 | 205 | } |
ThomBMT | 4:8f67b8327300 | 206 | } |
ThomBMT | 4:8f67b8327300 | 207 | |
JurrienBos | 0:4591ba678a39 | 208 | int main() |
JurrienBos | 0:4591ba678a39 | 209 | { |
JurrienBos | 0:4591ba678a39 | 210 | pc.baud(115200); |
ThomBMT | 4:8f67b8327300 | 211 | PwmPin1.period_us(30); //60 microseconds pwm period, 16.7 kHz |
ThomBMT | 2:dc9766657afb | 212 | |
ThomBMT | 4:8f67b8327300 | 213 | StateTicker.attach(StateMachine, 0.002); |
ThomBMT | 4:8f67b8327300 | 214 | |
ThomBMT | 5:312186a0604d | 215 | printTicker.attach(&Printing, 2.0); |
ThomBMT | 2:dc9766657afb | 216 | |
JurrienBos | 0:4591ba678a39 | 217 | while(true) |
ThomBMT | 5:312186a0604d | 218 | { |
JurrienBos | 0:4591ba678a39 | 219 | } |
JurrienBos | 1:4bf64d003f3a | 220 | } |