Marijke Zondag / Mbed 2 deprecated Milestone_sample

Make 2 motors turn

Dependencies:   MODSERIAL mbed HIDScope biquadFilter

Fork of Minor_test_serial by Marijke Zondag

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?

UserRevisionLine numberNew 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 }