![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
..
Dependencies: HIDScope MODSERIAL biquadFilter mbed
main.cpp
- Committer:
- MarijkeZondag
- Date:
- 2018-10-22
- Revision:
- 15:be67b090b64a
- Parent:
- 14:fa09dae67390
- Child:
- 16:5f7196ddc77b
File content as of revision 15:be67b090b64a:
#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 (SW2); //Let op, is deze niet bezet? En 11? Even checken, als er een error komt, kan het hier zitten. InterruptIn button2 (SW3); 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); MODSERIAL pc(USBTX, USBRX); //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 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 = 0; 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; //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 ); 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 EMGFilter0() { double emg0 = emg0_in.read(); double bandpass0 = emg0band.step(emg0); double absolute0 = fabs(bandpass0); double notch0 = notch1.step(absolute0); } void EMGFilter1() { double emg1 = emg1_in.read(); double bandpass1 = emg1band.step(emg1); double absolute1 = fabs(bandpass1); double notch1 = notch2.step(absolute1); } void EMGFilter2() { double emg2 = emg2_in.read(); double bandpass2 = emg2band.step(emg2); double absolute2 = fabs(bandpass2); double notch2 = 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++; 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) { 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) { 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"); 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) wait(0.2f); 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); 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) { pwmpin1 = 1; directionpin1.write(1); } else { pwmpin1 = 0; } if(emgfilter1>Threshold1) { pwmpin2 = 1; directionpin2.write(1); } else { pwmpin2 = 0; } if(emgfilter2>Threshold2) { 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); //pc.printf("Encoder count: %f \n\r",encoderDegrees); } }