De hele robot in 1 keer bam
Dependencies: mbed QEI Servo HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 22:9079c6c0d898
- Parent:
- 21:e4569b47945e
- Child:
- 23:8a0a0b959af1
--- a/main.cpp Fri Oct 25 09:57:50 2019 +0000 +++ b/main.cpp Fri Oct 25 10:11:29 2019 +0000 @@ -31,6 +31,7 @@ // 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 +bool emg_state_changed; // Global variables for EMG reading AnalogIn emg1_in (A1); // Right biceps, x axis @@ -61,6 +62,7 @@ // Initialize tickers and timeouts Ticker tickSample; Ticker tickSampleCalibration; +Timer timerCalibration; Timeout timeoutCalibrationMVC; Timeout timeoutCalibrationRest; @@ -216,17 +218,29 @@ */ // Finish up calibration of MVC -void calibrationMVCFinished() +void calibrationFinished() { - tickSampleCalibration.detach(); // Stop calibration ticker to remove interrupt + + switch( emg_curr_state ) { + case emg_cal_MVC: + emg1_MVC = getMean(emg1_cal); // Store MVC globally + emg1_MVC_stdev = getStdev(emg1_cal, emg1_MVC); // Store MVC stde globally + vector<double>().swap(emg1_cal); // Empty vector to prevent memory overflow - emg1_MVC = getMean(emg1_cal); // Store MVC globally - emg1_MVC_stdev = getStdev(emg1_cal, emg1_MVC); // Store MVC stde globally - 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 + vector<double>().swap(emg2_cal); // Empty vector to prevent memory overflow + break; + case emg_cal_rest: + emg1_MVC = getMean(emg1_cal); // Store MVC globally + emg1_MVC_stdev = getStdev(emg1_cal, emg1_MVC); // Store MVC stde globally + vector<double>().swap(emg1_cal); // Empty vector to prevent memory overflow - emg2_MVC = getMean(emg2_cal); // Store MVC globally - emg2_MVC_stdev = getStdev(emg2_cal, emg2_MVC); // Store MVC stde globally - vector<double>().swap(emg2_cal); + emg2_MVC = getMean(emg2_cal); // Store MVC globally + emg2_MVC_stdev = getStdev(emg2_cal, emg2_MVC); // Store MVC stde globally + vector<double>().swap(emg2_cal); // Empty vector to prevent memory overflow + break; + } led_b = 1; // Turn off calibration led } @@ -249,10 +263,13 @@ // Run calibration of EMG void do_emg_cal() { - if (stateChanged == true) { - stateChanged == false; + if ( emg_state_changed == true ) { + emg_state_changed == false; led_b = 0; // Turn on calibration led - switch(emg_curr_state) { + timerCalibration.reset(); + timerCalibration.start(); + + switch( emg_curr_state ) { case emg_cal_MVC: timeoutCalibrationMVC.attach( &calibrationMVCFinished, Tcal); // Stop MVC calibration after interval tickSampleCalibration.attach( &sampleCalibration, Ts ); // Start sample ticker @@ -265,100 +282,102 @@ // Allemaal dingen doen tot de end conditions true zijn - if (timer klaar) { + if ( timerCalibration.read() >= Tcal ) { + tickSampleCalibration.detach(); // Stop calibration ticker to remove interrupt + + calibrationFinished(); + emg_curr_state == emg_wait; stateChanged == true; + pc.printf("Calibration step finished"); + } } -} -/* -// Run calibration in rest -void calibrationRest() -{ - timeoutCalibrationRest.attach( &calibrationRestFinished, Tcal); // Stop rest calibration after interval - tickSampleCalibration.attach( &sampleCalibration, Ts ); // Start sample ticker - led_b = 0; // Turn on calibration led -} -*/ + /* + // Run calibration in rest + void calibrationRest() + { + timeoutCalibrationRest.attach( &calibrationRestFinished, Tcal); // Stop rest calibration after interval + tickSampleCalibration.attach( &sampleCalibration, Ts ); // Start sample ticker + led_b = 0; // Turn on calibration led + } + */ // 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 + 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); -} + pc.printf("Factor: %f TH: %f\r\n", factor1, emg1_th); + } -/* ------- 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() -{ - pc.baud(115200); // MODSERIAL rate - pc.printf("Starting\r\n"); - - // tickSample.attach(&sample, Ts); // Initialize sample ticker - - // Create BQ chains to reduce computations - bqc1_notch.add( &bq1_notch ); - bqc1_high.add( &bq1_H1 ).add( &bq1_H2 ); - bqc1_low.add( &bq1_L1 ).add( &bq1_L2 ); - - bqc2_notch.add( &bq2_notch ); - bqc2_high.add( &bq2_H1 ).add( &bq2_H2 ); - bqc2_low.add( &bq2_L1 ).add( &bq2_L2 ); - - bqc3_notch.add( &bq3_notch ); - bqc3_high.add( &bq3_H1 ).add( &bq3_H2 ); - bqc3_low.add( &bq3_L1 ).add( &bq3_L2 ); - - led_b = 1; // Turn blue led off at startup - led_g = 1; // Turn green led off at startup - led_r = 1; // Turn red led off at startup - - // If any filter chain is unstable, red led will light up - if (checkBQChainStable) { - led_r = 1; // LED off - } else { - led_r = 0; // LED on + /* + ------ 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; + } } - button1.fall( &calibrationMVC ); // Run MVC calibration on button press - button2.fall( &calibrationRest ); // Run rest calibration on button press - button3.fall( &makeScale ); // Create scale factors and close calibration at button press + void main() { + pc.baud(115200); // MODSERIAL rate + pc.printf("Starting\r\n"); + + // tickSample.attach(&sample, Ts); // Initialize sample ticker - while(true) { + // Create BQ chains to reduce computations + bqc1_notch.add( &bq1_notch ); + bqc1_high.add( &bq1_H1 ).add( &bq1_H2 ); + bqc1_low.add( &bq1_L1 ).add( &bq1_L2 ); + + bqc2_notch.add( &bq2_notch ); + bqc2_high.add( &bq2_H1 ).add( &bq2_H2 ); + bqc2_low.add( &bq2_L1 ).add( &bq2_L2 ); + + bqc3_notch.add( &bq3_notch ); + bqc3_high.add( &bq3_H1 ).add( &bq3_H2 ); + bqc3_low.add( &bq3_L1 ).add( &bq3_L2 ); - // Show that system is running - // led_g = !led_g; - 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 + led_b = 1; // Turn blue led off at startup + led_g = 1; // Turn green led off at startup + led_r = 1; // Turn red led off at startup + + // If any filter chain is unstable, red led will light up + if (checkBQChainStable) { + led_r = 1; // LED off + } else { + led_r = 0; // LED on + } + + button1.fall( &calibrationMVC ); // Run MVC calibration on button press + button2.fall( &calibrationRest ); // Run rest calibration on button press + button3.fall( &makeScale ); // Create scale factors and close calibration at button press + + while(true) { + + // Show that system is running + // led_g = !led_g; + 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