M9 Biorobotics Group 8
/
CARD_program
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed QEI Servo HIDScope biquadFilter MODSERIAL FastPWM
Revision 22:9079c6c0d898, committed 2019-10-25
- Comitter:
- Jellehierck
- Date:
- Fri Oct 25 10:11:29 2019 +0000
- Parent:
- 21:e4569b47945e
- Child:
- 23:8a0a0b959af1
- Commit message:
- Still working on calibration state machine;
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- 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