![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
encoder
Dependencies: MODSERIAL mbed HIDScope biquadFilter
Fork of Project_script by
Diff: main.cpp
- Revision:
- 13:a3d4b4daf5b4
- Parent:
- 12:eaed305a76c3
- Child:
- 14:fa09dae67390
--- 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); } }