![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
werkt nog niet
Dependencies: HIDScope MODSERIAL biquadFilter mbed QEI
Fork of Project_script_union by
Diff: main.cpp
- Revision:
- 23:97a976a8f0fc
- Parent:
- 22:5d956c93b3ae
- Child:
- 24:6d63ad38fef7
diff -r 5d956c93b3ae -r 97a976a8f0fc main.cpp --- a/main.cpp Mon Oct 29 14:58:25 2018 +0000 +++ b/main.cpp Tue Oct 30 10:37:45 2018 +0000 @@ -17,24 +17,25 @@ InterruptIn button1 (D10); //Is this one available? We need to make a map of which pins are used for what. InterruptIn button2 (D11); -DigitalOut directionpin1 (D4); //Motor direction pin -DigitalOut directionpin2 (D7); +DigitalOut directionpin1 (D7); +DigitalOut directionpin2 (D4); +PwmOut pwmpin1 (D6); +PwmOut pwmpin2 (D5); DigitalOut ledr (LED_RED); DigitalOut ledb (LED_BLUE); DigitalOut ledg (LED_GREEN); -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); //Serial communication to see if the code works step by step, turn on if hidscope is off -HIDScope scope( 6 ); //HIDScope set to 3x2 channels for 3 muscles, raw data + filtered +//HIDScope scope( 6 ); //HIDScope set to 3x2 channels for 3 muscles, raw data + filtered //Tickers Ticker HIDScope_tick; //Ticker for HIDScope Ticker filter_tick; //Ticker for EMG filter Ticker MovAg_tick; //Ticker to calculate Moving Average +Ticker Motor_tick; //Ticker motor aansturen //Global variables const float T = 0.002f; //Ticker period @@ -110,6 +111,7 @@ if(x==4) //Reset back to x = -1 { x = -1; + emg_cal=0; //reset, motors off } } @@ -128,7 +130,7 @@ wait(0.001f); //Does there need to be a wait? } Mean0 = sum/sizeCal; //Calculate mean of the datapoints in the calibration set (2000 samples) - Threshold0 = Mean0/2; //Threshold calculation = 0.5*mean + Threshold0 = Mean0/2; //Threshold calculation = 0.5*mean break; //Stop. Threshold is calculated, we will use this further in the code } case 1: //If calibration state 1: @@ -141,7 +143,7 @@ wait(0.001f); } Mean1 = sum/sizeCal; - Threshold1 = Mean1/2; + Threshold1 = Mean1/2; break; } case 2: //If calibration state 2: @@ -149,12 +151,12 @@ sum = 0.0; for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 2 { - StoreCal1[j] = emg2_filt; + StoreCal2[j] = emg2_filt; sum+=StoreCal2[j]; wait(0.001f); } Mean2 = sum/sizeCal; - Threshold2 = Mean2/2; + Threshold2 = Mean2/2; break; } case 3: //EMG is calibrated, robot can be set to Home position. @@ -173,17 +175,17 @@ 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(0,emg0_raw); //scope.set(1,emg0_filt); - scope.set(1,movAg0); //als moving average werkt - scope.set(2,emg1_raw); + //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(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.set(5,movAg2); //als moving average werkt - scope.send(); //Send data to HIDScope server + //scope.send(); //Send data to HIDScope server } void EMGFilter0() @@ -244,40 +246,18 @@ EMGFilter2(); } - -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 ); - - 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 = 1; //Begin led = blue, press button for first state of calibration --> led will turn red - ledb = 0; - ledg = 1; - - 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 - - if(emg_cal==1) //After calibration is finished, emg_cal will be 1. Otherwise 0. +void motor_control() +{ + while(emg_cal==1) //After calibration is finished, emg_cal will be 1. Otherwise 0. { - //while (true) - // { + // pc.printf("Movag0 = %f , Movag1 = %f, Movag2 = %f \n\r",movAg0, movAg1, movAg2); + // pc.printf("Thresh0 = %f , Thresh1 = %f, Thresh2 = %f \n\r",Threshold0, Threshold1, Threshold2); + // wait(2.0f); 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); + directionpin1.write(1); //translatie vooruit ledr = 1; //Blue ledb = 0; @@ -295,7 +275,7 @@ 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); + directionpin2.write(0); //rotatie omhoog ledr = 0; //red ledb = 1; ledg = 1; @@ -307,29 +287,58 @@ ledb = 0; ledg = 0; } - 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); + // 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 = 1; + // directionpin1.write(0); //translatie achteruit + // directionpin2.write(1); //rotatie omlaag + + //ledr = 1; //green + //ledb = 1; + //ledg = 0; + //} + //else //If not higher than the threshold, motors will not turn at all + //{ + // pwmpin1 = 0; + // pwmpin2 = 0; + + // ledr = 0; //white + // ledb = 0; + // ledg = 0; + //} - ledr = 1; //green - ledb = 1; - ledg = 0; - } - else //If not higher than the threshold, motors will not turn at all - { - pwmpin1 = 0; - pwmpin2 = 0; - - ledr = 0; //white - ledb = 0; - ledg = 0; - } - - + break; + } +} + + +int main() +{ + pc.baud(115200); + pc.printf("Hello World!\r\n"); //Serial communication only works if hidscope is turned off. + pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz + + 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 ); - //} - } + while(true) + { + filter_tick.attach(&emg_filtered,T); //EMG signals filtered every T sec. + MovAg_tick.attach(&MovAg,T); //Moving average calculation every T sec. + Motor_tick.attach(&motor_control,T); //Test motor control + + // HIDScope_tick.attach(&HIDScope_sample,T); //EMG signals raw + filtered to HIDScope 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); + + pc.printf("x is %i\n\r",x); + pc.printf("Movag0 = %f , Movag1 = %f, Movag2 = %f \n\r",movAg0, movAg1, movAg2); + pc.printf("Thresh0 = %f , Thresh1 = %f, Thresh2 = %f \n\r",Threshold0, Threshold1, Threshold2); + //wait(2.0f); + } }