De hele robot in 1 keer bam
Dependencies: mbed QEI Servo HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 31:b5188b6d45db
- Parent:
- 30:bac3b60d6283
- Child:
- 32:b9b9c50f5429
--- a/main.cpp Mon Oct 28 15:47:48 2019 +0000 +++ b/main.cpp Mon Oct 28 16:34:03 2019 +0000 @@ -55,6 +55,7 @@ double emg1_factor; double emg1_th; double emg1_out; +double emg1_norm; vector<double> emg1_cal; int emg1_cal_size; @@ -67,6 +68,7 @@ double emg2_factor; double emg2_th; double emg2_out; +double emg2_norm; vector<double> emg2_cal; int emg2_cal_size; @@ -79,6 +81,7 @@ double emg3_factor; double emg3_th; double emg3_out; +double emg3_norm; vector<double> emg3_cal; int emg3_cal_size; @@ -218,13 +221,6 @@ double emg3_rectify = fabs( emg3_hp ); // Rectify emg3_env = bqc3_low.step( emg3_rectify ); // Filter lowpass (completes envelope) - // Set HIDScope outputs - scope.set(0, emg1_n ); // One notch filtered EMG to check 50Hz disturbance - scope.set(1, emg1_env ); - scope.set(2, emg2_env ); - scope.set(3, emg2_env ); - scope.send(); - if (calibrateNow == true) { // Only add values to EMG vectors if calibration flag is true emg1_cal.push_back(emg1_env); // Add values to calibration vector // emg1_cal_size = emg1_cal.size(); // Used for debugging @@ -318,14 +314,14 @@ button2.fall( NULL ); // Disable interrupt during calibration emg_curr_state = emg_cal_MVC; // Set next state emg_state_changed = true; // Enable entry functions - + } else if ( button2_pressed ) { button2_pressed = false; // Disable button pressed function until next button press button1.fall( NULL ); // Disable interrupt during calibration button2.fall( NULL ); // Disable interrupt during calibration emg_curr_state = emg_cal_rest; // Set next state emg_state_changed = true; // Enable entry functions - + } else if ( emg_MVC_cal_done && emg_rest_cal_done ) { button1.fall( NULL ); // Disable interrupt during operation button2.fall( NULL ); // Disable interrupt during operation @@ -352,8 +348,15 @@ emg3_cal.reserve(Fs * Tcal); } - // Do nothing until end condition is met - + // Do stuff until end condition is met + // Set HIDScope outputs + scope.set(0, emg1 ); // One notch filtered EMG to check 50Hz disturbance + scope.set(1, emg1_env ); + scope.set(2, emg2_env ); + scope.set(3, emg2_env ); + scope.send(); + + // State transition guard if ( timerCalibration.read() >= Tcal ) { // After interval Tcal the calibration step is finished sampleNow = false; // Disable signal sampling in sampleSignal() @@ -380,15 +383,15 @@ 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 - + sampleNow = true; // Enable signal sampling in sampleSignal() calibrateNow = false; // Disable calibration vector functionality in sampleSignal() } // Do stuff until end condition is met - double emg1_norm = emg1_env * emg1_factor; // Normalize EMG signal with calibrated factor - double emg2_norm = emg2_env * emg2_factor; // Idem - double emg3_norm = emg3_env * emg3_factor; // Idem + emg1_norm = emg1_env * emg1_factor; // Normalize EMG signal with calibrated factor + emg2_norm = emg2_env * emg2_factor; // Idem + emg3_norm = emg3_env * emg3_factor; // Idem // Set normalized EMG output signal (CAN BE MOVED TO EXTERNAL FUNCTION BECAUSE IT IS REPEATED 3 TIMES) if ( emg1_norm < emg1_th ) { // If below threshold, emg_out = 0 (ignored) @@ -420,12 +423,14 @@ } // Set HIDScope outputs - scope.set(0, emg1_env ); // One envelope for reference - scope.set(1, emg1_out ); - scope.set(2, emg2_out ); - scope.set(3, emg3_out ); + scope.set(0, emg1 ); + scope.set(1, emg1_env ); + scope.set(2, emg1_norm ); + scope.set(3, emg1_out ); + //scope.set(2, emg2_out ); + //scope.set(3, emg3_out ); scope.send(); - + led_g = !led_g; @@ -507,7 +512,10 @@ // Show that system is running // led_g = !led_g; // pc.printf("currentState: %i vectors size: %i %i %i\r\n", emg_curr_state, emg1_cal_size, emg2_cal_size, emg3_cal_size); - pc.printf("emg_state: %i emg1_env: %f emg1_out: %f emg1_th: %f emg1_factor: %f\r\n", emg_curr_state, emg1_env, emg1_out, emg1_th, emg1_factor); + pc.printf("emg_state: %i emg1_env: %f emg1_out: %f emg1_th: %f emg1_factor: %f\r\n", emg_curr_state, emg1_env, emg1_out, emg1_th, emg1_factor); + pc.printf(" emg1_MVC: %f emg1_rest: %f \r\n", emg1_MVC, emg1_rest); + //pc.printf(" emg2_env: %f emg2_out: %f emg2_th: %f emg2_factor: %f\r\n", emg2_env, emg2_out, emg2_th, emg2_factor); + //pc.printf(" emg3_env: %f emg3_out: %f emg3_th: %f emg3_factor: %f\r\n", emg3_env, emg3_out, emg3_th, emg3_factor); wait(0.5f); } } \ No newline at end of file