Wil je hier nog je PID controler kort uitleggen? Is sneller denk ik. Rest is gedaan volgens mij. Hier zit kall in.
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of another_try_from_scratch_on_emg by
Revision 57:c546edf67c5c, committed 2016-11-04
- Comitter:
- daniQQue
- Date:
- Fri Nov 04 15:12:52 2016 +0000
- Parent:
- 56:a38412383477
- Commit message:
- kallibratie, motors werken, pid controler
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r a38412383477 -r c546edf67c5c main.cpp --- a/main.cpp Thu Nov 03 16:41:10 2016 +0000 +++ b/main.cpp Fri Nov 04 15:12:52 2016 +0000 @@ -1,4 +1,4 @@ -//===================================================================================== +//======================================================================================================================================================= //libraries #include "mbed.h" //mbed revision 113 #include "HIDScope.h" //Hidscope by Tom Lankhorst @@ -7,7 +7,7 @@ #include "QEI.h" //QEI library for the encoders -//===================================================================================== +//======================================================================================================================================================= //Define objects //EMG @@ -20,7 +20,9 @@ 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 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; @@ -30,6 +32,11 @@ 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 //motors DigitalOut richting_motor1(D7); //motor 1 connected to motor 1 at k64f board; for turningtable @@ -40,14 +47,11 @@ //encoders DigitalIn encoder1A(D13); DigitalIn encoder1B(D12); -DigitalIn encoder2A(D11); -DigitalIn encoder2B(D10); //controller BiQuad PID_controller; - -//===================================================================================== +//======================================================================================================================================================= //define variables //thresholds @@ -55,9 +59,16 @@ 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; +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 @@ -68,53 +79,54 @@ int ccw=1; //counterclockwise direction //encoder -int counts_encoder1; -//int counts_encoder2; -float rev_counts_motor1; -float rev_counts_motor1_rad; -const float gearboxratio=131.25; // gearboxratio van encoder naar motor -const float rev_rond=64.0; // aantal revoluties per omgang van de encoder -QEI Encoder1(D13,D12,NC,rev_rond,QEI::X4_ENCODING); +int counts_encoder1; //variable to count the pulses given by the encoder, 1 indicates motor 1. +float rev_counts_motor1; //Calculated revolutions +float rev_counts_motor1_rad; //calculated revolutions in rad! +const float gearboxratio=131.25; // gearboxratio from encoder to motor +const float rev_rond=64.0; // number of revolutions per rotation + +QEI Encoder1(D13,D12,NC,rev_rond,QEI::X4_ENCODING); //To set the Encoder. //reference -volatile float d_ref = 0; -const float w_ref = 1.5; -volatile double t_start; -volatile double w_var; -const double Ts = 0.001; +volatile float d_ref = 0; +const float w_ref = 1.5; +volatile double t_start; +volatile double w_var; +const double Ts = 0.001; //Time for diverse tickers. It is comparable to a frequency of 1000Hz. //controller -const double Kp = 0.3823; -const double Ki = 0.1279; -const double Kd = 0.2519; -const double N = 100; -volatile double error1; -volatile double controlOutput; -bool start_motor = true; -volatile double starttime; -//======================================= +const double Kp = 0.3823; +const double Ki = 0.1279; +const double Kd = 0.2519; +const double N = 100; +volatile double error1; +volatile double controlOutput; +bool start_motor = true; +volatile double starttime; + +//======================================================================================================================================================= //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? -BiQuad filternotch1_b1 (9.9376e-01 , -1.8902e-00, 9.9376e-01 , -1.8902e-00 , 9.875e-01); // second order notch filter, with frequency of? +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 20 Hz +BiQuad filternotch1_b1 (9.9376e-01 , -1.8902e-00, 9.9376e-01 , -1.8902e-00 , 9.875e-01); // second order notch filter, with frequency of 49-51 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? -BiQuad filternotch1_t1 (9.9376e-01 , -1.8902e-00, 9.9376e-01 , -1.8902e-00 , 9.875e-01); // second order notch filter, with frequency of? +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 20 Hz +BiQuad filternotch1_t1 (9.9376e-01 , -1.8902e-00, 9.9376e-01 , -1.8902e-00 , 9.875e-01); // second order notch filter, with frequency of 49-51 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? -BiQuad filternotch1_b2 (9.9376e-01 , -1.8902e-00, 9.9376e-01 , -1.8902e-00 , 9.875e-01); // second order notch filter, with frequency of? +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 20 Hz +BiQuad filternotch1_b2 (9.9376e-01 , -1.8902e-00, 9.9376e-01 , -1.8902e-00 , 9.875e-01); // second order notch filter, with frequency of 49-51 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? -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? -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? +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. @@ -143,9 +155,10 @@ } } -//====================================================================== +//======================================================================================================================================================= //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 @@ -198,16 +211,18 @@ scope.set(3, onoffsignal_biceps); // 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 + 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){ + if (start_motor == true){ timer.start(); } if (onoffsignal_biceps==-1 && switch_signal%2==0){ //switch even t_start = timer.read_ms(); - start_motor = false; + start_motor = false; //It means that motor 2 is running and therefore the PID controllor should not be working. Therefore the bool is set on false. if (t_start < 1.0){ w_var = t_start*1.5; @@ -251,12 +266,15 @@ } } - +//======================================================================================================================================================= +//This void calculates the error and makes the control output. void m1_controller(){ error1 = d_ref-rev_counts_motor1_rad; controlOutput = PID_controller.step(error1); } +//======================================================================================================================================================= +//This void calculated the number of rotations that the motor has done in rad. It is put in a void because with the ticker, this ensures that it is updated continuously. void encoders(){ counts_encoder1 = Encoder1.getPulses(); rev_counts_motor1 = (float)counts_encoder1/(gearboxratio*rev_rond); @@ -264,9 +282,144 @@ } -//====================================================================== +//======================================================================================================================================================= + +//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 -//====================================================================== +//======================================================================================================================================================= int main() { @@ -275,11 +428,13 @@ 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); +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); @@ -304,37 +459,26 @@ pc.printf("\r\n"); } -//============================================================================================== +//======================================================================================================================================================= //endless loop while (true) { // neverending loop -/*counts_encoder1 = Encoder1.getPulses(); -rev_counts_motor1=(float)counts_encoder1/(gearboxratio*rev_rond); -rev_counts_motor1_rad=rev_counts_motor1*6.28318530718; */ - -pc.printf("%f %f \r \n", d_ref, rev_counts_motor1_rad); -//pc.printf("%f ", rev_counts_motor1_rad); -//pc.printf("%f",w_var); -//pc.printf("%d\n",start_motor); - - if (onoffsignal_biceps==-1){ //left biceps contracted if (switch_signal%2==0){ //switch even speedmotor1=controlOutput; - //richting_motor1 = ccw; //motor 1, left - //pwm_motor1 = speedmotor1; //speed of motor 1 + if (speedmotor1<0){ - richting_motor1 = cw; + richting_motor1 = cw; // motor 1, right } else { - richting_motor1 = ccw; + richting_motor1 = ccw; //motor 1, left } - pwm_motor1 = fabs(speedmotor1); //speed of motor 1 - // pc.printf("%f\r\n", pwm_motor1.read()); + pwm_motor1 = fabs(speedmotor1); //speed of motor 1 + } @@ -351,26 +495,25 @@ if (switch_signal%2==0) //switch signal even { speedmotor1=controlOutput; - //richting_motor1 = ccw; //motor 1, left - //pwm_motor1 = speedmotor1; //speed of motor 1 + if (speedmotor1<0){ - richting_motor1 = cw; + richting_motor1 = cw; //motor 1, right } else { - richting_motor1 = ccw; + richting_motor1 = ccw; //motor 1, left } pwm_motor1 = fabs(speedmotor1); //speed of motor 1 - // pc.printf("%f\r\n", pwm_motor1.read()); + } - else //switch signal odd + else //switch signal odd { - richting_motor2 = cw; //motor 2. down + richting_motor2 = cw; //motor 2, down pwm_motor2 = speedmotor2; //speed motor 2 } } else{ - //no contraction of biceps + //no contraction of biceps, thus no motoraction. pwm_motor2=0; pwm_motor1=0; start_motor = true; @@ -382,4 +525,4 @@ } //int main closed -//=============================================================================================1 \ No newline at end of file +//=======================================================================================================================================================