State machine
Dependencies: HIDScope QEI biquadFilter mbed
Fork of State_machine by
main.cpp
- Committer:
- CasperK
- Date:
- 2018-10-31
- Revision:
- 3:ed4676f76a5c
- Parent:
- 2:3f67b4833256
- Child:
- 4:dfe39188f2b2
File content as of revision 3:ed4676f76a5c:
#include "mbed.h" #include "QEI.h" #include "HIDScope.h" #include "MODSERIAL.h" #include "BiQuad.h" #include "math.h" #define IGNORECOUNT 100 PwmOut pwmpin1(D6); PwmOut pwmpin2(D5); AnalogIn potmeter1(A5); AnalogIn potmeter2(A4); DigitalIn button1(D2); DigitalIn button2(D3); DigitalOut directionpin1(D4); DigitalOut directionpin2(D7); QEI motor1(D13,D12,NC, 32); QEI motor2(D11,D10,NC, 32); //Define objects AnalogIn emg0( A0 ); // EMG at A0 BiQuad emg0bq1(0.8848578, -1.7697156, 0.8848578, -1.7539023, 0.7855289); // highpass at 30Hz Q at around 1 BiQuad emg0bq2(0.0773021,0.1546042,0.0773021,-1.3098283,0.6190368); // lowpass at 130 Hz Q at around .6 BiQuad emg0bq3(0.9556457,-1.81774618, 0.955645, -1.817746, 0.9112914); // 50 Hz notch Q at 4.5 BiQuadChain emg0bqc1; // merged chain of three filters BiQuadChain emg0bqc2; BiQuadChain emg0bqc3; AnalogIn emg1( A1 ); // EMG at A1 BiQuad emg1bq1(0.8848578, -1.7697156, 0.8848578, -1.7539023, 0.7855289); // highpass at 30Hz Q at around 1 BiQuad emg1bq2(0.0773021,0.1546042,0.0773021,-1.3098283,0.6190368); // lowpass at 130 Hz Q at around .6 BiQuad emg1bq3(0.9556457,-1.81774618, 0.955645, -1.817746, 0.9112914); // 50 Hz notch Q at 4.5 BiQuadChain emg1bqc; // merged chain of three filters AnalogIn emg2( A2 ); // EMG at A2 BiQuad emg2bq1(0.8848578, -1.7697156, 0.8848578, -1.7539023, 0.7855289); // highpass at 30Hz Q at around 1 BiQuad emg2bq2(0.0773021,0.1546042,0.0773021,-1.3098283,0.6190368); // lowpass at 130 Hz Q at around .6 BiQuad emg2bq3(0.9556457,-1.81774618, 0.955645, -1.817746, 0.9112914); // 50 Hz notch Q at 4.5 BiQuadChain emg2bqc; // merged chain of three filters DigitalIn kill_switch(SW2); //position has to be changed DigitalIn next_switch(SW3); //name and position should be replaced enum states{PositionCalibration, EmgCalibration, Movement, KILL}; states CurrentState; Ticker sample_timer; Ticker MotorsTicker; Timer timer; //for testing purposes DigitalOut ledred(LED_RED); DigitalOut ledgreen(LED_GREEN); DigitalOut ledblue(LED_BLUE); MODSERIAL pc(USBTX, USBRX); HIDScope scope(5); bool emg0Bool = 0; // I don't know if these NEED to be global, but when I tried to put them in they wouldn't work... int emg0Ignore = 0; bool emg1Bool = 0; int emg1Ignore = 0; bool emg2Bool = 0; int emg2Ignore = 0; double input = 0; // raw input double filtHigh = 0; // filtered after highpass double filtlow = 0; // filtered after lowpass double filtNotch = 0; // filtered after notch double emg0filteredAbs; float threshold0; float threshold1; float threshold2; volatile float pwm_value1 = 0.0; volatile float pwm_value2 = 0.0; /** Sample functions * these functions sample the emg and send it to HIDScope **/ bool emg0Filter(void){ input = emg0.read(); scope.set( 0, input); filtHigh = emg0bqc1.step(emg0.read()); scope.set( 1, filtHigh); filtlow = emg0bqc2.step(emg0.read()); scope.set( 2, filtlow); filtNotch = emg0bqc3.step(emg0.read()); scope.set( 3, filtNotch); /* this is the threshhold */ emg0filteredAbs = fabs(filtNotch); if (emg0filteredAbs > threshold0) { // when above threshold set bool to 1, here can the parameters be changed using global variables emg0Bool = 1; emg0Ignore = IGNORECOUNT; // here is the counter increased ( at 1000 Hz, this is 0.1 sec) } else if (emg0Ignore < 0){ // if the ignore-counter is down to zero, set the bool back to 0 emg0Bool = 0; } else { emg0Ignore--; // else decrease counter by one each time has passed without threshold being met } scope.set( 4, emg0Bool); scope.send(); return emg0Bool; } bool emg1Filter(void){ double emg1filteredAbs = fabs( emg1bqc.step(emg1.read())); // Filter and make absolute /* this is the threshhold */ if (emg1filteredAbs > threshold1) { // when above threshold set bool to 1 here can the parameters be changed using global variables emg1Bool = true; emg1Ignore = IGNORECOUNT; // here is the counter increased ( at 1000 Hz, this is 0.1 sec) } else if (emg1Ignore < 0){ // if the ignore-counter is down to zero, set the bool back to 0 emg1Bool = false; } else { emg1Ignore--; // else decrease counter by one each time has passed without threshold being met } return emg1Bool; } bool emg2Filter(void){ double emg2filteredAbs = fabs( emg2bqc.step(emg2.read())); // Filter and make absolute /* this is the threshhold */ if (emg2filteredAbs > threshold2) { // when above threshold set bool to 1 here can the parameters be changed using global variables emg2Bool = true; emg2Ignore = IGNORECOUNT; // here is the counter increased ( at 1000 Hz, this is 0.1 sec) } else if (emg2Ignore < 0){ // if the ignore-counter is down to zero, set the bool back to 0 emg2Bool = false; } else { emg2Ignore--; // else decrease counter by one each time has passed without threshold being met } return emg2Bool; } void sample() { bool Bool1 = emg0Filter(); // whatever name casper uses for the bool // bool Bool2 = emg1Filter(); // bool Bool3 = emg2Filter(); } void positionCalibration() { while(!button1){ directionpin1 = true; pwm_value1 = 0.7f; } pwm_value1 = 0.0f; while(!button2){ directionpin2 = true; pwm_value2 = 0.7f; } pwm_value2 = 0.0f; // pwm_value1 = potmeter1; // pwm_value2 = potmeter2; if (!next_switch) { CurrentState = EmgCalibration; pc.printf("current state = EmgCalibration\n\r"); } } void emg0Calibration() { int C = 500; // half a second at 1000Hz double A0=0, A1=0, A2=0, A3=0, A4=0; double emg0FiAbs; while (C > 0){ emg0FiAbs = fabs( emg1bqc.step(emg0.read())); if (C==500){ //first instance make all values the first in case this is the highest A0=A1=A2=A3=A4=emg0FiAbs; } else if(emg0FiAbs > A0){ // if there is a higher value change the inputs to be the highest 5 A4=A3; A3=A2; A2=A1; A1=A0; A0=emg0FiAbs; } C--; wait(0.001f); threshold0 = (A0+A1+A2+A3+A4)/5*0.4; // average of the 5 highest values x 0,4 to create the threshold } if (!next_switch) { CurrentState = Movement; pc.printf("current state = Movement\n\r"); } } void emg1Calibration() { } void emg2Calibration() { } void movement() { } void move_motors() { pwmpin1 = pwm_value1; pwmpin2 = pwm_value2; } int main() { pc.baud(115200); pc.printf(" ** program reset **\n\r"); pwmpin1.period_us(60); pwmpin2.period_us(60); directionpin1 = true; directionpin2 = true; ledred = true; ledgreen = true; ledblue = true; // emg filters // combining biquad chains is done in main, before the ticker, so only once. emg0bqc1.add( &emg0bq1 ); emg0bqc2.add( &emg0bq1 ).add( &emg0bq2 ); emg0bqc3.add( &emg0bq1 ).add( &emg0bq2 ).add ( &emg0bq3 ); // combining biquad chains is done in main, before the ticker, so only once. MotorsTicker.attach(&move_motors, 0.02f); //ticker at 50Hz sample_timer.attach(&sample, 0.001); //ticker at 1000Hz CurrentState = PositionCalibration; pc.printf("current state = PositionCalibration\n\r"); while (true) { switch(CurrentState) { case PositionCalibration: positionCalibration(); if (!kill_switch) { CurrentState = KILL; pc.printf("current state = KILL\n\r"); } break; case EmgCalibration: emg0Calibration(); //emg1Calibration(); //emg2Calibration(); if (!kill_switch) { CurrentState = KILL; pc.printf("current state = KILL\n\r"); } break; case Movement: movement(); if (!kill_switch) { CurrentState = KILL; pc.printf("current state = KILL\n\r"); } break; case KILL: bool u = true; ledgreen = true; ledblue = true; ledred = false; pwm_value1 = 0; pwm_value2 = 0; timer.start(); if (timer.read_ms()> 2000) { timer.stop(); timer.reset(); while(u) { if (!kill_switch) { timer.start(); u = false; ledred = true; CurrentState = PositionCalibration; pc.printf("current state = PositionCalibration\n\r"); wait(1.0f); } } } break; } wait(0.2f); } }