![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
..
Dependencies: HIDScope MODSERIAL biquadFilter mbed
Revision 25:b8e9ee9a1bd8, committed 2018-11-13
- Comitter:
- MarijkeZondag
- Date:
- Tue Nov 13 09:58:42 2018 +0000
- Parent:
- 24:6d63ad38fef7
- Commit message:
- VLD demo 2 spieren, lage snelheid motoren
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Tue Oct 30 13:09:58 2018 +0000 +++ b/main.cpp Tue Nov 13 09:58:42 2018 +0000 @@ -12,7 +12,6 @@ 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); //Is this one available? We need to make a map of which pins are used for what. InterruptIn button2 (D11); @@ -41,21 +40,21 @@ const float T = 0.002f; //Ticker period //EMG filter -double emg0_filt, emg1_filt, emg2_filt; //Variables for filtered EMG data channel 0, 1 and 2 -double emg0_raw, emg1_raw, emg2_raw; -double emg0_filt_x, emg1_filt_x, emg2_filt_x; +double emg0_filt, emg1_filt; //Variables for filtered EMG data channel 0, 1 and 2 +double emg0_raw, emg1_raw; +double emg0_filt_x, emg1_filt_x; 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??) +double sum, sum1, sum2; //variables used to sum elements in array +double StoreArray0[windowsize], StoreArray1[windowsize]; //Empty arrays to calculate MoveAg +double movAg0, movAg1; //outcome of MovAg (moet dit een array zijn??) //Calibration variables int x = -1; //Start switch, colour LED is blue. int emg_cal = 0; //if emg_cal is set to 1, motors can begin to work in this code (!!) const int sizeCal = 1500; //size of the dataset used for calibration, eerst 2000 -double StoreCal0[sizeCal], StoreCal1[sizeCal], StoreCal2[sizeCal]; //arrays to put the dataset of the calibration in -double Mean0,Mean1,Mean2; //average of maximum tightening -double Threshold0, Threshold1, Threshold2; +double StoreCal0[sizeCal], StoreCal1[sizeCal]; //arrays to put the dataset of the calibration in +double Mean0,Mean1; //average of maximum tightening +double Threshold0, Threshold1; //Biquad //Variables for the biquad band filters (alle 3 dezelfde maar je kan niet 3x 'emg0band' aanroepen ofzo) BiQuadChain emg0filter; @@ -70,12 +69,6 @@ BiQuad emg1band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 ); BiQuad notch2( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter -BiQuadChain emg2filter; -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 notch3( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter - //Functions void switch_to_calibrate() @@ -95,12 +88,6 @@ ledb = 0; ledg = 1; } - else if (x==2) //If x = 2, led is green - { - ledr = 1; - ledb = 1; - ledg = 0; - } else //If x = 3 or 4, led is white { ledr = 0; @@ -108,7 +95,7 @@ ledg = 0; } - if(x==4) //Reset back to x = -1 + if(x==3) //Reset back to x = -1 { x = -1; emg_cal=0; //reset, motors off @@ -146,20 +133,7 @@ Threshold1 = Mean1/2; break; } - case 2: //If calibration state 2: - { - sum = 0.0; - for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 2 - { - StoreCal2[j] = emg2_filt; - sum+=StoreCal2[j]; - wait(0.001f); - } - Mean2 = sum/sizeCal; - Threshold2 = Mean2/2; - break; - } - case 3: //EMG is calibrated, robot can be set to Home position. + case 2: //EMG is calibrated, robot can be set to Home position. { emg_cal = 1; //This is the setting for which the motors can begin turning in this code (!!) wait(0.001f); @@ -172,22 +146,6 @@ } } -void HIDScope_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_raw); - //scope.set(1,emg0_filt); - //scope.set(1,movAg0); //als moving average werkt - //scope.set(2,emg1_raw); - //scope.set(3,emg1_filt); - //scope.set(3,movAg1); //als moving average werkt - //scope.set(4,emg2_raw); - //scope.set(5,emg2_filt); - //scope.set(5,movAg2); //als moving average werkt - - //scope.send(); //Send data to HIDScope server -} - void EMGFilter0() { emg0_raw = emg0_in.read(); //give name to raw EMG0 data @@ -201,13 +159,6 @@ emg1_filt_x = emg1filter.step(emg1_raw); //Use biquad chain to filter raw EMG data emg1_filt = abs(emg1_filt_x); //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch. } - -void EMGFilter2() -{ - emg2_raw = emg2_in.read(); //Give name to raw EMG1 data - emg2_filt_x = emg2filter.step(emg2_raw); //Use biquad chain to filter raw EMG data - emg2_filt = abs(emg2_filt_x); //Rectifier. LET OP: volgorde filter: band-notch-rectifier. -} void MovAg() //Calculate moving average (MovAg), klopt nog niet!! { @@ -215,27 +166,22 @@ { StoreArray0[i] = StoreArray0[i-1]; //Shifts the i'th element one place to the right, this makes it "rolling or moving" average. StoreArray1[i] = StoreArray1[i-1]; - StoreArray2[i] = StoreArray2[i-1]; } StoreArray0[0] = emg0_filt; //Stores the latest datapoint of the filtered signal in the first element of the array StoreArray1[0] = emg1_filt; - StoreArray2[0] = emg2_filt; sum1 = 0.0; sum2 = 0.0; - sum3 = 0.0; for(int a = 0; a<= windowsize-1; a++) //Sums the elements in the arrays { 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. } @@ -243,7 +189,6 @@ { EMGFilter0(); EMGFilter1(); - EMGFilter2(); } void motor_control() @@ -253,8 +198,8 @@ if(movAg0>Threshold0) //If the filtered EMG signal of muscle 0 is higher than the threshold, motor1 will turn in 1 direction { - pwmpin1 = 1; - directionpin1.write(1); //translatie vooruit + pwmpin1 = 0.05; + directionpin1.write(1); //rotation down ledr = 1; //Blue ledb = 0; @@ -263,24 +208,12 @@ } else if(movAg1>Threshold1) //If the filtered EMG signal of muscle 1 is higher than the threshold, motor2 will turn in 1 direction { - pwmpin2 = 1; - directionpin2.write(1); //rotatie omhoog + pwmpin2 = 0.05; + directionpin2.write(1); //forward translation ledr = 0; //red ledb = 1; ledg = 1; } - else if(movAg2>Threshold2) //If the filtered EMG signal of muscle 2 is higher than the threshold, motor 1 and 2 will turn - { - - pwmpin1 = 1; - pwmpin2 = 1; - directionpin1.write(0); //translatie achteruit - directionpin2.write(0); //rotatie omlaag - - ledr = 1; //green - ledb = 1; - ledg = 0; - } else //If not higher than the threshold, motors will not turn at all { pwmpin1 = 0; //Motoren doen niets @@ -304,7 +237,6 @@ emg0filter.add( &emg0band1 ).add( &emg0band2 ).add( &emg0band3 ).add( ¬ch1 ); //attach biquad elements to chain emg1filter.add( &emg1band1 ).add( &emg1band2 ).add( &emg1band3 ).add( ¬ch2 ); - emg2filter.add( &emg2band1 ).add( &emg2band2 ).add( &emg2band3 ).add( ¬ch3 ); while(true) {