met knopjes, voor Wubs, zit PID in dus restrictie.
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of a_pid_kal_end_def by
Revision 60:c165691c4e86, committed 2016-11-14
- Comitter:
- daniQQue
- Date:
- Mon Nov 14 12:13:38 2016 +0000
- Parent:
- 59:1725a3f02f37
- Commit message:
- met buttons
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 1725a3f02f37 -r c165691c4e86 main.cpp --- a/main.cpp Tue Nov 08 20:31:45 2016 +0000 +++ b/main.cpp Mon Nov 14 12:13:38 2016 +0000 @@ -9,34 +9,23 @@ //======================================================================================================================================================= //Define objects - -//EMG -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++ - //Tickers -Ticker sample_timer; //ticker for EMG signal sampling, analog becomes digital -Ticker ticker_switch; //ticker for switch, every second it is possible to switch Ticker ticker_referenceangle; //ticker for the reference angle Ticker ticker_controllerm1; //ticker for the controller (PID) of motor 1 Ticker ticker_encoder; //ticker for encoderfunction motor 1 -Ticker ticker_calibration_biceps; //ticker for calibration biceps -Ticker ticker_calibration_triceps; //ticker for calibation triceps //Timer Timer timer; //Monitoring -HIDScope scope(5); //open 5 channels in hidscope MODSERIAL pc(USBTX, USBRX); //pc connection DigitalOut red(LED_RED); //LED on K64F board, 1 is out; 0 is on DigitalOut green(LED_GREEN); //LED on K64f board, 1 is out; o is on -DigitalOut blue(LED_BLUE); //LED on K64f board, 1 is out; o is on //buttons -DigitalIn button_calibration_biceps (SW3); //button to start calibration biceps -DigitalIn button_calibration_triceps (SW2); // button to start calibration triceps +DigitalIn button__right_biceps(D9); +DigitalIn button__left_biceps(PTC12); +InterruptIn button_switch(SW3); //motors DigitalOut richting_motor1(D7); //motor 1 connected to motor 1 at k64f board; for turningtable @@ -54,22 +43,8 @@ //======================================================================================================================================================= //define variables -//thresholds -double treshold_biceps_right = 0.04; //common values that work. -double treshold_biceps_left = -0.04; //tested on multiple persons -double treshold_triceps = -0.04; //triceps and left biceps is specified negative, thus negative treshold - - -//calibration variables -const float percentage_max_triceps=0.25; //percentage from max to calculate new treshold -const float percentage_max_biceps =0.3; //percentage from max to calculate new treshold -double max_biceps; //calibration maximum biceps -double max_triceps; //calibration maximum triceps - //on/off and switch signals int switch_signal = 0; //start of counter, switch made by even and odd numbers -int onoffsignal_biceps; //on/off signal created by the bicepssignal. (-1: left biceps contract, 0: nothing contracted, 1: right biceps contracted) -int switch_signal_triceps; //motorvariables float speedmotor1=0.18; //speed of motor 1 is 0.18 pwm at start @@ -105,32 +80,13 @@ //======================================================================================================================================================= -//filter coefficients - -//b1 = biceps right arm -BiQuad filterhigh_b1(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01); // second order highpass filter, with frequency of 10 Hz -BiQuad filternotch1_b1 (9.5654e-01, -1.9131e+00, 9.5654e-01 ,-1.9112e+00 ,9.1498e-01; // IIRnotch filter, with frequency of 50 Hz - -//t1= triceps right arm -BiQuad filterhigh_t1(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01); // second order highpass filter, with frequency of 10 Hz -BiQuad filternotch1_t1 (9.5654e-01, -1.9131e+00, 9.5654e-01 ,-1.9112e+00 ,9.1498e-01; // IIRnotch filter, with frequency of 50 Hz - -//b2= biceps left arm -BiQuad filterhigh_b2(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01); // second order highpass filter, with frequency of 10 Hz -BiQuad filternotch1_b2 (9.5654e-01, -1.9131e+00, 9.5654e-01 ,-1.9112e+00 ,9.1498e-01; // IIRnotch filter, with frequency of 50 Hz - -//after abs filtering -BiQuad filterlow_b1 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01); // second order lowpass filter, with frequency of 2 Hz -BiQuad filterlow_t1 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01); // second order lowpass filter, with frequency of 2 Hz -BiQuad filterlow_b2 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01); // second order lowpass filter, with frequency of 2 Hz - //======================================================================================================================================================= //voids //======================================================================================================================================================= //function teller void switch_function() { // The switch function. Makes it possible to switch between the motors. It simply adds one at switch_signal. - if(switch_signal_triceps==1){ + if(button_switch==0){ switch_signal++; // To monitor what is happening: we will show the text in putty and change led color from red to green or vice versa. @@ -139,92 +95,28 @@ red=!red; if (switch_signal%2==0){ - 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"); + pc.printf("If you push button 1 , the robot will go right \r\n"); + pc.printf("If you push button 2, the robot will go left \r\n"); pc.printf("\r\n"); } else{ - 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"); + pc.printf("If you push button 1, the robot will go up \r\n"); + pc.printf("If you push button 2, the robot will go down \r\n"); pc.printf("\r\n"); } } } - -//======================================================================================================================================================= -//functions which are called in ticker to sample the analog signal and make the on/off and switch signal. - -//Filter void :// funciton which is called in ticker to sample the analog signal and make the on/off and switch signal. -void filter(){ - //biceps right arm read+filtering - double emg_biceps_right=emg_biceps_right_in.read(); //read the emg value from the elektrodes - double emg_filtered_high_biceps_right= filterhigh_b1.step(emg_biceps_right); //high pass filter, to remove offset - double emg_filtered_high_notch_1_biceps_right=filternotch1_b1.step(emg_filtered_high_biceps_right); //notch filter, to remove noise - double emg_abs_biceps_right=fabs(emg_filtered_high_notch_1_biceps_right); //rectify the signal, fabs because float - double emg_filtered_biceps_right=filterlow_b1.step(emg_abs_biceps_right); //low pass filter to envelope the signal - - //triceps right arm read+filtering - double emg_triceps_right=emg_triceps_right_in.read(); //read the emg value from the elektrodes - double emg_filtered_high_triceps_right= filterhigh_t1.step(emg_triceps_right); //high pass filter, to remove offset - double emg_filtered_high_notch_1_triceps_right=filternotch1_t1.step(emg_filtered_high_triceps_right); //notch filter, to remove noise - double emg_abs_triceps_right=fabs(emg_filtered_high_notch_1_triceps_right); //rectify the signal, fabs because float - double emg_filtered_triceps_right=filterlow_t1.step(emg_abs_triceps_right); //low pass filter to envelope the signal - - //biceps left arm read+filtering - double emg_biceps_left=emg_biceps_left_in.read(); //read the emg value from the elektrodes - double emg_filtered_high_biceps_left= filterhigh_b2.step(emg_biceps_left); //high pass filter, to remove offset - double emg_filtered_high_notch_1_biceps_left=filternotch1_b2.step(emg_filtered_high_biceps_left); //notch filter, to remove noise - double emg_abs_biceps_left=fabs(emg_filtered_high_notch_1_biceps_left); //rectify the signal, fabs because float - double emg_filtered_biceps_left=filterlow_b2.step(emg_abs_biceps_left); //low pass filter to envelope the signal - - //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>treshold_biceps_right){ - onoffsignal_biceps=1; - } - - else if (signal_biceps_sum<treshold_biceps_left){ - onoffsignal_biceps=-1; - } - - else{ - onoffsignal_biceps=0; - } - - //creating on/off signal for switch (left arm) - - if (bicepstriceps_rightarm<treshold_triceps){ - switch_signal_triceps=1; - } - - else{ - switch_signal_triceps=0; - } - - //send signals to scope to monitor the EMG signals - scope.set(0, emg_filtered_biceps_right); //set emg signal of right biceps to scope in channel 0 - scope.set(1, emg_filtered_triceps_right); // set emg signal of right triceps to scope in channel 1 - scope.set(2, emg_filtered_biceps_left); // set emg signal of left biceps to scope in channel 2 - scope.set(3, bicepstriceps_rightarm); // set on/off signal for the motors to scope in channel 3 - scope.set(4, switch_signal_triceps); // set the switch signal to scope in channel 4 - - scope.send(); //send all the signals to the scope -} -//======================================================================================================================================================= + //======================================================================================================================================================= //reference void makes the reference that the controllor should follow. There is only a controller for motor 1. void reference(){ if (start_motor == true){ //bool that is true when the motor starts turning timer.start(); //timer that starts counting in milliseconds } - if (onoffsignal_biceps==-1 && switch_signal%2==0){ //the signal of the biceps is -1 and the switch is even, so motor 1 is being controlled + if (button__left_biceps==0 && switch_signal%2==0){ //the signal of the biceps is -1 and the switch is even, so motor 1 is being controlled t_start = timer.read_ms(); //read the current time passed from the timer start_motor = false; //it means that the motor is not running or has started up @@ -248,7 +140,7 @@ d_ref = d_ref; //if there is no signal, the referance angle is constant } - if (onoffsignal_biceps==1 && switch_signal%2==0){ //the signal of the biceps is -1 and the switch is even, so motor 1 is being controlled + if (button__right_biceps==0 && switch_signal%2==0){ //the signal of the biceps is -1 and the switch is even, so motor 1 is being controlled t_start = timer.read_ms(); start_motor = false; @@ -284,151 +176,9 @@ void encoders(){ counts_encoder1 = Encoder1.getPulses(); rev_counts_motor1 = (float)counts_encoder1/(gearboxratio*rev_rond); - rev_counts_motor1_rad = rev_counts_motor1*6.28318530718; //calculate the angle in radians + rev_counts_motor1_rad = rev_counts_motor1*6.28318530718; + } - -//======================================================================================================================================================= - -//The calibration of the Biceps threshold is started by a button. -//It determines the maximum reachable EMG signal and takes a percentage of this to determine the new threshold. -void calibration_biceps(){ - if (button_calibration_biceps==0){ //only runs when button is pressed - - //detach tickers of other voids that control the switched and motors. To avoid unwanted moving and switching of the motors. - ticker_switch.detach(); - sample_timer.detach(); - - //let the user know what is happening, blue led on: calibration is going. - pc.printf("start of calibration biceps, contract maximal \r\n"); - pc.printf("\r\n"); - red=1; - green=1; - blue=0; - - //start callibration of biceps - for(int n =0; n<1500;n++){ //read for 1500 samples as calibration - - //biceps right arm read+filtering - double emg_biceps_right=emg_biceps_right_in.read(); //read the emg value from the elektrodes - double emg_filtered_high_biceps_right= filterhigh_b1.step(emg_biceps_right); //high pass filter, to remove offset - double emg_filtered_high_notch_1_biceps_right=filternotch1_b1.step(emg_filtered_high_biceps_right); //notch filter, to remove noise - double emg_abs_biceps_right=fabs(emg_filtered_high_notch_1_biceps_right); //rectify the signal, fabs because float - double emg_filtered_biceps_right=filterlow_b1.step(emg_abs_biceps_right); //low pass filter to envelope the signal - - //triceps right arm read+filtering - double emg_triceps_right=emg_triceps_right_in.read(); //read the emg value from the elektrodes - double emg_filtered_high_triceps_right= filterhigh_t1.step(emg_triceps_right); //high pass filter, to remove offset - double emg_filtered_high_notch_1_triceps_right=filternotch1_t1.step(emg_filtered_high_triceps_right); //notch filter, to remove noise - double emg_abs_triceps_right=fabs(emg_filtered_high_notch_1_triceps_right); //rectify the signal, fabs because float - double emg_filtered_triceps_right=filterlow_t1.step(emg_abs_triceps_right); //low pass filter to envelope the signal - - //biceps is +, triceps is - - double bicepstriceps_rightarm=emg_filtered_biceps_right-emg_filtered_triceps_right; - - if (bicepstriceps_rightarm > max_biceps){ //determine what the highest reachable emg signal is - - max_biceps = bicepstriceps_rightarm; - - } - wait(0.001f); //to sample at same freq; 1000Hz - } - - treshold_biceps_right=percentage_max_biceps*max_biceps; //determine new treshold, right biceps is + - treshold_biceps_left=-treshold_biceps_right; //determine new treshold, right biceps is - - - //toggle lights to see the calibration is done. Also show in putty that the calibration is done. - blue=!blue; - - pc.printf(" end of calibration\r\n",treshold_biceps_right ); - pc.printf(" change of cv biceps: %f ",treshold_biceps_right ); - - wait(0.2f); - - //remind the person of what motor will go on an which direction - if (switch_signal%2==0){ - green=0; - red=1; - } - - else{ - green=1; - red=0; - } - } - //reattach the functions to the tickers that were detached. - ticker_switch.attach(&switch_function,1.0); - sample_timer.attach(&filter, 0.001); -} -//======================================================================================================================================================= - -//The calibration of the triceps threshold is started by a button. -//It determines the maximum reachable EMG signal and takes a percentage of this to determine the new threshold. -void calibration_triceps(){ - if(button_calibration_triceps==0){ //only runs when button is pressed - - //detach tickers of other voids that control the switched and motors. To avoid unwanted moving and switching of the motors. - ticker_switch.detach(); - sample_timer.detach(); - - //toggel LEDS and let the user know that callibration of triceps is starting. - red=1; - green=1; - blue=0; - - pc.printf("start of calibration triceps\r\n"); - pc.printf("\r\n"); - - //start calibration of triceps - for(int n =0; n<1500;n++){ //read for 2000 samples as calibration - - //biceps right arm read+filtering - double emg_biceps_right=emg_biceps_right_in.read(); //read the emg value from the elektrodes - double emg_filtered_high_biceps_right= filterhigh_b1.step(emg_biceps_right); //high pass filter, to remove offset - double emg_filtered_high_notch_1_biceps_right=filternotch1_b1.step(emg_filtered_high_biceps_right); //notch filter, to remove noise - double emg_abs_biceps_right=fabs(emg_filtered_high_notch_1_biceps_right); //rectify the signal, fabs because float - double emg_filtered_biceps_right=filterlow_b1.step(emg_abs_biceps_right); //low pass filter to envelope the signal - - //triceps right arm read+filtering - double emg_triceps_right=emg_triceps_right_in.read(); //read the emg value from the elektrodes - double emg_filtered_high_triceps_right= filterhigh_t1.step(emg_triceps_right); //high pass filter, to remove offset - double emg_filtered_high_notch_1_triceps_right=filternotch1_t1.step(emg_filtered_high_triceps_right); //notch filter, to remove noise - double emg_abs_triceps_right=fabs(emg_filtered_high_notch_1_triceps_right); //rectify the signal, fabs because float - double emg_filtered_triceps_right=filterlow_t1.step(emg_abs_triceps_right); //low pass filter to envelope the signal - - //biceps is +, triceps is - - double bicepstriceps_rightarm=emg_filtered_biceps_right-emg_filtered_triceps_right; - - if (bicepstriceps_rightarm < max_triceps){ //determine what the lowest reachable emg of triceps (max in negative part) signal is - - max_triceps = bicepstriceps_rightarm; - - } - wait(0.001f); //to sample at same freq; 1000Hz - } - treshold_triceps=percentage_max_triceps*max_triceps; //calculate the new treshold. This is a negative number due to the sum! - - //Let the user know that the calibration is done. - pc.printf(" end of calibration\r\n"); - pc.printf(" change of cv triceps: %f ",treshold_triceps ); - blue=!blue; - wait(0.2f); - if (switch_signal%2==0){ - green=0; - red=1; - } - - else{ - green=1; - red=0; - } - } - - //reattach the functions to the tickers that were detached. - sample_timer.attach(&filter, 0.001); - ticker_switch.attach(&switch_function,1.0); -} -//======================================================================================================================================================= - //======================================================================================================================================================= //program //======================================================================================================================================================= @@ -436,18 +186,13 @@ pc.baud(115200); //connect with pc with baudrate 115200 green=1; //led is off (1), at beginning - blue=1; //led is off (1), at beginning red=0; //led is on (0), at beginning //attach tickers to functions - sample_timer.attach(&filter, Ts); //continously execute the EMG reader and filter, it ensures that filter and sampling is executed every 1/frequency seconds - ticker_switch.attach(&switch_function,1.0); //it is possible to switch only once in a second, this ensures that the switch is not reacting on one signal multiple times. ticker_referenceangle.attach(&reference, Ts); ticker_controllerm1.attach(&m1_controller, Ts); ticker_encoder.attach(&encoders, Ts); - ticker_calibration_biceps.attach (&calibration_biceps,2.0); //to call calibration biceps, stop EMG sampling and switch - ticker_calibration_triceps.attach(&calibration_triceps,2.0); //to call calibration triceps, stop EMG sampling and switch - + //PID controller PID_controller.PIDF(Kp,Ki,Kd,N,Ts); @@ -476,8 +221,8 @@ while (true) { //neverending loop - - if (onoffsignal_biceps==-1){ //left biceps contracted + button_switch.fall(&switch_function); + if (button__left_biceps==0){ //left biceps contracted if (switch_signal%2==0){ //switch even @@ -502,7 +247,7 @@ } } - else if (onoffsignal_biceps==1){ //right biceps contracted + else if (button__right_biceps==0){ //right biceps contracted if (switch_signal%2==0){ //switch signal even speedmotor1=controlOutput;