pid gecomment
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of a_pid_kal_end_def by
Diff: main.cpp
- Revision:
- 44:c79e5944ac91
- Parent:
- 40:187ef29de53d
- Child:
- 45:08bddea67bd8
--- a/main.cpp Tue Nov 01 08:48:08 2016 +0000 +++ b/main.cpp Wed Nov 02 15:25:03 2016 +0000 @@ -27,32 +27,10 @@ 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 -double signal_right_arm; int n = 0; //start van de teller wordt op nul gesteld -//biceps arm 1, right arm -double emg_biceps_right; -double emg_filtered_high_biceps_right; -double emg_abs_biceps_right; -double emg_filtered_biceps_right; -double emg_filtered_high_notch_1_biceps_right; -//double emg_filtered_high_notch_1_2_biceps_right; - -//triceps arm 1, right arm -double emg_triceps_right; -double emg_filtered_high_triceps_right; -double emg_abs_triceps_right; -double emg_filtered_triceps_right; -double emg_filtered_high_notch_1_triceps_right; - -//biceps arm 1, left arm -double emg_biceps_left; -double emg_filtered_high_biceps_left; -double emg_abs_biceps_left; -double emg_filtered_biceps_left; -double emg_filtered_high_notch_1_biceps_left; - -//before abs filtering +//======================================= +//filter coefficients //b1 = biceps right arm BiQuad filterhigh_b1(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01); @@ -95,55 +73,56 @@ void filter(){ //biceps right arm read+filtering - emg_biceps_right=emg_biceps_right_in.read(); //read the emg value from the elektrodes - emg_filtered_high_biceps_right= filterhigh_b1.step(emg_biceps_right); - emg_filtered_high_notch_1_biceps_right=filternotch1_b1.step(emg_filtered_high_biceps_right); - //emg_filtered_high_notch_1_2_biceps_right=filternotch2_b1.step(emg_filtered_high_notch_1_biceps_right); - emg_abs_biceps_right=fabs(emg_filtered_high_notch_1_biceps_right); //fabs because float - emg_filtered_biceps_right=filterlow_b1.step(emg_abs_biceps_right); + 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); + double emg_filtered_high_notch_1_biceps_right=filternotch1_b1.step(emg_filtered_high_biceps_right); + double emg_abs_biceps_right=fabs(emg_filtered_high_notch_1_biceps_right); //fabs because float + double emg_filtered_biceps_right=filterlow_b1.step(emg_abs_biceps_right); //triceps right arm read+filtering - emg_triceps_right=emg_triceps_right_in.read(); //read the emg value from the elektrodes - emg_filtered_high_triceps_right= filterhigh_t1.step(emg_triceps_right); - emg_filtered_high_notch_1_triceps_right=filternotch1_t1.step(emg_filtered_high_triceps_right); - //emg_filtered_high_notch_1_2_triceps_right=filternotch2_t1.step(emg_filtered_high_notch_1_triceps_right); - emg_abs_triceps_right=fabs(emg_filtered_high_notch_1_triceps_right); //fabs because float - emg_filtered_triceps_right=filterlow_t1.step(emg_abs_triceps_right); + 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); + double emg_filtered_high_notch_1_triceps_right=filternotch1_t1.step(emg_filtered_high_triceps_right); + double emg_abs_triceps_right=fabs(emg_filtered_high_notch_1_triceps_right); //fabs because float + double emg_filtered_triceps_right=filterlow_t1.step(emg_abs_triceps_right); //biceps left arm read+filtering - emg_biceps_left=emg_biceps_left_in.read(); //read the emg value from the elektrodes - emg_filtered_high_biceps_left= filterhigh_b2.step(emg_biceps_left); - emg_filtered_high_notch_1_biceps_left=filternotch1_b2.step(emg_filtered_high_biceps_left); - emg_abs_biceps_left=fabs(emg_filtered_high_notch_1_biceps_left); //fabs because float - emg_filtered_biceps_left=filterlow_b2.step(emg_abs_biceps_left); + 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); + 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 - - signal_right_arm=emg_filtered_biceps_right-emg_filtered_triceps_right; + 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! - if (signal_right_arm>cut_off_value_biceps) - {onoffsignal_rightarm=1;} + //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;} - else if (signal_right_arm<cut_off_value_triceps) + else if (signal_biceps_sum<cut_off_value_biceps_left) { - onoffsignal_rightarm=-1; + onoffsignal_biceps=-1; } else - {onoffsignal_rightarm=0;} + {onoffsignal_biceps=0;} //creating on/off signal for switch (left arm) - if (emg_filtered_biceps_left>cut_off_value_biceps) + if (bicepstriceps_rightarm<cut_off_value_triceps) { - switch_signal_leftarm=1; + switch_signal_triceps=1; } else { - switch_signal_leftarm=0; + switch_signal_triceps=0; } - //send signals to scope 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 @@ -164,6 +143,8 @@ 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); + +//============================================================================================== //endless loop