Marijke Zondag
..
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)
{