De hele robot in 1 keer bam
Dependencies: mbed QEI Servo HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 13:2724d2e747f1
- Parent:
- 12:70f0710400c2
- Child:
- 14:3b6adb5000f1
--- a/main.cpp Mon Oct 21 14:36:24 2019 +0000 +++ b/main.cpp Tue Oct 22 07:49:24 2019 +0000 @@ -10,14 +10,13 @@ #include <vector> // For easy array management #include <numeric> // For manipulating array data -// PC serial connection -HIDScope scope( 3 ); -MODSERIAL pc(USBTX, USBRX); +/* +------ DEFINE MBED CONNECTIONS ------ +*/ -//EMG inputs definieren -AnalogIn emg1_in (A1); //emg van rechterbicep, voor de x-richting -AnalogIn emg2_in (A2); //emg van linkerbicep, voor de y-richting -AnalogIn emg3_in (A3); //emg van een derde (nog te bepalen) spier, voor het vernaderen van de richting +// PC serial connection +HIDScope scope( 5 ); +MODSERIAL pc(USBTX, USBRX); // LED DigitalOut led_g(LED_GREEN); @@ -29,52 +28,68 @@ InterruptIn button2(D10); InterruptIn button3(SW3); -//variablen voor EMG +// Global variables for EMG reading +AnalogIn emg1_in (A1); // Right biceps, x axis +AnalogIn emg2_in (A2); // Left biceps, y axis +AnalogIn emg3_in (A3); // Third muscle, TBD + double emg1; -double emg2; -double emg3; double emg1_MVC; double emg1_MVC_stdev; double emg1_rest; double emg1_rest_stdev; - vector<double> emg1_cal; +double emg2; +double emg2_MVC; +double emg2_MVC_stdev; +double emg2_rest; +double emg2_rest_stdev; +vector<double> emg2_cal; -// Initialize tickers +double emg3; +double emg3_MVC; +double emg3_MVC_stdev; +double emg3_rest; +double emg3_rest_stdev; +vector<double> emg3_cal; + +// Initialize tickers and timeouts Ticker tickSample; +Ticker tickSampleCalibration; Timeout timeoutCalibrationMVC; Timeout timeoutCalibrationRest; -Ticker tickSampleCalibration; -// Sample rate -const double Fs = 500; // Sampling frequency (s) -const double Ts = 1/Fs; // Sampling time (s) - +/* +------ GLOBAL VARIABLES ------ +*/ +const int Fs = 500; // Sampling frequency (s) const double Tcal = 10.0f; // Calibration duration (s) - -int trim_cal = 1; // Trim the beginning of the calibration vector to reduce transient behaviour by X seconds -int trim_cal_i = trim_cal * Fs - 1; +int trim_cal = 1; // Trim transient behaviour of calibration (s) -// Notch filter coefficients (iirnotch Q factor 35 @50Hz) from MATLAB in the following form: -// b01 b11 b21 a01 a11 a21 -BiQuad bq_notch( 0.995636295063941, -1.89829218816065, 0.995636295063941, 1, -1.89829218816065, 0.991272590127882); +// Calculate global variables +const double Ts = 1/Fs; // Sampling time (s) +int trim_cal_i = trim_cal * Fs - 1; // Determine iterator of transient behaviour trim + +// Notch biquad filter coefficients (iirnotch Q factor 35 @50Hz) from MATLAB: +BiQuad bq_notch( 0.995636295063941, -1.89829218816065, 0.995636295063941, 1, -1.89829218816065, 0.991272590127882); // b01 b11 b21 a01 a11 a21 BiQuadChain bqc_notch; -// Highpass filter coefficients (butter 4th order @10Hz cutoff) from MATLAB in the following form: -// b01 b11 b21 a01 a11 a21 -// b02 b12 b22 a02 a12 a22 -BiQuad bq_H1(0.922946103200875, -1.84589220640175, 0.922946103200875, 1, -1.88920703055163, 0.892769008131025); -BiQuad bq_H2(1, -2, 1, 1, -1.95046575793011, 0.954143234875078); -BiQuadChain bqc_high; // Used to chain two 2nd other filters into a 4th order filter +// Highpass biquad filter coefficients (butter 4th order @10Hz cutoff) from MATLAB +BiQuad bq_H1(0.922946103200875, -1.84589220640175, 0.922946103200875, 1, -1.88920703055163, 0.892769008131025); // b01 b11 b21 a01 a11 a21 +BiQuad bq_H2(1, -2, 1, 1, -1.95046575793011, 0.954143234875078); // b02 b12 b22 a02 a12 a22 +BiQuadChain bqc_high; -// Lowpass filter coefficients (butter 4th order @5Hz cutoff) from MATLAB in the following form: -// b01 b11 b21 a01 a11 a21 -// b02 b12 b22 a02 a12 a22 -BiQuad bq_L1(5.32116245737504e-08, 1.06423249147501e-07, 5.32116245737504e-08, 1, -1.94396715039462, 0.944882378004138); -BiQuad bq_L2(1, 2, 1, 1, -1.97586467534468, 0.976794920438162); -BiQuadChain bqc_low; // Used to chain two 2nd other filters into a 4th order filter +// Lowpass biquad filter coefficients (butter 4th order @5Hz cutoff) from MATLAB: +BiQuad bq_L1(5.32116245737504e-08, 1.06423249147501e-07, 5.32116245737504e-08, 1, -1.94396715039462, 0.944882378004138); // b01 b11 b21 a01 a11 a21 +BiQuad bq_L2(1, 2, 1, 1, -1.97586467534468, 0.976794920438162); // b02 b12 b22 a02 a12 a22 +BiQuadChain bqc_low; +/* +------ HELPER FUNCTIONS ------ +*/ + +// Return mean of vector double getMean(const vector<double> &vect) { double sum = 0.0; @@ -86,6 +101,7 @@ return sum/vect_n; } +// Return standard deviation of vector double getStdev(const vector<double> &vect, const double vect_mean) { double sum2 = 0.0; @@ -98,7 +114,7 @@ return output; } -// Check if filters are stable +// Check filter stability bool checkBQChainStable() { bool n_stable = bqc_notch.stable(); @@ -112,7 +128,9 @@ } } - +/* +------ TICKER FUNCTIONS ------ +*/ /* // Read samples, filter samples and output to HIDScope void sample() @@ -153,77 +171,82 @@ double emg1_n = bqc_notch.step( emg1 ); // Filter notch scope.set(1, emg1_n); double emg1_hp = bqc_high.step( emg1_n ); // Filter highpass + scope.set(2, emg1_n); double emg1_rectify = fabs( emg1_hp ); // Rectify + scope.set(3, emg1_n); double emg1_env = bqc_low.step( emg1_rectify ); // Filter lowpass (completes envelope) // Output EMG after filters - scope.set(2, emg1_env ); + scope.set(4, emg1_env ); scope.send(); emg1_cal.push_back(emg1_env); } +/* +------ EMG CALIBRATION FUNCTIONS ------ +*/ + +// Finish up calibration of MVC void calibrationMVCFinished() { - tickSampleCalibration.detach(); - - emg1_MVC = getMean(emg1_cal); - emg1_MVC_stdev = getStdev(emg1_cal, emg1_MVC); - - emg1_cal.clear(); - - led_b = 1; + 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 + led_b = 1; // Turn off calibration led } +// Run calibration of MVC void calibrationMVC() { - timeoutCalibrationMVC.attach( &calibrationMVCFinished, Tcal); - tickSampleCalibration.attach( &sampleCalibration, Ts ); - led_b = 0; + 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() { - tickSampleCalibration.detach(); - - emg1_rest = getMean(emg1_cal); - emg1_rest_stdev = getStdev(emg1_cal, emg1_rest); - - emg1_cal.clear(); - - led_b = 1; + 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 + led_b = 1; // Turn off calibration led } void calibrationRest() { - timeoutCalibrationRest.attach( &calibrationRestFinished, Tcal); - tickSampleCalibration.attach( &sampleCalibration, Ts ); - led_b = 0; + 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; - double factor1 = 1 / emg1_MVC; - double emg1_th = emg1_rest * factor1 + margin_percentage/100; + 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); } void main() { - pc.baud(115200); + pc.baud(115200); // MODSERIAL rate pc.printf("Starting\r\n"); - // Initialize sample ticker - // tickSample.attach(&sample, Ts); + + // tickSample.attach(&sample, Ts); // Initialize sample ticker // Create BQ chains to reduce computations bqc_notch.add( &bq_notch ); bqc_high.add( &bq_H1 ).add( &bq_H2 ); bqc_low.add( &bq_L1 ).add( &bq_L2 ); - led_b = 1; // Turn led off at startup - led_g = 1; + 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) { @@ -232,9 +255,9 @@ led_r = 0; // LED on } - button1.fall( &calibrationMVC ); - button2.fall( &calibrationRest ); - button3.fall( &makeScale ); + 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) {