De hele robot in 1 keer bam
Dependencies: mbed QEI Servo HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 8:ea3de43c9e8b
- Parent:
- 7:7a088536f1c9
- Child:
- 9:f6a935be28e1
- Child:
- 10:97a79aa10a56
--- a/main.cpp Mon Oct 21 09:21:17 2019 +0000 +++ b/main.cpp Mon Oct 21 09:57:42 2019 +0000 @@ -2,7 +2,7 @@ #include "mbed.h" //Base library #include "HIDScope.h" // to see if program is working and EMG is filtered properly // #include "QEI.h"// is needed for the encoder -// #include "MODSERIAL.h"// in order for connection with the pc +#include "MODSERIAL.h"// in order for connection with the pc #include "BiQuad.h" // #include "FastPWM.h" // #include "Arduino.h" //misschien handig omdat we het EMG arduino board gebruiken (?) @@ -12,7 +12,7 @@ // PC serial connection HIDScope scope( 2 ); -// MODSERIAL pc(USBTX, USBRX); +MODSERIAL pc(USBTX, USBRX); //EMG inputs definieren AnalogIn emg1_in (A1); //emg van rechterbicep, voor de x-richting @@ -22,6 +22,11 @@ // LED DigitalOut led_g(LED_GREEN); DigitalOut led_r(LED_RED); +DigitalOut led_b(LED_BLUE); + +// Buttons +InterruptIn button1(D11); +InterruptIn button2(D10); //variablen voor EMG double emg1; @@ -59,9 +64,27 @@ 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 stdev(const vector<double> &vect) +double getMean(const vector<double> &vect) { - + double sum = 0.0; + int vect_n = vect.size(); + + for ( int i = 0; i < vect_n; i++ ) { + sum += vect[i]; + } + return sum/vect_n; +} + +double getStdev(const vector<double> &vect, const double vect_mean) +{ + double sum2 = 0.0; + int vect_n = vect.size(); + + for ( int i = 0; i < vect_n; i++ ) { + sum2 += pow( vect[i] - vect_mean, 2 ); + } + double output = sqrt( sum2 / vect_n ); + return output; } // Check if filters are stable @@ -129,26 +152,37 @@ 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, 10.0f); + timeoutCalibrationMVC.attach( &calibrationMVCFinished, 5.0f); tickSampleCalibration.attach( &sampleCalibration, Ts ); + led_b = 0; } void main() { + pc.baud(115200); + pc.printf("Starting\r\n"); // Initialize sample ticker - tickSample.attach(&sample, Ts); + // tickSample.attach(&sample, Ts); // Create BQ chains to reduce computations bqc_notch_high.add( &bq_notch ).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; + // If any filter chain is unstable, red led will light up if (checkBQChainStable) { led_r = 1; // LED off @@ -156,10 +190,12 @@ led_r = 0; // LED on } + button1.fall( &calibrationMVC ); + while(true) { // Show that system is running - led_g = !led_g; + // led_g = !led_g; wait(0.5); } } \ No newline at end of file