kalibratie stappen project EMG
Dependencies: HIDScope MODSERIAL biquadFilter mbed
Fork of Milestone_sample by
Diff: main.cpp
- Revision:
- 22:91ad5984b2f2
- Parent:
- 21:1da43fdbd254
diff -r 1da43fdbd254 -r 91ad5984b2f2 main.cpp --- a/main.cpp Mon Oct 22 13:44:40 2018 +0000 +++ b/main.cpp Wed Oct 24 19:41:41 2018 +0000 @@ -1,158 +1,51 @@ #include "mbed.h" #include "MODSERIAL.h" -#include "BiQuad.h" -#include "HIDScope.h" #include <math.h> AnalogIn emg0_in (A0); AnalogIn emg1_in (A1); AnalogIn emg2_in (A2); -InterruptIn button1 (D10); //Let op, is deze niet bezet? En 11? Even checken, als er een error komt, kan het hier zitten. +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 test if the code works + -MODSERIAL pc(USBTX, USBRX); - -//HIDscope -Ticker sample_timer; -HIDScope scope( 3 ); - -//Global variables -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 -const int windowsize = 150; //Size of the array over which the moving average (MovAg) is calculated +//EMG filter variables (also necessary for calibration parts) +double emg0_filt, emg1_filt, emg2_filt; //Variables for filtered EMG data channel 0, 1 and 2 +double emg0_raw, emg1_raw, emg2_raw; +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 - -//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; +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 -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 ); +//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 = 2000; //size of the dataset used for calibration +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; //Threshold variables -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 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 +58,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 +82,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 +108,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 +121,81 @@ 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() -{ - if(encoderB==false) - { - encoder++; - } - else - { - encoder--; - } -} - -void encoderA_fall() -{ - if(encoderB==true) - { - encoder++; - } - else - { - encoder--; - } -} - -void encoderB_rise() -{ - if(encoderA==true) - { - encoder++; - } - else - { - encoder--; - } -} - -void encoderB_fall() -{ - if(encoderA==false) - { - encoder++; - } - else - { - encoder--; - } -} - - -// Main function start. + int main() { - //pc.baud(115200); - //pc.printf("hello\n\r"); + pc.baud(115200); + pc.printf("hello\n\r"); //Check does code work - 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) + { + + if(emg0_filt>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); } - else + else //If it is not higher than the threshold, the motor will not turn at all. { pwmpin1 = 0; } - if(emgfilter1>Threshold1) + if(emg1_filt>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); } - else + else //If not higher than the threshold, motor will not turn at all { pwmpin2 = 0; } - if(emgfilter2>Threshold2) + if(emg2_filt>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); } - else + else //If not higher than the threshold, motors will not turn at all { 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); - - //pc.printf("Encoder count: %f \n\r",encoderDegrees); + - } + } + } + + //Opmerkingen Marijke: ik ben niet zeker van waar ik de while loop moet plaatsen. + //Misschien is aan het einde buiten de if beter, schrijf dan while(1){}. + + //De manier van aansturen van de motoren is nu puur om te testen of de kalibratie werkt. Op deze manier kunnen + //We namelijk niet met 2 spieren 2 motoren tegelijk aansturen. } -}