![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
..
Dependencies: HIDScope MODSERIAL biquadFilter mbed
main.cpp
- Committer:
- MarijkeZondag
- Date:
- 2018-10-19
- Revision:
- 11:b95b0e9e1b89
- Parent:
- 10:39ec51206c8b
- Child:
- 12:eaed305a76c3
File content as of revision 11:b95b0e9e1b89:
#include "mbed.h" #include "MODSERIAL.h" #include "BiQuad.h" #include "HIDScope.h" #include <math.h> AnalogIn emg0_in (A0); AnalogIn emg1_in (A1); AnalogIn emg2_in (A2); DigitalIn button2 (D10); //Let op, is deze niet bezet? InterruptIn encoderA (D9); InterruptIn encoderB (D8); DigitalOut directionpin1 (D4); DigitalOut directionpin2 (D7); PwmOut pwmpin1 (D5); PwmOut pwmpin2 (D6); MODSERIAL pc(USBTX, USBRX); //Global variables int encoder = 0; //Biquad BiQuadChain emg0band; BiQuad emg0band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 ); BiQuad emg0band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 ); BiQuad emg0band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 ); BiQuadChain emg1band; BiQuad emg1band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 ); BiQuad emg1band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 ); BiQuad emg1band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 ); BiQuadChain emg2band; BiQuad emg2band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 ); BiQuad emg2band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 ); BiQuad emg2band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 ); BiQuad notch1( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter //Tickers Ticker emg_filter_tick; Ticker Mov_av_tick; //Functions void EMGFilter0() { double emg0 = emg0_in.read(); double bandpass0 = emg0band.step(emg0); double absolute0 = fabs(bandpass0); double notch0 = notch1.step(absolute0); } void EMGFilter1() { double emg1 = emg1_in.read(); double bandpass1 = emg1band.step(emg1); double absolute1 = fabs(bandpass1); double notch1 = notch1.step(absolute1); } void EMGFilter2() { double emg2 = emg2_in.read(); double bandpass2 = emg2band.step(emg2); double absolute2 = fabs(bandpass2); double notch2 = notch1.step(absolute2); } void encoderA_rise() { if(encoderB==false) { encoder++; } else { encoder--; } } void encoderA_fall() { if(encoderB==true) { encoder++; } else { encoder--; } } void encoderB_rise() { if(encoderA==true) { encoder++; } else { encoder--; } } void encoderB_fall() { if(encoderA==false) { encoder++; } else { encoder--; } } // Main function start. int main() { pc.baud(115200); pc.printf("hello\n\r"); //EMG signaal filteren // Deze? of die hierondbqc.add( &bq1 ).add( &bq2 ).add( &bq3 ); bqc = bq1 * bq2 * bq3; emgSampleTicker.attach(&emgSample,0.01); //100 Hz while(true){/*do not return from main()*/} pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz encoderA.rise(&encoderA_rise); encoderA.fall(&encoderA_fall); encoderB.rise(&encoderB_rise); encoderB.fall(&encoderB_fall); while (true) { //Motor aansturen en encoder uitlezen float u1 = potmetervalue1; float u2 = potmetervalue2; float m1 = ((u1*2.0f)-1.0f); float m2 = ((u2*2.0f)-1.0f); pwmpin1 = fabs(m1*0.6f)+0.4f; //pwm duty cycle can only be positive, floating, 0.4f is "inefficiënt", dit tellen we erbij op, en keer 0.6 om te corrigeren voor de helling. directionpin1.write(m1>0); //Indien waar, motor draait rechtsom. Indien niet waar, motor draait linksom. wait(0.01f); //zodat de code niet oneindig doorgaat. pwmpin2 = fabs(m2*0.6f)+0.4f; directionpin2.write(m2>0); float encoderDegrees = float(encoder)*(360.0/8400.0); pc.printf("Encoder count: %f \n\r",encoderDegrees); } }