![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
encoder
Dependencies: MODSERIAL mbed HIDScope biquadFilter
Fork of Project_script by
main.cpp@19:466ada92bf65, 2018-10-22 (annotated)
- Committer:
- MarijkeZondag
- Date:
- Mon Oct 22 12:57:28 2018 +0000
- Revision:
- 19:466ada92bf65
- Parent:
- 18:898f54c6aa3d
- Child:
- 20:bd9495b31f50
hidscope werkend hopelijk;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
vsluiter | 0:c8f15874531b | 1 | #include "mbed.h" |
vsluiter | 0:c8f15874531b | 2 | #include "MODSERIAL.h" |
MarijkeZondag | 10:39ec51206c8b | 3 | #include "BiQuad.h" |
MarijkeZondag | 10:39ec51206c8b | 4 | #include "HIDScope.h" |
MarijkeZondag | 10:39ec51206c8b | 5 | #include <math.h> |
vsluiter | 0:c8f15874531b | 6 | |
MarijkeZondag | 10:39ec51206c8b | 7 | AnalogIn emg0_in (A0); |
MarijkeZondag | 10:39ec51206c8b | 8 | AnalogIn emg1_in (A1); |
MarijkeZondag | 10:39ec51206c8b | 9 | AnalogIn emg2_in (A2); |
MarijkeZondag | 10:39ec51206c8b | 10 | |
MarijkeZondag | 16:5f7196ddc77b | 11 | InterruptIn button1 (D10); //Let op, is deze niet bezet? En 11? Even checken, als er een error komt, kan het hier zitten. |
MarijkeZondag | 16:5f7196ddc77b | 12 | InterruptIn button2 (D11); |
MarijkeZondag | 9:c722418997b5 | 13 | InterruptIn encoderA (D9); |
MarijkeZondag | 9:c722418997b5 | 14 | InterruptIn encoderB (D8); |
MarijkeZondag | 6:f4bbb73f3989 | 15 | |
MarijkeZondag | 9:c722418997b5 | 16 | DigitalOut directionpin1 (D4); |
MarijkeZondag | 9:c722418997b5 | 17 | DigitalOut directionpin2 (D7); |
MarijkeZondag | 17:741798018c0d | 18 | DigitalOut ledr (LED_RED); |
MarijkeZondag | 17:741798018c0d | 19 | DigitalOut ledb (LED_BLUE); |
MarijkeZondag | 17:741798018c0d | 20 | DigitalOut ledg (LED_GREEN); |
MarijkeZondag | 13:a3d4b4daf5b4 | 21 | |
MarijkeZondag | 9:c722418997b5 | 22 | PwmOut pwmpin1 (D5); |
MarijkeZondag | 9:c722418997b5 | 23 | PwmOut pwmpin2 (D6); |
MarijkeZondag | 9:c722418997b5 | 24 | |
MarijkeZondag | 6:f4bbb73f3989 | 25 | |
vsluiter | 0:c8f15874531b | 26 | MODSERIAL pc(USBTX, USBRX); |
vsluiter | 0:c8f15874531b | 27 | |
MarijkeZondag | 19:466ada92bf65 | 28 | //HIDscope |
MarijkeZondag | 19:466ada92bf65 | 29 | Ticker sample_timer; |
MarijkeZondag | 19:466ada92bf65 | 30 | HIDScope scope( 2 ); |
MarijkeZondag | 19:466ada92bf65 | 31 | DigitalOut led(LED1); |
MarijkeZondag | 9:c722418997b5 | 32 | |
MarijkeZondag | 9:c722418997b5 | 33 | //Global variables |
MarijkeZondag | 13:a3d4b4daf5b4 | 34 | int encoder = 0; //Starting point encoder |
MarijkeZondag | 12:eaed305a76c3 | 35 | const float T = 0.001f; //Ticker period |
MarijkeZondag | 10:39ec51206c8b | 36 | |
MarijkeZondag | 13:a3d4b4daf5b4 | 37 | //EMG filter |
MarijkeZondag | 13:a3d4b4daf5b4 | 38 | double emgfilter0, emgfilter1, emgfilter2; //Filtered EMG data 0, 1 and 2 |
MarijkeZondag | 14:fa09dae67390 | 39 | const int windowsize = 150; //Size of the array over which the moving average (MovAg) is calculated |
MarijkeZondag | 13:a3d4b4daf5b4 | 40 | double sum, sum1, sum2, sum3; //variables used to sum elements in array |
MarijkeZondag | 14:fa09dae67390 | 41 | double StoreArray0[windowsize], StoreArray1[windowsize], StoreArray2[windowsize]; //Empty arrays to calculate MoveAg |
MarijkeZondag | 13:a3d4b4daf5b4 | 42 | double movAg0,movAg1,movAg2; //outcome of MovAg |
MarijkeZondag | 13:a3d4b4daf5b4 | 43 | |
MarijkeZondag | 13:a3d4b4daf5b4 | 44 | //calibration |
MarijkeZondag | 18:898f54c6aa3d | 45 | static int x = -1; |
MarijkeZondag | 13:a3d4b4daf5b4 | 46 | int emg_cal = 0; |
MarijkeZondag | 13:a3d4b4daf5b4 | 47 | const int sizeCal = 2000; |
MarijkeZondag | 14:fa09dae67390 | 48 | double StoreCal0[sizeCal], StoreCal1[sizeCal], StoreCal2[sizeCal]; |
MarijkeZondag | 14:fa09dae67390 | 49 | double Mean0,Mean1,Mean2; |
MarijkeZondag | 13:a3d4b4daf5b4 | 50 | double Threshold0 = 1, Threshold1 = 1, Threshold2 = 1; |
MarijkeZondag | 13:a3d4b4daf5b4 | 51 | |
MarijkeZondag | 13:a3d4b4daf5b4 | 52 | |
MarijkeZondag | 10:39ec51206c8b | 53 | //Biquad |
MarijkeZondag | 10:39ec51206c8b | 54 | BiQuadChain emg0band; |
MarijkeZondag | 10:39ec51206c8b | 55 | BiQuad emg0band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 56 | BiQuad emg0band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 57 | BiQuad emg0band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 58 | |
MarijkeZondag | 10:39ec51206c8b | 59 | BiQuadChain emg1band; |
MarijkeZondag | 10:39ec51206c8b | 60 | BiQuad emg1band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 61 | BiQuad emg1band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 62 | BiQuad emg1band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 63 | |
MarijkeZondag | 10:39ec51206c8b | 64 | BiQuadChain emg2band; |
MarijkeZondag | 10:39ec51206c8b | 65 | BiQuad emg2band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 66 | BiQuad emg2band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 67 | BiQuad emg2band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 68 | |
MarijkeZondag | 10:39ec51206c8b | 69 | BiQuad notch1( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter |
MarijkeZondag | 14:fa09dae67390 | 70 | BiQuad notch2( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter |
MarijkeZondag | 14:fa09dae67390 | 71 | BiQuad notch3( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter |
MarijkeZondag | 10:39ec51206c8b | 72 | |
MarijkeZondag | 10:39ec51206c8b | 73 | |
MarijkeZondag | 10:39ec51206c8b | 74 | //Tickers |
MarijkeZondag | 12:eaed305a76c3 | 75 | Ticker filter_tick; |
MarijkeZondag | 12:eaed305a76c3 | 76 | Ticker MovAg_tick; |
MarijkeZondag | 9:c722418997b5 | 77 | |
MarijkeZondag | 9:c722418997b5 | 78 | //Functions |
MarijkeZondag | 19:466ada92bf65 | 79 | void sample() |
MarijkeZondag | 19:466ada92bf65 | 80 | { |
MarijkeZondag | 19:466ada92bf65 | 81 | /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */ |
MarijkeZondag | 19:466ada92bf65 | 82 | scope.set(0, emg0_in.read() ); |
MarijkeZondag | 19:466ada92bf65 | 83 | scope.set(1, emg1_in.read() ); |
MarijkeZondag | 19:466ada92bf65 | 84 | /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels) |
MarijkeZondag | 19:466ada92bf65 | 85 | * Ensure that enough channels are available (HIDScope scope( 2 )) |
MarijkeZondag | 19:466ada92bf65 | 86 | * Finally, send all channels to the PC at once */ |
MarijkeZondag | 19:466ada92bf65 | 87 | scope.send(); |
MarijkeZondag | 19:466ada92bf65 | 88 | /* To indicate that the function is working, the LED is toggled */ |
MarijkeZondag | 19:466ada92bf65 | 89 | led = !led; |
MarijkeZondag | 19:466ada92bf65 | 90 | } |
MarijkeZondag | 19:466ada92bf65 | 91 | |
MarijkeZondag | 10:39ec51206c8b | 92 | void EMGFilter0() |
MarijkeZondag | 10:39ec51206c8b | 93 | { |
MarijkeZondag | 11:b95b0e9e1b89 | 94 | double emg0 = emg0_in.read(); |
MarijkeZondag | 11:b95b0e9e1b89 | 95 | double bandpass0 = emg0band.step(emg0); |
MarijkeZondag | 11:b95b0e9e1b89 | 96 | double absolute0 = fabs(bandpass0); |
MarijkeZondag | 11:b95b0e9e1b89 | 97 | double notch0 = notch1.step(absolute0); |
MarijkeZondag | 10:39ec51206c8b | 98 | } |
MarijkeZondag | 10:39ec51206c8b | 99 | |
MarijkeZondag | 10:39ec51206c8b | 100 | void EMGFilter1() |
MarijkeZondag | 10:39ec51206c8b | 101 | { |
MarijkeZondag | 11:b95b0e9e1b89 | 102 | double emg1 = emg1_in.read(); |
MarijkeZondag | 11:b95b0e9e1b89 | 103 | double bandpass1 = emg1band.step(emg1); |
MarijkeZondag | 11:b95b0e9e1b89 | 104 | double absolute1 = fabs(bandpass1); |
MarijkeZondag | 14:fa09dae67390 | 105 | double notch1 = notch2.step(absolute1); |
MarijkeZondag | 10:39ec51206c8b | 106 | } |
MarijkeZondag | 10:39ec51206c8b | 107 | |
MarijkeZondag | 10:39ec51206c8b | 108 | void EMGFilter2() |
MarijkeZondag | 10:39ec51206c8b | 109 | { |
MarijkeZondag | 11:b95b0e9e1b89 | 110 | double emg2 = emg2_in.read(); |
MarijkeZondag | 11:b95b0e9e1b89 | 111 | double bandpass2 = emg2band.step(emg2); |
MarijkeZondag | 11:b95b0e9e1b89 | 112 | double absolute2 = fabs(bandpass2); |
MarijkeZondag | 14:fa09dae67390 | 113 | double notch2 = notch3.step(absolute2); |
MarijkeZondag | 10:39ec51206c8b | 114 | } |
MarijkeZondag | 10:39ec51206c8b | 115 | |
MarijkeZondag | 13:a3d4b4daf5b4 | 116 | void MovAg() //Calculate moving average (MovAg) |
MarijkeZondag | 13:a3d4b4daf5b4 | 117 | { |
MarijkeZondag | 13:a3d4b4daf5b4 | 118 | for (int i = windowsize-1; i>=0; i--) //Make array of the last datapoints of the filtered signal |
MarijkeZondag | 13:a3d4b4daf5b4 | 119 | { |
MarijkeZondag | 13:a3d4b4daf5b4 | 120 | StoreArray0[i] = StoreArray0[i-1]; |
MarijkeZondag | 13:a3d4b4daf5b4 | 121 | StoreArray1[i] = StoreArray1[i-1]; |
MarijkeZondag | 13:a3d4b4daf5b4 | 122 | StoreArray2[i] = StoreArray2[i-1]; |
MarijkeZondag | 13:a3d4b4daf5b4 | 123 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 124 | |
MarijkeZondag | 13:a3d4b4daf5b4 | 125 | StoreArray0[0] = emgfilter0; //Stores the latest datapoint in the first element of the array |
MarijkeZondag | 13:a3d4b4daf5b4 | 126 | StoreArray1[0] = emgfilter1; |
MarijkeZondag | 13:a3d4b4daf5b4 | 127 | StoreArray2[0] = emgfilter2; |
MarijkeZondag | 13:a3d4b4daf5b4 | 128 | |
MarijkeZondag | 13:a3d4b4daf5b4 | 129 | sum1 = 0.0; |
MarijkeZondag | 13:a3d4b4daf5b4 | 130 | sum2 = 0.0; |
MarijkeZondag | 13:a3d4b4daf5b4 | 131 | sum3 = 0.0; |
MarijkeZondag | 13:a3d4b4daf5b4 | 132 | |
MarijkeZondag | 13:a3d4b4daf5b4 | 133 | for(int a = 0; a<= windowsize-1; a++) //Sum the elements in the array |
MarijkeZondag | 13:a3d4b4daf5b4 | 134 | { |
MarijkeZondag | 13:a3d4b4daf5b4 | 135 | sum1 += StoreArray0[a]; |
MarijkeZondag | 13:a3d4b4daf5b4 | 136 | sum2 += StoreArray1[a]; |
MarijkeZondag | 13:a3d4b4daf5b4 | 137 | sum3 += StoreArray2[a]; |
MarijkeZondag | 13:a3d4b4daf5b4 | 138 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 139 | |
MarijkeZondag | 13:a3d4b4daf5b4 | 140 | movAg0 = sum1/windowsize; //calculates an average in the array |
MarijkeZondag | 13:a3d4b4daf5b4 | 141 | movAg1 = sum2/windowsize; |
MarijkeZondag | 14:fa09dae67390 | 142 | movAg2 = sum3/windowsize; |
MarijkeZondag | 13:a3d4b4daf5b4 | 143 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 144 | |
MarijkeZondag | 13:a3d4b4daf5b4 | 145 | void emg_filtered() //Call all filter functions |
MarijkeZondag | 12:eaed305a76c3 | 146 | { |
MarijkeZondag | 12:eaed305a76c3 | 147 | EMGFilter0(); |
MarijkeZondag | 12:eaed305a76c3 | 148 | EMGFilter1(); |
MarijkeZondag | 12:eaed305a76c3 | 149 | EMGFilter2(); |
MarijkeZondag | 13:a3d4b4daf5b4 | 150 | MovAg(); |
MarijkeZondag | 12:eaed305a76c3 | 151 | } |
MarijkeZondag | 12:eaed305a76c3 | 152 | |
MarijkeZondag | 13:a3d4b4daf5b4 | 153 | void switch_to_calibrate() |
MarijkeZondag | 13:a3d4b4daf5b4 | 154 | { |
MarijkeZondag | 13:a3d4b4daf5b4 | 155 | x++; |
MarijkeZondag | 13:a3d4b4daf5b4 | 156 | |
MarijkeZondag | 13:a3d4b4daf5b4 | 157 | if(x==0) //If x = 0, led is red |
MarijkeZondag | 13:a3d4b4daf5b4 | 158 | { |
MarijkeZondag | 17:741798018c0d | 159 | ledr = 0; |
MarijkeZondag | 17:741798018c0d | 160 | ledb = 1; |
MarijkeZondag | 17:741798018c0d | 161 | ledg = 1; |
MarijkeZondag | 13:a3d4b4daf5b4 | 162 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 163 | else if (x==1) //If x = 1, led is blue |
MarijkeZondag | 13:a3d4b4daf5b4 | 164 | { |
MarijkeZondag | 17:741798018c0d | 165 | ledr = 1; |
MarijkeZondag | 17:741798018c0d | 166 | ledb = 0; |
MarijkeZondag | 17:741798018c0d | 167 | ledg = 1; |
MarijkeZondag | 13:a3d4b4daf5b4 | 168 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 169 | else if (x == 2) //If x = 2, led is green |
MarijkeZondag | 13:a3d4b4daf5b4 | 170 | { |
MarijkeZondag | 17:741798018c0d | 171 | ledr = 1; |
MarijkeZondag | 17:741798018c0d | 172 | ledb = 1; |
MarijkeZondag | 17:741798018c0d | 173 | ledg = 0; |
MarijkeZondag | 13:a3d4b4daf5b4 | 174 | } |
MarijkeZondag | 18:898f54c6aa3d | 175 | else //If x = 3, led is white |
MarijkeZondag | 13:a3d4b4daf5b4 | 176 | { |
MarijkeZondag | 17:741798018c0d | 177 | ledr = 0; |
MarijkeZondag | 17:741798018c0d | 178 | ledb = 0; |
MarijkeZondag | 17:741798018c0d | 179 | ledg = 0; |
MarijkeZondag | 13:a3d4b4daf5b4 | 180 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 181 | |
MarijkeZondag | 13:a3d4b4daf5b4 | 182 | if(x>=4) //Reset back to x = 0 |
MarijkeZondag | 13:a3d4b4daf5b4 | 183 | { |
MarijkeZondag | 16:5f7196ddc77b | 184 | x = -1; |
MarijkeZondag | 13:a3d4b4daf5b4 | 185 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 186 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 187 | |
MarijkeZondag | 13:a3d4b4daf5b4 | 188 | |
MarijkeZondag | 13:a3d4b4daf5b4 | 189 | void calibrate(void) |
MarijkeZondag | 12:eaed305a76c3 | 190 | { |
MarijkeZondag | 13:a3d4b4daf5b4 | 191 | switch(x) |
MarijkeZondag | 13:a3d4b4daf5b4 | 192 | { |
MarijkeZondag | 13:a3d4b4daf5b4 | 193 | case 0: |
MarijkeZondag | 13:a3d4b4daf5b4 | 194 | { |
MarijkeZondag | 13:a3d4b4daf5b4 | 195 | sum = 0.0; |
MarijkeZondag | 13:a3d4b4daf5b4 | 196 | for(int j = 0; j<=sizeCal-1; j++) |
MarijkeZondag | 13:a3d4b4daf5b4 | 197 | { |
MarijkeZondag | 13:a3d4b4daf5b4 | 198 | StoreCal0[j] = emgfilter0; |
MarijkeZondag | 13:a3d4b4daf5b4 | 199 | sum+=StoreCal0[j]; |
MarijkeZondag | 18:898f54c6aa3d | 200 | //wait(0.001f); |
MarijkeZondag | 13:a3d4b4daf5b4 | 201 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 202 | Mean0 = sum/sizeCal; |
MarijkeZondag | 13:a3d4b4daf5b4 | 203 | Threshold0 = Mean0/2; |
MarijkeZondag | 13:a3d4b4daf5b4 | 204 | break; |
MarijkeZondag | 13:a3d4b4daf5b4 | 205 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 206 | case 1: |
MarijkeZondag | 13:a3d4b4daf5b4 | 207 | { |
MarijkeZondag | 13:a3d4b4daf5b4 | 208 | sum = 0.0; |
MarijkeZondag | 13:a3d4b4daf5b4 | 209 | for(int j = 0; j<=sizeCal-1; j++) |
MarijkeZondag | 13:a3d4b4daf5b4 | 210 | { |
MarijkeZondag | 13:a3d4b4daf5b4 | 211 | StoreCal1[j] = emgfilter1; |
MarijkeZondag | 13:a3d4b4daf5b4 | 212 | sum+=StoreCal1[j]; |
MarijkeZondag | 18:898f54c6aa3d | 213 | //wait(0.001f); |
MarijkeZondag | 13:a3d4b4daf5b4 | 214 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 215 | Mean1 = sum/sizeCal; |
MarijkeZondag | 13:a3d4b4daf5b4 | 216 | Threshold1 = Mean1/2; |
MarijkeZondag | 13:a3d4b4daf5b4 | 217 | break; |
MarijkeZondag | 13:a3d4b4daf5b4 | 218 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 219 | case 2: |
MarijkeZondag | 13:a3d4b4daf5b4 | 220 | { |
MarijkeZondag | 13:a3d4b4daf5b4 | 221 | sum = 0.0; |
MarijkeZondag | 13:a3d4b4daf5b4 | 222 | for(int j = 0; j<=sizeCal-1; j++) |
MarijkeZondag | 13:a3d4b4daf5b4 | 223 | { |
MarijkeZondag | 13:a3d4b4daf5b4 | 224 | StoreCal1[j] = emgfilter2; |
MarijkeZondag | 13:a3d4b4daf5b4 | 225 | sum+=StoreCal2[j]; |
MarijkeZondag | 18:898f54c6aa3d | 226 | //wait(0.001f); |
MarijkeZondag | 13:a3d4b4daf5b4 | 227 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 228 | Mean2 = sum/sizeCal; |
MarijkeZondag | 13:a3d4b4daf5b4 | 229 | Threshold2 = Mean2/2; |
MarijkeZondag | 13:a3d4b4daf5b4 | 230 | break; |
MarijkeZondag | 13:a3d4b4daf5b4 | 231 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 232 | case 3: //EMG is calibrated, robot can be set to Home position. |
MarijkeZondag | 13:a3d4b4daf5b4 | 233 | { |
MarijkeZondag | 13:a3d4b4daf5b4 | 234 | emg_cal = 1; |
MarijkeZondag | 18:898f54c6aa3d | 235 | //wait(0.001f); |
MarijkeZondag | 13:a3d4b4daf5b4 | 236 | break; |
MarijkeZondag | 13:a3d4b4daf5b4 | 237 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 238 | default: //Ensures nothing happens if x is not 0,1 or 2. |
MarijkeZondag | 13:a3d4b4daf5b4 | 239 | { |
MarijkeZondag | 13:a3d4b4daf5b4 | 240 | break; |
MarijkeZondag | 13:a3d4b4daf5b4 | 241 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 242 | } |
MarijkeZondag | 12:eaed305a76c3 | 243 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 244 | |
MarijkeZondag | 8:895d941a5910 | 245 | void encoderA_rise() |
MarijkeZondag | 8:895d941a5910 | 246 | { |
MarijkeZondag | 8:895d941a5910 | 247 | if(encoderB==false) |
MarijkeZondag | 8:895d941a5910 | 248 | { |
MarijkeZondag | 8:895d941a5910 | 249 | encoder++; |
MarijkeZondag | 8:895d941a5910 | 250 | } |
MarijkeZondag | 8:895d941a5910 | 251 | else |
MarijkeZondag | 8:895d941a5910 | 252 | { |
MarijkeZondag | 8:895d941a5910 | 253 | encoder--; |
MarijkeZondag | 8:895d941a5910 | 254 | } |
MarijkeZondag | 8:895d941a5910 | 255 | } |
MarijkeZondag | 8:895d941a5910 | 256 | |
MarijkeZondag | 8:895d941a5910 | 257 | void encoderA_fall() |
MarijkeZondag | 8:895d941a5910 | 258 | { |
MarijkeZondag | 8:895d941a5910 | 259 | if(encoderB==true) |
MarijkeZondag | 8:895d941a5910 | 260 | { |
MarijkeZondag | 8:895d941a5910 | 261 | encoder++; |
MarijkeZondag | 8:895d941a5910 | 262 | } |
MarijkeZondag | 8:895d941a5910 | 263 | else |
MarijkeZondag | 8:895d941a5910 | 264 | { |
MarijkeZondag | 8:895d941a5910 | 265 | encoder--; |
MarijkeZondag | 8:895d941a5910 | 266 | } |
MarijkeZondag | 8:895d941a5910 | 267 | } |
MarijkeZondag | 8:895d941a5910 | 268 | |
MarijkeZondag | 8:895d941a5910 | 269 | void encoderB_rise() |
MarijkeZondag | 8:895d941a5910 | 270 | { |
MarijkeZondag | 8:895d941a5910 | 271 | if(encoderA==true) |
MarijkeZondag | 8:895d941a5910 | 272 | { |
MarijkeZondag | 8:895d941a5910 | 273 | encoder++; |
MarijkeZondag | 8:895d941a5910 | 274 | } |
MarijkeZondag | 8:895d941a5910 | 275 | else |
MarijkeZondag | 8:895d941a5910 | 276 | { |
MarijkeZondag | 8:895d941a5910 | 277 | encoder--; |
MarijkeZondag | 8:895d941a5910 | 278 | } |
MarijkeZondag | 8:895d941a5910 | 279 | } |
MarijkeZondag | 8:895d941a5910 | 280 | |
MarijkeZondag | 8:895d941a5910 | 281 | void encoderB_fall() |
MarijkeZondag | 8:895d941a5910 | 282 | { |
MarijkeZondag | 8:895d941a5910 | 283 | if(encoderA==false) |
MarijkeZondag | 8:895d941a5910 | 284 | { |
MarijkeZondag | 8:895d941a5910 | 285 | encoder++; |
MarijkeZondag | 8:895d941a5910 | 286 | } |
MarijkeZondag | 8:895d941a5910 | 287 | else |
MarijkeZondag | 8:895d941a5910 | 288 | { |
MarijkeZondag | 8:895d941a5910 | 289 | encoder--; |
MarijkeZondag | 8:895d941a5910 | 290 | } |
MarijkeZondag | 8:895d941a5910 | 291 | } |
MarijkeZondag | 8:895d941a5910 | 292 | |
MarijkeZondag | 8:895d941a5910 | 293 | |
MarijkeZondag | 9:c722418997b5 | 294 | // Main function start. |
MarijkeZondag | 8:895d941a5910 | 295 | |
vsluiter | 0:c8f15874531b | 296 | int main() |
MarijkeZondag | 3:a3ad58758696 | 297 | { |
MarijkeZondag | 13:a3d4b4daf5b4 | 298 | //pc.baud(115200); |
MarijkeZondag | 13:a3d4b4daf5b4 | 299 | //pc.printf("hello\n\r"); |
MarijkeZondag | 19:466ada92bf65 | 300 | sample_timer.attach(&sample, 0.002); |
MarijkeZondag | 10:39ec51206c8b | 301 | |
MarijkeZondag | 17:741798018c0d | 302 | ledr = 0; //Begin led = red, first state of calibration |
MarijkeZondag | 17:741798018c0d | 303 | ledb = 1; |
MarijkeZondag | 17:741798018c0d | 304 | ledg = 1; |
MarijkeZondag | 13:a3d4b4daf5b4 | 305 | |
MarijkeZondag | 13:a3d4b4daf5b4 | 306 | filter_tick.attach(&emg_filtered,T); //EMG signals filtered + moving average every T sec. |
MarijkeZondag | 13:a3d4b4daf5b4 | 307 | button1.rise(switch_to_calibrate); //Switch state of calibration (which muscle) |
MarijkeZondag | 15:be67b090b64a | 308 | wait(0.2f); |
MarijkeZondag | 13:a3d4b4daf5b4 | 309 | button2.rise(calibrate); //calibrate threshold for 3 muscles |
MarijkeZondag | 15:be67b090b64a | 310 | wait(0.2f); |
MarijkeZondag | 13:a3d4b4daf5b4 | 311 | |
MarijkeZondag | 13:a3d4b4daf5b4 | 312 | pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz |
MarijkeZondag | 6:f4bbb73f3989 | 313 | |
MarijkeZondag | 8:895d941a5910 | 314 | encoderA.rise(&encoderA_rise); |
MarijkeZondag | 8:895d941a5910 | 315 | encoderA.fall(&encoderA_fall); |
MarijkeZondag | 8:895d941a5910 | 316 | encoderB.rise(&encoderB_rise); |
MarijkeZondag | 8:895d941a5910 | 317 | encoderB.fall(&encoderB_fall); |
vsluiter | 0:c8f15874531b | 318 | |
MarijkeZondag | 16:5f7196ddc77b | 319 | if(emg_cal==1) |
MarijkeZondag | 16:5f7196ddc77b | 320 | { |
MarijkeZondag | 16:5f7196ddc77b | 321 | while (true) |
MarijkeZondag | 16:5f7196ddc77b | 322 | { |
MarijkeZondag | 16:5f7196ddc77b | 323 | //Motor aansturen en encoder uitlezen |
MarijkeZondag | 16:5f7196ddc77b | 324 | //float u1 = potmetervalue1; |
MarijkeZondag | 16:5f7196ddc77b | 325 | //float u2 = potmetervalue2; |
MarijkeZondag | 16:5f7196ddc77b | 326 | |
MarijkeZondag | 16:5f7196ddc77b | 327 | //float m1 = ((u1*2.0f)-1.0f); |
MarijkeZondag | 16:5f7196ddc77b | 328 | //float m2 = ((u2*2.0f)-1.0f); |
MarijkeZondag | 16:5f7196ddc77b | 329 | |
MarijkeZondag | 16:5f7196ddc77b | 330 | //pwmpin1 = fabs(m1*0.6f)+0.4f; //pwm duty cycle can only be positive, floating, 0.4f is "inefficiënt", dit tellen we erbij op, en keer 0.6 om te corrigeren voor de helling. |
MarijkeZondag | 16:5f7196ddc77b | 331 | |
MarijkeZondag | 16:5f7196ddc77b | 332 | if(emgfilter0>Threshold0) |
MarijkeZondag | 16:5f7196ddc77b | 333 | { |
MarijkeZondag | 16:5f7196ddc77b | 334 | pwmpin1 = 1; |
MarijkeZondag | 16:5f7196ddc77b | 335 | directionpin1.write(1); |
MarijkeZondag | 16:5f7196ddc77b | 336 | } |
MarijkeZondag | 16:5f7196ddc77b | 337 | else |
MarijkeZondag | 16:5f7196ddc77b | 338 | { |
MarijkeZondag | 16:5f7196ddc77b | 339 | pwmpin1 = 0; |
MarijkeZondag | 16:5f7196ddc77b | 340 | } |
MarijkeZondag | 9:c722418997b5 | 341 | |
MarijkeZondag | 16:5f7196ddc77b | 342 | if(emgfilter1>Threshold1) |
MarijkeZondag | 16:5f7196ddc77b | 343 | { |
MarijkeZondag | 16:5f7196ddc77b | 344 | pwmpin2 = 1; |
MarijkeZondag | 16:5f7196ddc77b | 345 | directionpin2.write(1); |
MarijkeZondag | 16:5f7196ddc77b | 346 | } |
MarijkeZondag | 16:5f7196ddc77b | 347 | else |
MarijkeZondag | 16:5f7196ddc77b | 348 | { |
MarijkeZondag | 16:5f7196ddc77b | 349 | pwmpin2 = 0; |
MarijkeZondag | 16:5f7196ddc77b | 350 | } |
MarijkeZondag | 16:5f7196ddc77b | 351 | if(emgfilter2>Threshold2) |
MarijkeZondag | 16:5f7196ddc77b | 352 | { |
MarijkeZondag | 16:5f7196ddc77b | 353 | pwmpin1 = 1; |
MarijkeZondag | 16:5f7196ddc77b | 354 | pwmpin2 = 2; |
MarijkeZondag | 16:5f7196ddc77b | 355 | directionpin1.write(1); |
MarijkeZondag | 16:5f7196ddc77b | 356 | directionpin2.write(1); |
MarijkeZondag | 16:5f7196ddc77b | 357 | } |
MarijkeZondag | 16:5f7196ddc77b | 358 | else |
MarijkeZondag | 16:5f7196ddc77b | 359 | { |
MarijkeZondag | 16:5f7196ddc77b | 360 | pwmpin1 = 0; |
MarijkeZondag | 16:5f7196ddc77b | 361 | pwmpin2 = 0; |
MarijkeZondag | 16:5f7196ddc77b | 362 | } |
MarijkeZondag | 16:5f7196ddc77b | 363 | |
MarijkeZondag | 16:5f7196ddc77b | 364 | //Indien waar, motor draait rechtsom. Indien niet waar, motor draait linksom. |
MarijkeZondag | 16:5f7196ddc77b | 365 | //wait(0.01f); //zodat de code niet oneindig doorgaat. |
MarijkeZondag | 16:5f7196ddc77b | 366 | //pwmpin2 = fabs(m2*0.6f)+0.4f; |
MarijkeZondag | 16:5f7196ddc77b | 367 | //directionpin2.write(m2>0); |
MarijkeZondag | 16:5f7196ddc77b | 368 | |
MarijkeZondag | 16:5f7196ddc77b | 369 | //float encoderDegrees = float(encoder)*(360.0/8400.0); |
MarijkeZondag | 16:5f7196ddc77b | 370 | |
MarijkeZondag | 16:5f7196ddc77b | 371 | //pc.printf("Encoder count: %f \n\r",encoderDegrees); |
MarijkeZondag | 10:39ec51206c8b | 372 | |
vsluiter | 0:c8f15874531b | 373 | } |
vsluiter | 0:c8f15874531b | 374 | } |
MarijkeZondag | 19:466ada92bf65 | 375 | while(1) {} |
MarijkeZondag | 16:5f7196ddc77b | 376 | } |