Marijke Zondag
werkt nog niet
Dependencies: HIDScope MODSERIAL biquadFilter mbed QEI
Fork of
Project_script_union
by 
Marijke Zondag
Diff: main.cpp
- Revision:
- 22:5d956c93b3ae
- Parent:
- 21:1da43fdbd254
- Child:
- 23:97a976a8f0fc
diff -r 1da43fdbd254 -r 5d956c93b3ae main.cpp
--- a/main.cpp Mon Oct 22 13:44:40 2018 +0000
+++ b/main.cpp Mon Oct 29 14:58:25 2018 +0000
@@ -4,155 +4,84 @@
#include "HIDScope.h"
#include <math.h>
-AnalogIn emg0_in (A0);
-AnalogIn emg1_in (A1);
-AnalogIn emg2_in (A2);
+//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
-InterruptIn button1 (D10); //Let op, is deze niet bezet? En 11? Even checken, als er een error komt, kan het hier zitten.
+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);
-InterruptIn encoderA (D9);
-InterruptIn encoderB (D8);
-DigitalOut directionpin1 (D4);
+DigitalOut directionpin1 (D4); //Motor direction pin
DigitalOut directionpin2 (D7);
+
DigitalOut ledr (LED_RED);
DigitalOut ledb (LED_BLUE);
DigitalOut ledg (LED_GREEN);
-PwmOut pwmpin1 (D5);
+PwmOut pwmpin1 (D5); //Pulse width modulation --> speed motor
PwmOut pwmpin2 (D6);
+//MODSERIAL pc(USBTX, USBRX); //Serial communication to see if the code works step by step, turn on if hidscope is off
-MODSERIAL pc(USBTX, USBRX);
+HIDScope scope( 6 ); //HIDScope set to 3x2 channels for 3 muscles, raw data + filtered
-//HIDscope
-Ticker sample_timer;
-HIDScope scope( 3 );
+//Tickers
+Ticker HIDScope_tick; //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
-const float T = 0.001f; //Ticker period
+const float T = 0.002f; //Ticker period
//EMG filter
-double emgfilter0, emgfilter1, emgfilter2; //Filtered EMG data 0, 1 and 2
-const int windowsize = 150; //Size of the array over which the moving average (MovAg) is calculated
+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;
+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
+double StoreArray0[windowsize], StoreArray1[windowsize], StoreArray2[windowsize]; //Empty arrays to calculate MoveAg
+double movAg0, movAg1, movAg2; //outcome of MovAg (moet dit een array zijn??)
-//calibration
-int x = -1;
-int emg_cal = 0;
-const int sizeCal = 2000;
-double StoreCal0[sizeCal], StoreCal1[sizeCal], StoreCal2[sizeCal];
-double Mean0,Mean1,Mean2;
-double Threshold0 = 1, Threshold1 = 1, Threshold2 = 1;
+//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;
-
-//Biquad
-BiQuadChain emg0band;
+//Biquad //Variables for the biquad band filters (alle 3 dezelfde maar je kan niet 3x 'emg0band' aanroepen ofzo)
+BiQuadChain emg0filter;
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 );
+BiQuad notch1( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter biquad coefficients
-BiQuadChain emg1band;
+BiQuadChain emg1filter;
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 );
+BiQuad notch2( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter
-BiQuadChain emg2band;
+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 notch1( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter
-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
-
-//Tickers
-Ticker filter_tick;
-Ticker MovAg_tick;
-
//Functions
-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() );
- scope.set(1, emg1_in.read() );
- scope.set(2, emg2_in.read() );
- /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
- * Ensure that enough channels are available (HIDScope scope( 2 ))
- * Finally, send all channels to the PC at once */
- scope.send();
- /* To indicate that the function is working, the LED is toggled */
-}
-
-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;
-}
-
-void emg_filtered() //Call all filter functions
-{
- EMGFilter0();
- EMGFilter1();
- EMGFilter2();
- MovAg();
-}
void switch_to_calibrate()
{
- x++;
-
+ x++; //Every time function gets called, x increases. Every button press --> new calibration state.
+ //Starts with x = -1. So when function gets called 1 time, x = 0. In the end, x = 4 will reset to -1.
+
if(x==0) //If x = 0, led is red
{
ledr = 0;
@@ -165,20 +94,20 @@
ledb = 0;
ledg = 1;
}
- else if (x == 2) //If x = 2, led is green
+ else if (x==2) //If x = 2, led is green
{
ledr = 1;
ledb = 1;
ledg = 0;
}
- else //If x = 3, led is white
+ else //If x = 3 or 4, led is white
{
ledr = 0;
ledb = 0;
ledg = 0;
}
- if(x==4) //Reset back to x = 0
+ if(x==4) //Reset back to x = -1
{
x = -1;
}
@@ -189,25 +118,25 @@
{
switch(x)
{
- case 0:
+ case 0: //If calibration state 0:
{
sum = 0.0;
- for(int j = 0; j<=sizeCal-1; j++)
+ for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 0
{
- StoreCal0[j] = emgfilter0;
+ StoreCal0[j] = emg0_filt;
sum+=StoreCal0[j];
- wait(0.001f);
+ wait(0.001f); //Does there need to be a wait?
}
- Mean0 = sum/sizeCal;
- Threshold0 = Mean0/2;
- break;
+ Mean0 = sum/sizeCal; //Calculate mean of the datapoints in the calibration set (2000 samples)
+ Threshold0 = Mean0/2; //Threshold calculation = 0.5*mean
+ break; //Stop. Threshold is calculated, we will use this further in the code
}
- case 1:
+ case 1: //If calibration state 1:
{
- sum = 0.0;
- for(int j = 0; j<=sizeCal-1; j++)
+ sum = 0.0;
+ for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 1
{
- StoreCal1[j] = emgfilter1;
+ StoreCal1[j] = emg1_filt;
sum+=StoreCal1[j];
wait(0.001f);
}
@@ -215,12 +144,12 @@
Threshold1 = Mean1/2;
break;
}
- case 2:
+ case 2: //If calibration state 2:
{
sum = 0.0;
- for(int j = 0; j<=sizeCal-1; j++)
+ for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 2
{
- StoreCal1[j] = emgfilter2;
+ StoreCal1[j] = emg2_filt;
sum+=StoreCal2[j];
wait(0.001f);
}
@@ -228,148 +157,179 @@
Threshold2 = Mean2/2;
break;
}
- case 3: //EMG is calibrated, robot can be set to Home position.
+ case 3: //EMG is calibrated, robot can be set to Home position.
{
- emg_cal = 1;
+ emg_cal = 1; //This is the setting for which the motors can begin turning in this code (!!)
wait(0.001f);
break;
}
- default: //Ensures nothing happens if x is not 0,1 or 2.
+ default: //Ensures nothing happens if x is not 0,1 or 2.
{
break;
}
}
}
-
-void encoderA_rise()
+
+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
+ emg0_filt_x = emg0filter.step(emg0_raw); //Use biquad chain to filter raw EMG data
+ emg0_filt = abs(emg0_filt_x); //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch, stel er komt iets raars uit, dan Notch uit de biquad chain halen en aparte chain voor aanmaken.
+}
+
+void EMGFilter1()
{
- if(encoderB==false)
- {
- encoder++;
- }
- else
- {
- encoder--;
- }
+ emg1_raw = emg1_in.read(); //give name to raw EMG1 data
+ 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 encoderA_fall()
+void EMGFilter2()
{
- if(encoderB==true)
+ 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!!
+{
+ for (int i = windowsize-1; i>=0; i--) //Make arrays for the last datapoints of the filtered signals
{
- encoder++;
+ 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];
}
- else
+
+ 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
{
- encoder--;
+ 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 encoderB_rise()
+void emg_filtered() //Call all filter functions
{
- if(encoderA==true)
- {
- encoder++;
- }
- else
- {
- encoder--;
- }
-}
-
-void encoderB_fall()
-{
- if(encoderA==false)
- {
- encoder++;
- }
- else
- {
- encoder--;
- }
+ EMGFilter0();
+ EMGFilter1();
+ EMGFilter2();
}
-// Main function start.
+int main()
+{
+ //pc.baud(115200);
+ //pc.printf("Hello World!\r\n"); //Serial communication only works if hidscope is turned off.
+
+ 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 );
-int main()
-{
- //pc.baud(115200);
- //pc.printf("hello\n\r");
+ filter_tick.attach(&emg_filtered,T); //EMG signals filtered every T sec.
+ MovAg_tick.attach(&MovAg,T); //Moving average calculation every T sec.
+ HIDScope_tick.attach(&HIDScope_sample,T); //EMG signals raw + filtered to HIDScope every T sec.
- ledr = 0; //Begin led = red, first state of calibration
- ledb = 1;
+ ledr = 1; //Begin led = blue, press button for first state of calibration --> led will turn red
+ ledb = 0;
ledg = 1;
-
- sample_timer.attach(&sample, 0.002); //HIDscope
-
- filter_tick.attach(&emg_filtered,T); //EMG signals filtered + moving average every T sec.
+
button1.rise(switch_to_calibrate); //Switch state of calibration (which muscle)
wait(0.2f);
- button2.rise(calibrate); //calibrate threshold for 3 muscles
+ button2.rise(calibrate); //Calibrate threshold for 3 muscles
wait(0.2f);
pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz
-
- encoderA.rise(&encoderA_rise);
- encoderA.fall(&encoderA_fall);
- encoderB.rise(&encoderB_rise);
- encoderB.fall(&encoderB_fall);
- if(emg_cal==1)
+ if(emg_cal==1) //After calibration is finished, emg_cal will be 1. Otherwise 0.
{
- while (true)
- {
- //Motor aansturen en encoder uitlezen
- //float u1 = potmetervalue1;
- //float u2 = potmetervalue2;
-
- //float m1 = ((u1*2.0f)-1.0f);
- //float m2 = ((u2*2.0f)-1.0f);
-
- //pwmpin1 = fabs(m1*0.6f)+0.4f; //pwm duty cycle can only be positive, floating, 0.4f is "inefficiënt", dit tellen we erbij op, en keer 0.6 om te corrigeren voor de helling.
-
- if(emgfilter0>Threshold0)
+ //while (true)
+ // {
+
+ 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);
+
+ ledr = 1; //Blue
+ ledb = 0;
+ ledg = 1;
+
}
- else
+ else //If it is not higher than the threshold, the motor will not turn at all.
{
pwmpin1 = 0;
+ ledr = 0; //white
+ ledb = 0;
+ ledg = 0;
}
- if(emgfilter1>Threshold1)
+ 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);
+ ledr = 0; //red
+ ledb = 1;
+ ledg = 1;
}
- else
+ else //If not higher than the threshold, motor will not turn at all
{
pwmpin2 = 0;
+ ledr = 0; //white
+ ledb = 0;
+ ledg = 0;
}
- if(emgfilter2>Threshold2)
+ 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 = 2;
directionpin1.write(1);
directionpin2.write(1);
+
+ ledr = 1; //green
+ ledb = 1;
+ ledg = 0;
}
- else
+ else //If not higher than the threshold, motors will not turn at all
{
pwmpin1 = 0;
pwmpin2 = 0;
+
+ ledr = 0; //white
+ ledb = 0;
+ ledg = 0;
}
- //Indien waar, motor draait rechtsom. Indien niet waar, motor draait linksom.
- //wait(0.01f); //zodat de code niet oneindig doorgaat.
- //pwmpin2 = fabs(m2*0.6f)+0.4f;
- //directionpin2.write(m2>0);
-
- //float encoderDegrees = float(encoder)*(360.0/8400.0);
-
- //pc.printf("Encoder count: %f \n\r",encoderDegrees);
+
- }
+ //}
+ }
}
-}