Make 2 motors turn
Dependencies: MODSERIAL mbed HIDScope biquadFilter
Fork of Minor_test_serial by
main.cpp@22:8e61050064a9, 2018-10-22 (annotated)
- Committer:
- MarijkeZondag
- Date:
- Mon Oct 22 17:03:36 2018 +0000
- Revision:
- 22:8e61050064a9
- Parent:
- 21:1da43fdbd254
comments;
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 | 22:8e61050064a9 | 7 | //ATTENTION: set mBed to version 151 |
MarijkeZondag | 22:8e61050064a9 | 8 | // set QEI to version 0, (gebruiken wij (nog) niet, is voor encoder) |
MarijkeZondag | 22:8e61050064a9 | 9 | // set MODSERIAL to version 44 |
MarijkeZondag | 22:8e61050064a9 | 10 | // set HIDScope to version 7 |
MarijkeZondag | 22:8e61050064a9 | 11 | // set biquadFilter to version 7 |
MarijkeZondag | 22:8e61050064a9 | 12 | |
MarijkeZondag | 22:8e61050064a9 | 13 | AnalogIn emg0_in (A0); //First raw EMG signal input |
MarijkeZondag | 22:8e61050064a9 | 14 | AnalogIn emg1_in (A1); //Second raw EMG signal input |
MarijkeZondag | 22:8e61050064a9 | 15 | AnalogIn emg2_in (A2); //Third raw EMG signal input |
MarijkeZondag | 10:39ec51206c8b | 16 | |
MarijkeZondag | 16:5f7196ddc77b | 17 | InterruptIn button1 (D10); //Let op, is deze niet bezet? En 11? Even checken, als er een error komt, kan het hier zitten. |
MarijkeZondag | 22:8e61050064a9 | 18 | InterruptIn button2 (D11); //Buttons for switch calibration states |
MarijkeZondag | 6:f4bbb73f3989 | 19 | |
MarijkeZondag | 22:8e61050064a9 | 20 | DigitalOut ledr (LED_RED); //LEDs to show in which state you are in |
MarijkeZondag | 17:741798018c0d | 21 | DigitalOut ledb (LED_BLUE); |
MarijkeZondag | 17:741798018c0d | 22 | DigitalOut ledg (LED_GREEN); |
MarijkeZondag | 13:a3d4b4daf5b4 | 23 | |
MarijkeZondag | 22:8e61050064a9 | 24 | MODSERIAL pc(USBTX, USBRX); //Serial communication to see if the code works step by step |
MarijkeZondag | 9:c722418997b5 | 25 | |
MarijkeZondag | 22:8e61050064a9 | 26 | HIDScope scope( 3 ); //HIDScope set to 3 channels for 3 muscles |
vsluiter | 0:c8f15874531b | 27 | |
MarijkeZondag | 19:466ada92bf65 | 28 | //HIDscope |
MarijkeZondag | 22:8e61050064a9 | 29 | Ticker sample_timer; //Ticker for HIDScope |
MarijkeZondag | 22:8e61050064a9 | 30 | Ticker filter_tick; //Ticker for EMG filter |
MarijkeZondag | 22:8e61050064a9 | 31 | Ticker MovAg_tick; //Ticker to calculate Moving Average |
MarijkeZondag | 9:c722418997b5 | 32 | |
MarijkeZondag | 9:c722418997b5 | 33 | //Global variables |
MarijkeZondag | 22:8e61050064a9 | 34 | int encoder = 0; //Starting point encoder (wordt nu nog niet gebruikt in de code) |
MarijkeZondag | 22:8e61050064a9 | 35 | const float T = 0.001f; //Ticker period |
MarijkeZondag | 10:39ec51206c8b | 36 | |
MarijkeZondag | 13:a3d4b4daf5b4 | 37 | //EMG filter |
MarijkeZondag | 22:8e61050064a9 | 38 | double emgfilter0, emgfilter1, emgfilter2; //Variables for filtered EMG data channel 0, 1 and 2 |
MarijkeZondag | 22:8e61050064a9 | 39 | const int windowsize = 150; //Size of the array over which the moving average (MovAg) is calculated. (random number) |
MarijkeZondag | 13:a3d4b4daf5b4 | 40 | double sum, sum1, sum2, sum3; //variables used to sum elements in array |
MarijkeZondag | 22:8e61050064a9 | 41 | double StoreArray0[windowsize], StoreArray1[windowsize], StoreArray2[windowsize]; //Empty arrays to calculate MoveAg |
MarijkeZondag | 22:8e61050064a9 | 42 | double movAg0,movAg1,movAg2; //outcome of MovAg (moet dit een array zijn??) |
MarijkeZondag | 13:a3d4b4daf5b4 | 43 | |
MarijkeZondag | 22:8e61050064a9 | 44 | //Biquad //Variables for the biquad band filters (alle 3 dezelfde maar je kan niet 3x 'emg0band' aanroepen ofzo) |
MarijkeZondag | 10:39ec51206c8b | 45 | BiQuadChain emg0band; |
MarijkeZondag | 10:39ec51206c8b | 46 | BiQuad emg0band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 47 | BiQuad emg0band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 48 | BiQuad emg0band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 49 | |
MarijkeZondag | 10:39ec51206c8b | 50 | BiQuadChain emg1band; |
MarijkeZondag | 10:39ec51206c8b | 51 | BiQuad emg1band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 52 | BiQuad emg1band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 53 | BiQuad emg1band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 54 | |
MarijkeZondag | 10:39ec51206c8b | 55 | BiQuadChain emg2band; |
MarijkeZondag | 10:39ec51206c8b | 56 | BiQuad emg2band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 57 | BiQuad emg2band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 58 | BiQuad emg2band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 59 | |
MarijkeZondag | 22:8e61050064a9 | 60 | BiQuad notch1( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter biquad coefficients |
MarijkeZondag | 14:fa09dae67390 | 61 | BiQuad notch2( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter |
MarijkeZondag | 14:fa09dae67390 | 62 | BiQuad notch3( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter |
MarijkeZondag | 10:39ec51206c8b | 63 | |
MarijkeZondag | 22:8e61050064a9 | 64 | //Functions |
MarijkeZondag | 10:39ec51206c8b | 65 | |
MarijkeZondag | 22:8e61050064a9 | 66 | //HIDScope connection |
MarijkeZondag | 19:466ada92bf65 | 67 | void sample() |
MarijkeZondag | 19:466ada92bf65 | 68 | { |
MarijkeZondag | 19:466ada92bf65 | 69 | /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */ |
MarijkeZondag | 22:8e61050064a9 | 70 | scope.set(0, emg0_in.read() ); //Raw EMG plot |
MarijkeZondag | 19:466ada92bf65 | 71 | scope.set(1, emg1_in.read() ); |
MarijkeZondag | 21:1da43fdbd254 | 72 | scope.set(2, emg2_in.read() ); |
MarijkeZondag | 22:8e61050064a9 | 73 | |
MarijkeZondag | 22:8e61050064a9 | 74 | //scope.set(0, movAg0.read() ); Werkt niet!! Hoe dan wel moving average in HIDScope?? |
MarijkeZondag | 22:8e61050064a9 | 75 | |
MarijkeZondag | 22:8e61050064a9 | 76 | scope.send(); //Send data to HIDScope server |
MarijkeZondag | 19:466ada92bf65 | 77 | } |
MarijkeZondag | 19:466ada92bf65 | 78 | |
MarijkeZondag | 10:39ec51206c8b | 79 | void EMGFilter0() |
MarijkeZondag | 10:39ec51206c8b | 80 | { |
MarijkeZondag | 11:b95b0e9e1b89 | 81 | double emg0 = emg0_in.read(); |
MarijkeZondag | 11:b95b0e9e1b89 | 82 | double bandpass0 = emg0band.step(emg0); |
MarijkeZondag | 11:b95b0e9e1b89 | 83 | double absolute0 = fabs(bandpass0); |
MarijkeZondag | 21:1da43fdbd254 | 84 | double emgfilter0 = notch1.step(absolute0); |
MarijkeZondag | 10:39ec51206c8b | 85 | } |
MarijkeZondag | 10:39ec51206c8b | 86 | |
MarijkeZondag | 10:39ec51206c8b | 87 | void EMGFilter1() |
MarijkeZondag | 10:39ec51206c8b | 88 | { |
MarijkeZondag | 11:b95b0e9e1b89 | 89 | double emg1 = emg1_in.read(); |
MarijkeZondag | 11:b95b0e9e1b89 | 90 | double bandpass1 = emg1band.step(emg1); |
MarijkeZondag | 11:b95b0e9e1b89 | 91 | double absolute1 = fabs(bandpass1); |
MarijkeZondag | 21:1da43fdbd254 | 92 | double emgfilter1 = notch2.step(absolute1); |
MarijkeZondag | 10:39ec51206c8b | 93 | } |
MarijkeZondag | 10:39ec51206c8b | 94 | |
MarijkeZondag | 10:39ec51206c8b | 95 | void EMGFilter2() |
MarijkeZondag | 10:39ec51206c8b | 96 | { |
MarijkeZondag | 11:b95b0e9e1b89 | 97 | double emg2 = emg2_in.read(); |
MarijkeZondag | 11:b95b0e9e1b89 | 98 | double bandpass2 = emg2band.step(emg2); |
MarijkeZondag | 11:b95b0e9e1b89 | 99 | double absolute2 = fabs(bandpass2); |
MarijkeZondag | 21:1da43fdbd254 | 100 | double emgfilter2 = notch3.step(absolute2); |
MarijkeZondag | 10:39ec51206c8b | 101 | } |
MarijkeZondag | 10:39ec51206c8b | 102 | |
MarijkeZondag | 13:a3d4b4daf5b4 | 103 | void MovAg() //Calculate moving average (MovAg) |
MarijkeZondag | 13:a3d4b4daf5b4 | 104 | { |
MarijkeZondag | 13:a3d4b4daf5b4 | 105 | for (int i = windowsize-1; i>=0; i--) //Make array of the last datapoints of the filtered signal |
MarijkeZondag | 13:a3d4b4daf5b4 | 106 | { |
MarijkeZondag | 13:a3d4b4daf5b4 | 107 | StoreArray0[i] = StoreArray0[i-1]; |
MarijkeZondag | 13:a3d4b4daf5b4 | 108 | StoreArray1[i] = StoreArray1[i-1]; |
MarijkeZondag | 13:a3d4b4daf5b4 | 109 | StoreArray2[i] = StoreArray2[i-1]; |
MarijkeZondag | 13:a3d4b4daf5b4 | 110 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 111 | |
MarijkeZondag | 13:a3d4b4daf5b4 | 112 | StoreArray0[0] = emgfilter0; //Stores the latest datapoint in the first element of the array |
MarijkeZondag | 13:a3d4b4daf5b4 | 113 | StoreArray1[0] = emgfilter1; |
MarijkeZondag | 13:a3d4b4daf5b4 | 114 | StoreArray2[0] = emgfilter2; |
MarijkeZondag | 13:a3d4b4daf5b4 | 115 | |
MarijkeZondag | 13:a3d4b4daf5b4 | 116 | sum1 = 0.0; |
MarijkeZondag | 13:a3d4b4daf5b4 | 117 | sum2 = 0.0; |
MarijkeZondag | 13:a3d4b4daf5b4 | 118 | sum3 = 0.0; |
MarijkeZondag | 13:a3d4b4daf5b4 | 119 | |
MarijkeZondag | 13:a3d4b4daf5b4 | 120 | for(int a = 0; a<= windowsize-1; a++) //Sum the elements in the array |
MarijkeZondag | 13:a3d4b4daf5b4 | 121 | { |
MarijkeZondag | 13:a3d4b4daf5b4 | 122 | sum1 += StoreArray0[a]; |
MarijkeZondag | 13:a3d4b4daf5b4 | 123 | sum2 += StoreArray1[a]; |
MarijkeZondag | 13:a3d4b4daf5b4 | 124 | sum3 += StoreArray2[a]; |
MarijkeZondag | 13:a3d4b4daf5b4 | 125 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 126 | |
MarijkeZondag | 13:a3d4b4daf5b4 | 127 | movAg0 = sum1/windowsize; //calculates an average in the array |
MarijkeZondag | 13:a3d4b4daf5b4 | 128 | movAg1 = sum2/windowsize; |
MarijkeZondag | 14:fa09dae67390 | 129 | movAg2 = sum3/windowsize; |
MarijkeZondag | 22:8e61050064a9 | 130 | //serial getallen sturen, als het 1 getal is gaat hier wat fout, als het een reeks is dan gaat er bij de input naar HIDscope wat fout. |
MarijkeZondag | 13:a3d4b4daf5b4 | 131 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 132 | |
MarijkeZondag | 13:a3d4b4daf5b4 | 133 | void emg_filtered() //Call all filter functions |
MarijkeZondag | 12:eaed305a76c3 | 134 | { |
MarijkeZondag | 12:eaed305a76c3 | 135 | EMGFilter0(); |
MarijkeZondag | 12:eaed305a76c3 | 136 | EMGFilter1(); |
MarijkeZondag | 12:eaed305a76c3 | 137 | EMGFilter2(); |
MarijkeZondag | 13:a3d4b4daf5b4 | 138 | MovAg(); |
MarijkeZondag | 12:eaed305a76c3 | 139 | } |
MarijkeZondag | 12:eaed305a76c3 | 140 | |
MarijkeZondag | 9:c722418997b5 | 141 | // Main function start. |
MarijkeZondag | 8:895d941a5910 | 142 | |
vsluiter | 0:c8f15874531b | 143 | int main() |
MarijkeZondag | 22:8e61050064a9 | 144 | { |
MarijkeZondag | 22:8e61050064a9 | 145 | pc.baud(115200); |
MarijkeZondag | 22:8e61050064a9 | 146 | pc.printf("hello\n\r"); |
MarijkeZondag | 22:8e61050064a9 | 147 | |
MarijkeZondag | 22:8e61050064a9 | 148 | while(true) |
MarijkeZondag | 22:8e61050064a9 | 149 | { |
MarijkeZondag | 6:f4bbb73f3989 | 150 | |
MarijkeZondag | 22:8e61050064a9 | 151 | sample_timer.attach(&sample, 0.002); |
MarijkeZondag | 22:8e61050064a9 | 152 | filter_tick.attach(&emg_filtered,T); //EMG signals filtered + moving average every T sec. |
MarijkeZondag | 22:8e61050064a9 | 153 | pc.printf("\n\r Moving average EMG 3 is: %d \n\r",movAg2); |
MarijkeZondag | 10:39ec51206c8b | 154 | |
MarijkeZondag | 22:8e61050064a9 | 155 | } |
vsluiter | 0:c8f15874531b | 156 | } |