De hele robot in 1 keer bam
Dependencies: mbed QEI Servo HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 9:f6a935be28e1
- Parent:
- 8:ea3de43c9e8b
--- a/main.cpp Mon Oct 21 09:57:42 2019 +0000 +++ b/main.cpp Mon Oct 21 11:57:25 2019 +0000 @@ -29,22 +29,24 @@ InterruptIn button2(D10); //variablen voor EMG -double emg1; -double emg2; -double emg3; - -vector<double> emg1_cal; - -int i_cal; +volatile double emg1; +volatile double emg2; +volatile double emg3; // Initialize tickers Ticker tickSample; Timeout timeoutCalibrationMVC; Ticker tickSampleCalibration; -// Sample rate -const double Fs = 500; //Hz -const double Ts = 1/Fs; //sec +// Initialize values +const double Fs = 100; // sampling frequency (Hz) +const double Ts = 1/Fs; // sampling time (s) +const double T_cal = 5.0f; // Calibration duration (s) + + +int i_cal = 0; // Global iterator of calibration +const int n_cal = Fs * T_cal; // Number of elements of calibration +double emg1_cal[n_cal]; // Initialize calibration array // Notch filter coefficients (iirnotch Q factor 35 @50Hz) from MATLAB in the following form: // b01 b11 b21 a01 a11 a21 @@ -64,10 +66,10 @@ 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 -double getMean(const vector<double> &vect) +double getMean(double vect[n_cal]) { double sum = 0.0; - int vect_n = vect.size(); + int vect_n = n_cal; for ( int i = 0; i < vect_n; i++ ) { sum += vect[i]; @@ -75,10 +77,10 @@ return sum/vect_n; } -double getStdev(const vector<double> &vect, const double vect_mean) +double getStdev(double vect[n_cal], double vect_mean) { double sum2 = 0.0; - int vect_n = vect.size(); + int vect_n = n_cal; for ( int i = 0; i < vect_n; i++ ) { sum2 += pow( vect[i] - vect_mean, 2 ); @@ -100,7 +102,7 @@ } } - +/* // Read samples, filter samples and output to HIDScope void sample() { @@ -124,12 +126,26 @@ // Output EMG after filters scope.set(1, emg1_env ); scope.send(); +} +*/ +void calibrationMVCFinished() +{ + // pc.printf("Stop calibration"); + tickSampleCalibration.detach(); + double emg1_mean = getMean(emg1_cal); + double emg1_stdev = getStdev(emg1_cal, emg1_mean); + led_b = 1; + + // pc.printf("EMG Mean: %f stdev: %f\r\n", emg1_mean, emg1_stdev); } void sampleCalibration() { + if (i_cal >= n_cal-1) { + calibrationMVCFinished(); + } // Read EMG inputs emg1 = emg1_in.read(); emg2 = emg2_in.read(); @@ -146,23 +162,14 @@ scope.set(1, emg1_env ); scope.send(); - emg1_cal.push_back(emg1_env); -} + emg1_cal[i_cal] = emg1_env; + i_cal++; -void calibrationMVCFinished() -{ - tickSampleCalibration.detach(); - double emg1_mean = getMean(emg1_cal); - double emg1_stdev = getStdev(emg1_cal, emg1_mean); - - led_b = 1; - - pc.printf("EMG Mean: %f stdev: %f\r\n", emg1_mean, emg1_stdev); } void calibrationMVC() { - timeoutCalibrationMVC.attach( &calibrationMVCFinished, 5.0f); + tickSampleCalibration.attach( &sampleCalibration, Ts ); led_b = 0; } @@ -171,8 +178,9 @@ void main() { - pc.baud(115200); + pc.baud(115200); pc.printf("Starting\r\n"); + // Initialize sample ticker // tickSample.attach(&sample, Ts);