pid gecomment
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of a_pid_kal_end_def by
Diff: main.cpp
- Revision:
- 45:08bddea67bd8
- Parent:
- 44:c79e5944ac91
- Child:
- 46:4a8889f9dc9f
--- a/main.cpp Wed Nov 02 15:25:03 2016 +0000 +++ b/main.cpp Wed Nov 02 16:18:25 2016 +0000 @@ -1,15 +1,17 @@ +//========================================== //libraries #include "mbed.h" #include "HIDScope.h" #include "BiQuad.h" #include "MODSERIAL.h" +//========================================= //Define objects AnalogIn emg_biceps_right_in( A0 ); //analog in to get EMG biceps (r) in to c++ AnalogIn emg_triceps_right_in(A1); //analog in to get EMG triceps (r) in to c++ AnalogIn emg_biceps_left_in (A2); //analog in to get EMG biceps (l) in to c++ Ticker sample_timer; //ticker -Ticker switch_function; //ticker +Ticker ticker_switch; //ticker HIDScope scope(5); //open 3 channels in hidscope MODSERIAL pc(USBTX, USBRX); //pc connection DigitalOut red(LED_RED); @@ -25,9 +27,13 @@ //other int onoffsignal_rightarm=0; // on/off signal: 1; biceps activation, 0: nothing, -1, triceps activation int switch_signal_leftarm=0; // switching between motors. -double cut_off_value_biceps =0.04; //gespecificeerd door floortje -double cut_off_value_triceps=-0.03; //gespecificeerd door floortje -int n = 0; //start van de teller wordt op nul gesteld +double cut_off_value_biceps_right =0.04; //gespecificeerd door floortje +double cut_off_value_biceps_left=-0.04; +double cut_off_value_triceps=-0.04; +int switch_signal = 0; //start van de teller wordt op nul gesteld + +int onoffsignal_biceps; +int switch_signal_triceps; //======================================= //filter coefficients @@ -49,14 +55,18 @@ BiQuad filterlow_t1 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01); BiQuad filterlow_b2 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01); +//====================================================================== +//voids +//====================================================================== + //function teller -void SwitchN() { // maakt simpele functie die 1 bij n optelt - if(switch_signal_leftarm==1) +void switch_function() { // maakt simpele functie die 1 bij n optelt + if(switch_signal_triceps==1) { - n++; + switch_signal++; green=!green; red=!red; - if (n%2==0) + if (switch_signal%2==0) {pc.printf("If you contract the biceps, the robot will go up \r\n"); pc.printf("If you contract the triceps, the robot will go down \r\n"); } @@ -92,14 +102,12 @@ double emg_filtered_high_notch_1_biceps_left=filternotch1_b2.step(emg_filtered_high_biceps_left); double emg_abs_biceps_left=fabs(emg_filtered_high_notch_1_biceps_left); //fabs because float double emg_filtered_biceps_left=filterlow_b2.step(emg_abs_biceps_left); - - //signal substraction of filter biceps and triceps. Biceps +,triceps - - double signal_right_arm=emg_filtered_biceps_right-emg_filtered_triceps_right; //creating of on/off signal with the created on/off signals, with if statement for right arm! //signal substraction of filter biceps and triceps. right Biceps + left biceps - double signal_biceps_sum=emg_filtered_biceps_right-emg_filtered_biceps_left; double bicepstriceps_rightarm=emg_filtered_biceps_right-emg_filtered_triceps_right; + //creating of on/off signal with the created on/off signals, with if statement for right arm! if (signal_biceps_sum>cut_off_value_biceps_right) {onoffsignal_biceps=1;} @@ -127,8 +135,8 @@ scope.set(0, emg_filtered_biceps_right); //set emg signal to scope in channel 0 scope.set(1, emg_filtered_triceps_right); // set emg signal to scope in channel 1 scope.set(2, emg_filtered_biceps_left); // set emg signal to scope in channel 2 - scope.set(3, onoffsignal_rightarm); // set emg signal to scope in channel 3 - scope.set(4, switch_signal_leftarm); + scope.set(3, onoffsignal_biceps); // set emg signal to scope in channel 3 + scope.set(4, switch_signal_triceps); scope.send(); //send all the signals to the scope } @@ -142,8 +150,18 @@ red=1; sample_timer.attach(&filter, 0.001); //continously execute the EMG reader and filter, it ensures that filter and sampling is executed every 1/frequency seconds -switch_function.attach(&SwitchN,1.0); +ticker_switch.attach(&switch_function,1.0); +if (switch_signal%2==0) + {pc.printf("If you contract the biceps, the robot will go up \r\n"); + pc.printf("If you contract the triceps, the robot will go down \r\n"); + } + + else + {pc.printf("If you contract the biceps, the robot will go right \r\n"); + pc.printf("If you contract the triceps, the robot will go left \r\n"); + } + //============================================================================================== //endless loop @@ -151,34 +169,34 @@ while (true) { // zorgt er voor dat de code oneindig doorgelopen wordt - if (onoffsignal_rightarm==-1) // als s ingedrukt wordt gebeurd het volgende + if (onoffsignal_biceps==-1) //left biceps contracted { - if (n%2==0) // als s ingedrukt wordt en het getal is even gebeurd het onderstaande + if (switch_signal%2==0) { - richting_motor1 = 1; + richting_motor1 = 1; //motor 1, left pwm_motor1 = 1; } - else // als s is ingedrukt maar het getal is niet even (dus oneven) gebeurdt het onderstaande + else { - richting_motor2 = 1; + richting_motor2 = 1; //motor 2, up pwm_motor2 = 1; } } - else if (onoffsignal_rightarm==1) // als d ingedrukt wordt gebeurd het volgende + else if (onoffsignal_biceps==1) // als d ingedrukt wordt gebeurd het volgende { - if (n%2==0) // als d is ingedrukt en n is even dan gebeurd het volgende + if (switch_signal%2==0) // als d is ingedrukt en n is even dan gebeurd het volgende { - richting_motor1 = 0; + richting_motor1 = 0; //motor 1, right pwm_motor1 = 1; } else // als d is ingedrukt maar het getal is niet even (dus oneven) gebeurt het onderstaande { - richting_motor2 = 0; + richting_motor2 = 0; //motor 2. down pwm_motor2 = 1; }