![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
final version
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of Project_script_union_final by
Diff: main.cpp
- Revision:
- 30:f04a35f2a06d
- Parent:
- 29:6cd4f5ac57c4
- Child:
- 31:0418ce58af56
diff -r 6cd4f5ac57c4 -r f04a35f2a06d main.cpp --- a/main.cpp Thu Nov 01 17:37:19 2018 +0000 +++ b/main.cpp Thu Nov 01 18:02:30 2018 +0000 @@ -38,10 +38,13 @@ //Tickers Ticker func_tick; Ticker movag_tick; -Ticker emg_tick; +Ticker emg_tick; +Ticker print_tick; + //Global variables -const float T = 0.002f; //Ticker period Deze wordt ook gebruikt in de PID, moet die niet anders??? +const float T = 0.002f; //Ticker period EMG, engine control +const float T2 = 0.2f; //Ticker print function //EMG filter double emg0_filt, emg1_filt, emg2_filt; //Variables for filtered EMG data channel 0, 1 and 2 @@ -108,26 +111,22 @@ //Variables PID controller double PI = 3.14159; -double Kp1 = 20.0; //Motor 1 eerst 17.5 , nu 1 +double Kp1 = 20.0; //Motor 1 double Ki1 = 1.02; double Kd1 = 1.0; double encoder_radians1=0; double err_integral1 = 0; double err_prev1, err_prev2; double err1, err2; +BiQuad LowPassFilterDer1( 1.12160e-01, 1.12160e-01, 0.00000e+00, -7.75680e-01, 0.00000e+00 ); //sample frequency 500 Hz, cutoff 20Hz low pass -//BiQuad LowPassFilterDer1(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); -//BiQuad LowPassFilterDer2(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); -BiQuad LowPassFilterDer1( 1.12160e-01, 1.12160e-01, 0.00000e+00, -7.75680e-01, 0.00000e+00 ); //sample frequency 500 Hz, cutoff 20Hz low pass -BiQuad LowPassFilterDer2( 1.12160e-01, 1.12160e-01, 0.00000e+00, -7.75680e-01, 0.00000e+00 ); - - -double Kp2 = 20.0; //Motor 2 eerst 17.5, nu 1 +double Kp2 = 20.0; //Motor 2 double Ki2 = 1.02; double Kd2 = 1.0; double encoder_radians2=0; double err_integral2 = 0; double u1, u2; +BiQuad LowPassFilterDer2( 1.12160e-01, 1.12160e-01, 0.00000e+00, -7.75680e-01, 0.00000e+00 ); int start_control = 0; @@ -429,53 +428,68 @@ void v_des_calculate_qref() { - while(emg_cal==1) //After calibration is finished, emg_cal will be 1. Otherwise 0. + while(emg_cal==1) //After calibration is finished, emg_cal will be 1. Otherwise 0. { - if(movAg1>Threshold1) //If the filtered EMG signal of muscle 1 is higher than the threshold, motor 1 will turn + if(movAg1>Threshold1 && movAg0<Threshold0) //If the filtered EMG signal of muscle 1 is higher than the threshold and the switch is off (movAg0) { - v_x = 0.5; //beweging in +x direction + v_x = 0.5; //movement in +x direction v_y = 0.0; - ledr = 0; //red + ledr = 0; //red ledb = 1; ledg = 1; } - else if(movAg2>Threshold2) //If the filtered EMG signal of muscle 2 is higher than the threshold, motor 1 and 2 will turn + else if(movAg2>Threshold2 && movAg0<Threshold0) //If the filtered EMG signal of muscle 2 is higher than the threshold and the switch is off (movAg0) { - v_y = 0.5; //beweging in +y direction + v_y = 0.5; //Movement in +y direction v_x = 0.0; - ledr = 1; //green + ledr = 1; //Green ledb = 1; ledg = 0; } - else if(movAg0>Threshold0) //If the filtered EMG signal of muscle 0 is higher than the threshold, motor1 will turn in 1 direction + else if(movAg0>Threshold0 && movAg1>Threshold1) //If the filtered EMG signal of muscle 1 is higher than the threshold and the switch is on (movAg0) { - v_x = -v_x; - v_y = -v_y; + v_y = 0.0; //Movement in -x direction + v_x = -0.5; - ledr = 1; //Blue + ledr = 0; //Purple ledb = 0; - ledg = 1; - } + ledg = 1; + } - else //If not higher than the threshold, motors will not turn at all + else if(movAg0>Threshold0 && movAg2>Threshold2) //If the filtered EMG signal of muscle 2 is higher than the threshold and the switch is on (movAg0) + { + v_y = -0.5; //Movement in -y direction + v_x = 0.0; + + ledr = 1; //Blue + ledb = 0; + ledg = 1; + } + else //If not higher than any threshold, motors will not turn at all { v_x = 0; v_y = 0; - ledr = 0; //white + ledr = 0; //White ledb = 0; ledg = 0; } - inverse_kinematics(); //Call inverse kinematics function + inverse_kinematics(); //Call inverse kinematics function break; } } +void printFunction() +{ + 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); +} + //------------------ Start main function --------------------------// @@ -484,29 +498,26 @@ 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 + 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 ); - emg_tick.attach(&emg_filtered,T); //EMG signals filtered + moving average every T sec. + emg_tick.attach(&emg_filtered,T); //EMG signals filtered + moving average every T sec. movag_tick.attach(&MovAg,T); - func_tick.attach(&v_des_calculate_qref,T); //v_des determined every T + func_tick.attach(&v_des_calculate_qref,T); //v_des determined every T + print_tick.attach(&printFunction,T2); + // HIDScope_tick.attach(&HIDScope_sample,T); //EMG signals raw + filtered to HIDScope every T sec. - // 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); //Wait to avoid bouncing of button - button2.rise(calibrate); //Calibrate threshold for 3 muscles - //wait(0.2f); //Wait to avoid bouncing of button + button1.rise(switch_to_calibrate); //Switch state of calibration (which muscle) + //wait(0.2f); //Wait to avoid bouncing of button + button2.rise(calibrate); //Calibrate threshold for 3 muscles + //wait(0.2f); //Wait to avoid bouncing of button while(true) { - 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); + ; } }