Make 2 motors turn
Dependencies: MODSERIAL mbed HIDScope biquadFilter
Fork of Minor_test_serial by
main.cpp
- Committer:
- MarijkeZondag
- Date:
- 2018-10-22
- Revision:
- 22:8e61050064a9
- Parent:
- 21:1da43fdbd254
File content as of revision 22:8e61050064a9:
#include "mbed.h" #include "MODSERIAL.h" #include "BiQuad.h" #include "HIDScope.h" #include <math.h> //ATTENTION: set mBed to version 151 // set QEI to version 0, (gebruiken wij (nog) niet, is voor encoder) // set MODSERIAL to version 44 // set HIDScope to version 7 // set biquadFilter to version 7 AnalogIn emg0_in (A0); //First raw EMG signal input AnalogIn emg1_in (A1); //Second raw EMG signal input AnalogIn emg2_in (A2); //Third raw EMG signal input InterruptIn button1 (D10); //Let op, is deze niet bezet? En 11? Even checken, als er een error komt, kan het hier zitten. InterruptIn button2 (D11); //Buttons for switch calibration states DigitalOut ledr (LED_RED); //LEDs to show in which state you are in DigitalOut ledb (LED_BLUE); DigitalOut ledg (LED_GREEN); MODSERIAL pc(USBTX, USBRX); //Serial communication to see if the code works step by step HIDScope scope( 3 ); //HIDScope set to 3 channels for 3 muscles //HIDscope Ticker sample_timer; //Ticker for HIDScope Ticker filter_tick; //Ticker for EMG filter Ticker MovAg_tick; //Ticker to calculate Moving Average //Global variables int encoder = 0; //Starting point encoder (wordt nu nog niet gebruikt in de code) const float T = 0.001f; //Ticker period //EMG filter double emgfilter0, emgfilter1, emgfilter2; //Variables for filtered EMG data channel 0, 1 and 2 const int windowsize = 150; //Size of the array over which the moving average (MovAg) is calculated. (random number) double sum, sum1, sum2, sum3; //variables used to sum elements in array double StoreArray0[windowsize], StoreArray1[windowsize], StoreArray2[windowsize]; //Empty arrays to calculate MoveAg double movAg0,movAg1,movAg2; //outcome of MovAg (moet dit een array zijn??) //Biquad //Variables for the biquad band filters (alle 3 dezelfde maar je kan niet 3x 'emg0band' aanroepen ofzo) BiQuadChain emg0band; BiQuad emg0band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 ); BiQuad emg0band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 ); BiQuad emg0band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 ); BiQuadChain emg1band; BiQuad emg1band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 ); BiQuad emg1band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 ); BiQuad emg1band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 ); BiQuadChain emg2band; BiQuad emg2band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 ); BiQuad emg2band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 ); BiQuad emg2band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 ); BiQuad notch1( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter biquad coefficients BiQuad notch2( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter BiQuad notch3( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter //Functions //HIDScope connection void sample() { /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */ scope.set(0, emg0_in.read() ); //Raw EMG plot scope.set(1, emg1_in.read() ); scope.set(2, emg2_in.read() ); //scope.set(0, movAg0.read() ); Werkt niet!! Hoe dan wel moving average in HIDScope?? scope.send(); //Send data to HIDScope server } void EMGFilter0() { double emg0 = emg0_in.read(); double bandpass0 = emg0band.step(emg0); double absolute0 = fabs(bandpass0); double emgfilter0 = notch1.step(absolute0); } void EMGFilter1() { double emg1 = emg1_in.read(); double bandpass1 = emg1band.step(emg1); double absolute1 = fabs(bandpass1); double emgfilter1 = notch2.step(absolute1); } void EMGFilter2() { double emg2 = emg2_in.read(); double bandpass2 = emg2band.step(emg2); double absolute2 = fabs(bandpass2); double emgfilter2 = notch3.step(absolute2); } void MovAg() //Calculate moving average (MovAg) { for (int i = windowsize-1; i>=0; i--) //Make array of the last datapoints of the filtered signal { StoreArray0[i] = StoreArray0[i-1]; StoreArray1[i] = StoreArray1[i-1]; StoreArray2[i] = StoreArray2[i-1]; } StoreArray0[0] = emgfilter0; //Stores the latest datapoint in the first element of the array StoreArray1[0] = emgfilter1; StoreArray2[0] = emgfilter2; sum1 = 0.0; sum2 = 0.0; sum3 = 0.0; for(int a = 0; a<= windowsize-1; a++) //Sum the elements in the array { sum1 += StoreArray0[a]; sum2 += StoreArray1[a]; sum3 += StoreArray2[a]; } movAg0 = sum1/windowsize; //calculates an average in the array movAg1 = sum2/windowsize; movAg2 = sum3/windowsize; //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. } void emg_filtered() //Call all filter functions { EMGFilter0(); EMGFilter1(); EMGFilter2(); MovAg(); } // Main function start. int main() { pc.baud(115200); pc.printf("hello\n\r"); while(true) { sample_timer.attach(&sample, 0.002); filter_tick.attach(&emg_filtered,T); //EMG signals filtered + moving average every T sec. pc.printf("\n\r Moving average EMG 3 is: %d \n\r",movAg2); } }