De hele robot in 1 keer bam
Dependencies: mbed QEI Servo HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 21:e4569b47945e
- Parent:
- 20:0e9218673aa8
- Child:
- 22:9079c6c0d898
diff -r 0e9218673aa8 -r e4569b47945e main.cpp --- a/main.cpp Fri Oct 25 09:15:22 2019 +0000 +++ b/main.cpp Fri Oct 25 09:57:50 2019 +0000 @@ -29,7 +29,7 @@ InterruptIn button3(SW3); // EMG Substates -enum EMG_States { emg_wait, emg_cal_MVC, emg_cal_rest, emg_scale, emg_operation }; // Define EMG substates +enum EMG_States { emg_wait, emg_cal_MVC, emg_cal_rest, emg_check_cal, emg_make_scale, emg_operation }; // Define EMG substates EMG_States emg_curr_state; // Initialize EMG substate variable // Global variables for EMG reading @@ -219,28 +219,18 @@ void calibrationMVCFinished() { tickSampleCalibration.detach(); // Stop calibration ticker to remove interrupt + emg1_MVC = getMean(emg1_cal); // Store MVC globally emg1_MVC_stdev = getStdev(emg1_cal, emg1_MVC); // Store MVC stde globally - emg1_cal.clear(); // Empty vector to prevent memory overflow - emg1_cal.shrink_to_fit(); // Empty vector to prevent memory overflow - + vector<double>().swap(emg1_cal); emg2_MVC = getMean(emg2_cal); // Store MVC globally emg2_MVC_stdev = getStdev(emg2_cal, emg2_MVC); // Store MVC stde globally - emg2_cal.clear(); // Empty vector to prevent memory overflow - emg2_cal.shrink_to_fit(); // Empty vector to prevent memory overflow + vector<double>().swap(emg2_cal); led_b = 1; // Turn off calibration led } -// Run calibration of MVC -void calibrationMVC() -{ - timeoutCalibrationMVC.attach( &calibrationMVCFinished, Tcal); // Stop MVC calibration after interval - tickSampleCalibration.attach( &sampleCalibration, Ts ); // Start sample ticker - led_b = 0; // Turn on calibration led -} - // Finish up calibration in rest void calibrationRestFinished() { @@ -248,16 +238,41 @@ emg1_rest = getMean(emg1_cal); // Store rest globally emg1_rest_stdev = getStdev(emg1_cal, emg1_rest);// Store rest stdev globally emg1_cal.clear(); // Empty vector to prevent memory overflow - emg1_cal.shrink_to_fit(); // Empty vector to prevent memory overflow emg2_rest = getMean(emg2_cal); // Store rest globally emg2_rest_stdev = getStdev(emg2_cal, emg2_rest);// Store rest stdev globally emg2_cal.clear(); // Empty vector to prevent memory overflow - emg2_cal.shrink_to_fit(); // Empty vector to prevent memory overflow led_b = 1; // Turn off calibration led } +// Run calibration of EMG +void do_emg_cal() +{ + if (stateChanged == true) { + stateChanged == false; + led_b = 0; // Turn on calibration led + switch(emg_curr_state) { + case emg_cal_MVC: + timeoutCalibrationMVC.attach( &calibrationMVCFinished, Tcal); // Stop MVC calibration after interval + tickSampleCalibration.attach( &sampleCalibration, Ts ); // Start sample ticker + break; + case emg_cal_rest: + timeoutCalibrationRest.attach( &calibrationRestFinished, Tcal); // Stop rest calibration after interval + tickSampleCalibration.attach( &sampleCalibration, Ts ); // Start sample ticker + break; + } + + // Allemaal dingen doen tot de end conditions true zijn + + if (timer klaar) { + emg_curr_state == emg_wait; + stateChanged == true; + pc.printf("Calibration step finished"); + } +} + +/* // Run calibration in rest void calibrationRest() { @@ -265,6 +280,7 @@ tickSampleCalibration.attach( &sampleCalibration, Ts ); // Start sample ticker led_b = 0; // Turn on calibration led } +*/ // Determine scale factors for operation mode void makeScale() @@ -279,8 +295,29 @@ /* ------ EMG SUBSTATE MACHINE ------ */ - - +void emg_state_machine() +{ + switch(emg_curr_state) { + case emg_wait: + //do_emg_wait(); + break; + case emg_cal_MVC: + do_emg_cal(); + break; + case emg_cal_rest: + do_emg_cal(); + break; + case emg_check_cal: + //do_emg_check_cal(); + break; + case emg_make_scale: + //do_make_scale(); + break; + case emg_operation: + //do_emg_operation(); + break; + } +} void main() { @@ -321,7 +358,7 @@ // Show that system is running // led_g = !led_g; - pc.printf("Vector emg1_cal: %f vector emg2_cal: %f\r\n", emg1_cal[0], emg2_cal[0]); + pc.printf("Vector emg1_cal: %i vector emg2_cal: %i\r\n", emg1_cal.size(), emg2_cal.size()); wait(1.0f); } } \ No newline at end of file