Yea it is working. done programinging for a while!
Dependencies: HIDScope QEI mbed
Revision 2:e84adf0a960f, committed 2015-11-02
- Comitter:
- yohoo15
- Date:
- Mon Nov 02 15:03:25 2015 +0000
- Parent:
- 1:267ff9a39ee4
- Commit message:
- with more comments
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 267ff9a39ee4 -r e84adf0a960f main.cpp --- a/main.cpp Thu Oct 29 14:29:21 2015 +0000 +++ b/main.cpp Mon Nov 02 15:03:25 2015 +0000 @@ -37,7 +37,7 @@ } - +/*************************************************************Begin Filtering **********************************************************************************************************/ double input_left = 0 ; double input_right = 0 ; //double input = 0; @@ -45,10 +45,10 @@ double filter_signal_hid_right = 0; //double input_right = 0; // defining threshold -double high_threshold = 1000; +double high_threshold = 900; double low_threshold = 125; -//*** making the v for everything and conquer the world*** +//*** making the v for everything and conquer the world *** //for left //for Notchfilter @@ -115,9 +115,6 @@ double n5_b = 0; -double filter_left; -double filter_right; - //general biquad filter that can be called in all the filter functions double biquad(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2) @@ -142,14 +139,7 @@ return average; } -/* -double threshold(double signal, const double lowtreshold, const double hightreshold) -{ - if (signal > hightreshold) - left = true; - else if (signal <lowtreshold) - left = false; -} + */ //Specifying filter coefficients highpass @@ -227,7 +217,6 @@ { input = input-0.45; //FIRST SUBTRACT MEAN THEN FILTER - //input_right = analog_emg_right.read(); // notch filter double y1 = biquad(input, notch_v11, notch_v21, notch1_a1, notch1_a2, notch1_b0, notch1_b1, notch1_b2); @@ -257,7 +246,6 @@ { input_b = input_b-0.45; //FIRST SUBTRACT MEAN THEN FILTER - //input_right = analog_emg_right.read(); // notch filter double y1_b = biquad(input_b, notch_v11_b, notch_v21_b, notch1_a1, notch1_a2, notch1_b0, notch1_b1, notch1_b2); @@ -283,13 +271,17 @@ return(filter_signal_b); } -/*************************************************************BEGIN motor control******************************************************************************************************/ + +/*************************************************************END Filtering **********************************************************************************************************/ +/*-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------*/ +/*************************************************************BEGIN motor control*****************************************************************************************************/ // Define pin for motor control DigitalOut directionPin(D4); PwmOut PWM(D5); DigitalOut directionPin_key(D7); PwmOut PWM_key(D6); -Ticker both_timer; + +Ticker both_timer; // to make a timer to prevent loping of the key press function // define rotation direction and begin possition const int cw = 1; const int ccw = 0; @@ -303,15 +295,11 @@ } -/* -double Rotation = -2; // rotation -double movement = Rotation * pulses_per_revolution; // times 360 to make Rotations degree. -*/ - +/********* the old PI controller ***********/ /* -// Reusable P controller +// Reusable PI controller double PI(double error, const double Kp, const double Ki, double Ts, double &e_int) { @@ -354,12 +342,13 @@ DigitalIn button_song(PTB9); DigitalIn button_do_nothing(PTD1); -//int to check if the button is pressed +//int to check if the button is pressed, 0 is pressed. const int Button_only_key_on = 0; const int Button_demo_on = 0; const int Button_song_on = 0; const int Button_do_nothing_on = 0; +//To say the buttons is not jet pressed. volatile bool only_key = false; volatile bool demo = false ; @@ -384,7 +373,7 @@ both_timer.attach(&Go_flag_both, 2); while(1) { - +/********************* Code for starting filtering ****************/ if(Flag_filteren) { Flag_filteren = false; // filter left and set bool @@ -408,14 +397,14 @@ } - +/********************* Code to look how the EMG/ filtered EMG signal looks like on hidsope ****************/ if(Flag_HIDScope) { Flag_HIDScope = false; HIDScope_kijken(); } - +/********************* Code to switch between modes (demo = left/right and keypress are active; only key = only the key press is active ****************/ if (button_only_key.read() == Button_only_key_on){ demo = false; @@ -430,6 +419,7 @@ pc.printf("demo /n"); } +/********************* Code Demo ****************/ if(demo){ // Pussing buttons to get input signal if( left_movement and right_movement == false) { @@ -446,12 +436,12 @@ if( left_movement and right_movement and flag_both){ directionPin_key.write(cw); PWM_key.write(1); - wait(0.14); + wait(0.05); PWM_key.write(0); - wait(1); + wait(0.3); directionPin_key.write(ccw); PWM_key.write(1); - wait(0.165f); + wait(0.039f); PWM_key.write(0); flag_both = false; } @@ -460,40 +450,42 @@ } } +/********************* Code only key ****************/ + if(only_key){ if(left_movement and flag_both){ directionPin_key.write(cw); PWM_key.write(1); - wait(0.14); + wait(0.05); PWM_key.write(0); - wait(1); + wait(0.3); directionPin_key.write(ccw); PWM_key.write(1); - wait(0.165f); + wait(0.039f); PWM_key.write(0); flag_both = false; } } - +/********************* Code to kill the emg controll to set the system at 'stand by'****************/ if (button_do_nothing.read() == Button_do_nothing_on){ + demo = false; + only_key = false; } +/********************* Code with pre programed feedforward commands to play the first bit of the dutch song 'Vader Jacob'. ****************/ if (button_song.read() == Button_song_on){ //pressing va - directionPin_key.write(cw); - PWM_key.write(1); - wait(0.14); //time it takes pwm of one to reach the key - //pause at the key for one second - PWM_key.write(0); - wait(1); - //move bakc to the starting position - directionPin_key.write(ccw); - PWM_key.write(1); - //time needs to be slightly longer than moving forward (no answer why) - wait(0.165f); - PWM_key.write(0); + directionPin_key.write(cw); + PWM_key.write(1); + wait(0.05); + PWM_key.write(0); + wait(0.3); + directionPin_key.write(ccw); + PWM_key.write(1); + wait(0.039f); + PWM_key.write(0); // go 1 key to right directionPin.write(cw); @@ -503,17 +495,14 @@ //pressing der directionPin_key.write(cw); - PWM_key.write(1); - wait(0.14); //time it takes pwm of one to reach the key - //pause at the key for one second - PWM_key.write(0); - wait(1); - //move bakc to the starting position - directionPin_key.write(ccw); - PWM_key.write(1); - //time needs to be slightly longer than moving forward (no answer why) - wait(0.165f); - PWM_key.write(0); + PWM_key.write(1); + wait(0.05); + PWM_key.write(0); + wait(0.3); + directionPin_key.write(ccw); + PWM_key.write(1); + wait(0.039f); + PWM_key.write(0); // go 1 key to right directionPin.write(cw); @@ -522,19 +511,16 @@ PWM.write(0); //pressing ja - directionPin_key.write(cw); - PWM_key.write(1); - wait(0.14); //time it takes pwm of one to reach the key - //pause at the key for one second - PWM_key.write(0); - wait(1); - //move bakc to the starting position - directionPin_key.write(ccw); - PWM_key.write(1); - //time needs to be slightly longer than moving forward (no answer why) - wait(0.165f); - PWM_key.write(0); - + directionPin_key.write(cw); + PWM_key.write(1); + wait(0.05); + PWM_key.write(0); + wait(0.3); + directionPin_key.write(ccw); + PWM_key.write(1); + wait(0.039f); + PWM_key.write(0); + // go 2 key to left directionPin.write(ccw); PWM.write(1); @@ -543,34 +529,28 @@ //pressing cob directionPin_key.write(cw); - PWM_key.write(1); - wait(0.14); //time it takes pwm of one to reach the key - //pause at the key for one second - PWM_key.write(0); - wait(1); - //move bakc to the starting position - directionPin_key.write(ccw); - PWM_key.write(1); - //time needs to be slightly longer than moving forward (no answer why) - wait(0.165f); - PWM_key.write(0); + PWM_key.write(1); + wait(0.05); + PWM_key.write(0); + wait(0.3); + directionPin_key.write(ccw); + PWM_key.write(1); + wait(0.039f); + PWM_key.write(0); - wait(1); + wait(0.5); //pressing va - directionPin_key.write(cw); - PWM_key.write(1); - wait(0.14); //time it takes pwm of one to reach the key - //pause at the key for one second - PWM_key.write(0); - wait(1); - //move bakc to the starting position - directionPin_key.write(ccw); - PWM_key.write(1); - //time needs to be slightly longer than moving forward (no answer why) - wait(0.165f); - PWM_key.write(0); - + directionPin_key.write(cw); + PWM_key.write(1); + wait(0.05); + PWM_key.write(0); + wait(0.3); + directionPin_key.write(ccw); + PWM_key.write(1); + wait(0.039f); + PWM_key.write(0); + // go 1 key to right directionPin.write(cw); PWM.write(1); @@ -578,19 +558,16 @@ PWM.write(0); //pressing der - directionPin_key.write(cw); - PWM_key.write(1); - wait(0.14); //time it takes pwm of one to reach the key - //pause at the key for one second - PWM_key.write(0); - wait(1); - //move bakc to the starting position - directionPin_key.write(ccw); - PWM_key.write(1); - //time needs to be slightly longer than moving forward (no answer why) - wait(0.165f); - PWM_key.write(0); - + directionPin_key.write(cw); + PWM_key.write(1); + wait(0.05); + PWM_key.write(0); + wait(0.3); + directionPin_key.write(ccw); + PWM_key.write(1); + wait(0.039f); + PWM_key.write(0); + // go 1 key to right directionPin.write(cw); PWM.write(1); @@ -598,18 +575,15 @@ PWM.write(0); //pressing ja - directionPin_key.write(cw); - PWM_key.write(1); - wait(0.14); //time it takes pwm of one to reach the key - //pause at the key for one second - PWM_key.write(0); - wait(1); - //move bakc to the starting position - directionPin_key.write(ccw); - PWM_key.write(1); - //time needs to be slightly longer than moving forward (no answer why) - wait(0.165f); - PWM_key.write(0); + directionPin_key.write(cw); + PWM_key.write(1); + wait(0.05); + PWM_key.write(0); + wait(0.3); + directionPin_key.write(ccw); + PWM_key.write(1); + wait(0.039f); + PWM_key.write(0); // go 2 key to left directionPin.write(ccw); @@ -618,18 +592,15 @@ PWM.write(0); //pressing cob - directionPin_key.write(cw); - PWM_key.write(1); - wait(0.14); //time it takes pwm of one to reach the key - //pause at the key for one second - PWM_key.write(0); - wait(1); - //move bakc to the starting position - directionPin_key.write(ccw); - PWM_key.write(1); - //time needs to be slightly longer than moving forward (no answer why) - wait(0.165f); - PWM_key.write(0); + directionPin_key.write(cw); + PWM_key.write(1); + wait(0.05); + PWM_key.write(0); + wait(0.3); + directionPin_key.write(ccw); + PWM_key.write(1); + wait(0.039f); + PWM_key.write(0); } } }