![](/media/cache/group/9e3cc099b3b04bca937a1cca1da81b19.jpg.50x50_q85.jpg)
De hele robot in 1 keer bam
Dependencies: mbed QEI Servo HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 26:7e81c7db6e7a
- Parent:
- 25:a1be4cf2ab0b
- Child:
- 27:f18da01093c9
--- a/main.cpp Fri Oct 25 13:55:49 2019 +0000 +++ b/main.cpp Fri Oct 25 14:32:35 2019 +0000 @@ -29,7 +29,7 @@ InterruptIn button3(SW3); // EMG Substates -enum EMG_States { emg_wait, emg_cal_MVC, emg_cal_rest, emg_make_scale, emg_operation }; // Define EMG substates +enum EMG_States { emg_wait, emg_cal_MVC, emg_cal_rest, emg_operation }; // Define EMG substates EMG_States emg_curr_state; // Initialize EMG substate variable bool emg_state_changed = true; @@ -47,26 +47,38 @@ AnalogIn emg3_in (A3); // Third muscle, TBD double emg1; +double emg1_env; double emg1_MVC; double emg1_MVC_stdev; double emg1_rest; double emg1_rest_stdev; +double emg1_factor; +double emg1_th; +double emg1_out; vector<double> emg1_cal; int emg1_cal_size; double emg2; +double emg2_env; double emg2_MVC; double emg2_MVC_stdev; double emg2_rest; double emg2_rest_stdev; +double emg2_factor; +double emg2_th; +double emg2_out; vector<double> emg2_cal; int emg2_cal_size; double emg3; +double emg3_env; double emg3_MVC; double emg3_MVC_stdev; double emg3_rest; double emg3_rest_stdev; +double emg3_factor; +double emg3_th; +double emg3_out; vector<double> emg3_cal; int emg3_cal_size; @@ -146,6 +158,13 @@ return output; } +// Rescale values to certain range +double rescale(double input, double out_min, double out_max, double in_min, double in_max) +{ + double output = out_min + ((input-in_min)/(in_max-in_min))*(out_max-out_min); // Based on MATLAB rescale function + return output; +} + // Handle button press void button1Press() { @@ -187,17 +206,17 @@ double emg1_n = bqc1_notch.step( emg1 ); // Filter notch double emg1_hp = bqc1_high.step( emg1_n ); // Filter highpass double emg1_rectify = fabs( emg1_hp ); // Rectify - double emg1_env = bqc1_low.step( emg1_rectify ); // Filter lowpass (completes envelope) + emg1_env = bqc1_low.step( emg1_rectify ); // Filter lowpass (completes envelope) double emg2_n = bqc2_notch.step( emg2 ); // Filter notch double emg2_hp = bqc2_high.step( emg2_n ); // Filter highpass double emg2_rectify = fabs( emg2_hp ); // Rectify - double emg2_env = bqc2_low.step( emg2_rectify ); // Filter lowpass (completes envelope) + emg2_env = bqc2_low.step( emg2_rectify ); // Filter lowpass (completes envelope) double emg3_n = bqc3_notch.step( emg3 ); // Filter notch double emg3_hp = bqc3_high.step( emg3_n ); // Filter highpass double emg3_rectify = fabs( emg3_hp ); // Rectify - double emg3_env = bqc3_low.step( emg3_rectify ); // Filter lowpass (completes envelope) + emg3_env = bqc3_low.step( emg3_rectify ); // Filter lowpass (completes envelope) scope.set(0, emg1_n); scope.set(1, emg2_n); @@ -205,7 +224,7 @@ scope.set(2, emg1_env ); scope.set(3, emg2_env ); scope.send(); - + if (calibrateNow) { emg1_cal.push_back(emg1_env); // Add values to calibration vector emg1_cal_size = emg1_cal.size(); @@ -298,7 +317,7 @@ emg_curr_state = emg_cal_rest; emg_state_changed = true; } else if ( emg_MVC_cal_done && emg_rest_cal_done ) { - emg_curr_state = emg_make_scale; + emg_curr_state = emg_operation; emg_state_changed = true; } } @@ -314,7 +333,7 @@ timerCalibration.start(); sampleNow = true; // Enable signal sampling in sampleSignal() calibrateNow = true; - + emg1_cal.reserve(Fs * Tcal); emg2_cal.reserve(Fs * Tcal); emg3_cal.reserve(Fs * Tcal); @@ -345,34 +364,59 @@ } } -void do_emg_make_scale() { +void do_emg_operation() +{ // Entry function if ( emg_state_changed == true ) { - emg_state_changed == false; - // More functions + emg_state_changed = false; + double margin_percentage = 10; // Set up % margin for rest + + emg1_factor = 1 / emg1_MVC; // Factor to normalize MVC + emg1_th = emg1_rest * emg1_factor + margin_percentage/100; // Set normalized rest threshold + emg2_factor = 1 / emg2_MVC; // Factor to normalize MVC + emg2_th = emg2_rest * emg2_factor + margin_percentage/100; // Set normalized rest threshold + emg3_factor = 1 / emg3_MVC; // Factor to normalize MVC + emg3_th = emg3_rest * emg3_factor + margin_percentage/100; // Set normalized rest threshold } // Do stuff until end condition is met - doStuff(); + + double emg1_norm = emg1_env * emg1_factor; + double emg2_norm = emg2_env * emg2_factor; + double emg3_norm = emg3_env * emg3_factor; + + if ( emg1_norm < emg1_th ) { + emg1_out = 0.0; + } else if ( emg1_norm > 1.0f ) { + emg1_out = 1.0; + } else { + emg1_out = rescale(emg1_norm, emg1_th, 1, 0, 1); + } + + if ( emg2_norm < emg2_th ) { + emg2_out = 0.0; + } else if ( emg2_norm > 1.0f ) { + emg2_out = 1.0; + } else { + emg2_out = rescale(emg2_norm, emg2_th, 1, 0, 1); + } + + if ( emg3_norm < emg3_th ) { + emg3_out = 0.0; + } else if ( emg3_norm > 1.0f ) { + emg3_out = 1.0; + } else { + emg3_out = rescale(emg3_norm, emg3_th, 1, 0, 1); + } // State transition guard - if ( endCondition == true ) { - emg_curr_state == next_state; - emg_state_changed == true; + if ( false ) { + emg_curr_state = emg_wait; + emg_state_changed = true; // More functions } } -// Determine scale factors for operation mode -void makeScale() -{ - double margin_percentage = 10; // Set up % margin for rest - double factor1 = 1 / emg1_MVC; // Factor to normalize MVC - double emg1_th = emg1_rest * factor1 + margin_percentage/100; // Set normalized rest threshold - - // pc.printf("Factor: %f TH: %f\r\n", factor1, emg1_th); -} - /* ------ EMG SUBSTATE MACHINE ------ */ @@ -388,11 +432,8 @@ case emg_cal_rest: do_emg_cal(); break; - case emg_make_scale: - do_emg_make_scale(); - break; case emg_operation: - //do_emg_operation(); + do_emg_operation(); break; } }