De hele robot in 1 keer bam
Dependencies: mbed QEI Servo HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 24:540c284e881d
- Parent:
- 23:8a0a0b959af1
- Child:
- 25:a1be4cf2ab0b
--- a/main.cpp Fri Oct 25 10:17:21 2019 +0000 +++ b/main.cpp Fri Oct 25 12:02:55 2019 +0000 @@ -62,9 +62,8 @@ // Initialize tickers and timeouts Ticker tickSample; Ticker tickSampleCalibration; +Ticker tickGlobal; // Set global ticker Timer timerCalibration; -Timeout timeoutCalibrationMVC; -Timeout timeoutCalibrationRest; /* ------ GLOBAL VARIABLES ------ @@ -153,32 +152,10 @@ /* ------ TICKER FUNCTIONS ------ */ -/* -// Read samples, filter samples and output to HIDScope -void sample() +void tickGlobalFunc() { - // Read EMG inputs - emg1 = emg1_in.read(); - emg2 = emg2_in.read(); - emg3 = emg3_in.read(); - - // Output raw EMG input - scope.set(0, emg1 ); - - // Filter notch and highpass - double emg1_n_hp = bqc1_notch_high.step( emg1 ); - - // Rectify - double emg1_rectify = fabs( emg1_n_hp ); - - // Filter lowpass (completes envelope) - double emg1_env = bqc1_low.step( emg1_rectify ); - - // Output EMG after filters - scope.set(1, emg1_env ); - scope.send(); + } -*/ void sampleCalibration() { @@ -241,37 +218,23 @@ vector<double>().swap(emg2_cal); // Empty vector to prevent memory overflow } -// Finish up calibration in rest -void calibrationRestFinished() -{ - tickSampleCalibration.detach(); // Stop calibration ticker to remove interrupt - 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 - - - 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 - led_b = 1; // Turn off calibration led -} - // Run calibration of EMG void do_emg_cal() { if ( emg_state_changed == true ) { emg_state_changed == false; + pc.printf("Starting calibration"); led_b = 0; // Turn on calibration led timerCalibration.reset(); timerCalibration.start(); switch( emg_curr_state ) { case emg_cal_MVC: - timeoutCalibrationMVC.attach( &calibrationMVCFinished, Tcal); // Stop MVC calibration after interval + pc.printf("MVC Calibration"); tickSampleCalibration.attach( &sampleCalibration, Ts ); // Start sample ticker break; case emg_cal_rest: - timeoutCalibrationRest.attach( &calibrationRestFinished, Tcal); // Stop rest calibration after interval + pc.printf("Rest calibration"); tickSampleCalibration.attach( &sampleCalibration, Ts ); // Start sample ticker break; } @@ -279,30 +242,19 @@ // Allemaal dingen doen tot de end conditions true zijn - if ( timerCalibration.read() >= Tcal ) { + if ( timerCalibration.read() >= Tcal ) { // After interval Tcal the calibration step is finished tickSampleCalibration.detach(); // Stop calibration ticker to remove interrupt calibrationFinished(); // Process calibration data led_b = 1; // Turn off calibration led - - emg_curr_state == emg_wait; - stateChanged == true; + emg_curr_state == emg_wait; // Set next state + stateChanged == true; // State has changed (to run 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 -} -*/ - // Determine scale factors for operation mode void makeScale() { @@ -370,6 +322,8 @@ } else { led_r = 0; // LED on } + + tickGlobal.attach( button1.fall( &calibrationMVC ); // Run MVC calibration on button press button2.fall( &calibrationRest ); // Run rest calibration on button press