Marijke Zondag
..
Dependencies: HIDScope MODSERIAL biquadFilter mbed
Diff: main.cpp
- Revision:
- 13:a3d4b4daf5b4
- Parent:
- 12:eaed305a76c3
- Child:
- 14:fa09dae67390
diff -r eaed305a76c3 -r a3d4b4daf5b4 main.cpp
--- a/main.cpp Fri Oct 19 14:39:35 2018 +0000
+++ b/main.cpp Mon Oct 22 08:35:21 2018 +0000
@@ -8,12 +8,17 @@
AnalogIn emg1_in (A1);
AnalogIn emg2_in (A2);
-DigitalIn button2 (D10); //Let op, is deze niet bezet?
+DigitalIn button1 (D10); //Let op, is deze niet bezet? En 11? Even checken, als er een error komt, kan het hier zitten.
+DigitalIn button2 (D11);
InterruptIn encoderA (D9);
InterruptIn encoderB (D8);
DigitalOut directionpin1 (D4);
DigitalOut directionpin2 (D7);
+DigitalOut led1 (LED_RED);
+DigitalOut led2 (LED_BLUE);
+DigitalOut led3 (LED_GREEN);
+
PwmOut pwmpin1 (D5);
PwmOut pwmpin2 (D6);
@@ -22,9 +27,25 @@
//Global variables
-int encoder = 0;
+int encoder = 0; //Starting point encoder
const float T = 0.001f; //Ticker period
+//EMG filter
+double emgfilter0, emgfilter1, emgfilter2; //Filtered EMG data 0, 1 and 2
+double windowsize = 150; //Size of the array over which the moving average (MovAg) is calculated
+double sum, sum1, sum2, sum3; //variables used to sum elements in array
+double StoreArray0[sizeMovAg] = [], StoreArray1[sizeMoveAg] = [], StoreArray2[sizeMoveAg] = []; //Empty arrays to calculate MoveAg
+double movAg0,movAg1,movAg2; //outcome of MovAg
+
+//calibration
+int x = 0;
+int emg_cal = 0;
+const int sizeCal = 2000;
+double storeCal0[sizeCal] = [], storeCal1[sizeCal] = [], storeCal2[sizeCal] = [];
+double meanCal0,meanCal1,meanCal2;
+double Threshold0 = 1, Threshold1 = 1, Threshold2 = 1;
+
+
//Biquad
BiQuadChain emg0band;
BiQuad emg0band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
@@ -73,18 +94,135 @@
double notch2 = notch1.step(absolute2);
}
-void emg_filtered() //call all filter functions
+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/sizeMovAg;
+}
+
+void emg_filtered() //Call all filter functions
{
EMGFilter0();
EMGFilter1();
EMGFilter2();
+ MovAg();
}
-void MovAg() //calculate moving average
+void switch_to_calibrate()
+{
+ x++;
+
+ if(x==0) //If x = 0, led is red
+ {
+ led1 = 0;
+ led2 = 1;
+ led3 = 1;
+ }
+ else if (x==1) //If x = 1, led is blue
+ {
+ led1 = 1;
+ led2 = 0;
+ led3 = 1;
+ }
+ else if (x == 2) //If x = 2, led is green
+ {
+ led1 = 1;
+ led2 = 1;
+ led3 = 0;
+ }
+ else //If x > 3, led is white
+ {
+ led1 = 0;
+ led2 = 0;
+ led3 = 0;
+ }
+
+ if(x>=4) //Reset back to x = 0
+ {
+ x = 0;
+ }
+}
+
+
+void calibrate(void)
{
- for i =
+ switch(x)
+ {
+ case 0:
+ {
+ sum = 0.0;
+ for(int j = 0; j<=sizeCal-1; j++)
+ {
+ StoreCal0[j] = emgfilter0;
+ sum+=StoreCal0[j];
+ wait(0.001f);
+ }
+ Mean0 = sum/sizeCal;
+ Threshold0 = Mean0/2;
+ break;
+ }
+ case 1:
+ {
+ sum = 0.0;
+ for(int j = 0; j<=sizeCal-1; j++)
+ {
+ StoreCal1[j] = emgfilter1;
+ sum+=StoreCal1[j];
+ wait(0.001f);
+ }
+ Mean1 = sum/sizeCal;
+ Threshold1 = Mean1/2;
+ break;
+ }
+ case 2:
+ {
+ sum = 0.0;
+ for(int j = 0; j<=sizeCal-1; j++)
+ {
+ StoreCal1[j] = emgfilter2;
+ 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.
+ {
+ emg_cal = 1;
+ wait(0.001f);
+ break;
+ }
+ default: //Ensures nothing happens if x is not 0,1 or 2.
+ {
+ break;
+ }
+ }
}
-
+
void encoderA_rise()
{
if(encoderB==false)
@@ -138,18 +276,18 @@
int main()
{
- pc.baud(115200);
- pc.printf("hello\n\r");
+ //pc.baud(115200);
+ //pc.printf("hello\n\r");
- //EMG signaal filteren
-
- filter_tick.attach(&emg_filtered,T); //EMG signals filtered every T sec.
- MovAg_tick.attach(&MovAg,T); //Moving average calculation every T sec.
-
- while(true){/*do not return from main()*/}
-
-
- pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz
+ led1 = 0; //Begin led = red, first state of calibration
+ led2 = 1;
+ led3 = 1;
+
+ 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)
+ button2.rise(calibrate); //calibrate threshold for 3 muscles
+
+ pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz
encoderA.rise(&encoderA_rise);
encoderA.fall(&encoderA_fall);
@@ -159,21 +297,54 @@
while (true)
{
//Motor aansturen en encoder uitlezen
- float u1 = potmetervalue1;
- float u2 = potmetervalue2;
+ //float u1 = potmetervalue1;
+ //float u2 = potmetervalue2;
- float m1 = ((u1*2.0f)-1.0f);
- float m2 = ((u2*2.0f)-1.0f);
+ //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)
+ {
+ pwmpin1 = 1;
+ directionpin1.write(1);
+ }
+ else
+ {
+ pwmpin1 = 0;
+ }
- 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.
- directionpin1.write(m1>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);
+ if(emgfilter1>Threshold1)
+ {
+ pwmpin2 = 1;
+ directionpin2.write(1);
+ }
+ else
+ {
+ pwmpin2 = 0;
+ }
+ if(emgfilter2>Thresheld2)
+ {
+ pwmpin1 = 1;
+ pwmpin2 = 2;
+ directionpin1.write(1);
+ directionpin2.write(1);
+ }
+ else
+ {
+ pwmpin1 = 0;
+ pwmpin2 = 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);
+ //float encoderDegrees = float(encoder)*(360.0/8400.0);
- pc.printf("Encoder count: %f \n\r",encoderDegrees);
+ //pc.printf("Encoder count: %f \n\r",encoderDegrees);
}
}