State machine
Dependencies: HIDScope QEI biquadFilter mbed
Fork of State_machine by
main.cpp
- Committer:
- CasperK
- Date:
- 2018-11-02
- Revision:
- 6:344e075c1221
- Parent:
- 5:e96d03bd557c
- Child:
- 7:af586102d80c
File content as of revision 6:344e075c1221:
#include "mbed.h" #include <stdio.h> #include "QEI.h" #include "HIDScope.h" //#include "MODSERIAL.h" #include "BiQuad.h" #include "math.h" #include "Constants.h" #define IGNORECOUNT 100 /* * Calibration functions */ void positionCalibration() { while(!button1) { directionpin1 = true; pwm_value1 = 0.7f; } pwm_value1 = 0.0f; while(!button2) { directionpin2 = true; pwm_value2 = 0.6f; } pwm_value2 = 0.0f; if(!next_switch) { CurrentState = Movement; motor1.reset(); motor2.reset(); } } void CalibrateEMG0(){ // calibrates only one muscle activities as testing concluded that it was unnessecary to do this for all muscles. ledred = !ledred; 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(emg1.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); } Calibration0 = (A0+A1+A2+A3+A4)/5*0.4; // average of the 5 highest values x 0,4 to create the threshold ledred = !ledred; } void CalibrateEMG1(){ // calibrates only one muscle activities as testing concluded that it was unnessecary to do this for all muscles. ledgreen = !ledgreen; int C = 500; // half a second at 1000Hz double A0=0, A1=0, A2=0, A3=0, A4=0; double emg1FiAbs; while (C > 0){ emg1FiAbs = fabs( emg1bqc.step(emg1.read())); if (C==500){ //first instance make all values the first in case this is the highest A0=A1=A2=A3=A4=emg1FiAbs; } else if(emg1FiAbs > A0){ // if there is a higher value change the inputs to be the highest 5 A4=A3; A3=A2; A2=A1; A1=A0; A0=emg1FiAbs; } C--; wait(0.001f); } Calibration1 = (A0+A1+A2+A3+A4)/5*0.4; // average of the 5 highest values x 0,4 to create the threshold ledgreen = !ledgreen; } void CalibrateEMG2(){ // calibrates only one muscle activities as testing concluded that it was unnessecary to do this for all muscles. ledblue = !ledblue; int C = 500; // half a second at 1000Hz double A0=0, A1=0, A2=0, A3=0, A4=0; double emg2FiAbs; while (C > 0){ emg2FiAbs = fabs( emg2bqc.step(emg2.read())); if (C==500){ //first instance make all values the first in case this is the highest A0=A1=A2=A3=A4=emg2FiAbs; } else if(emg2FiAbs > A0){ // if there is a higher value change the inputs to be the highest 5 A4=A3; A3=A2; A2=A1; A1=A0; A0=emg2FiAbs; } C--; wait(0.001f); } Calibration2 = (A0+A1+A2+A3+A4)/5*0.4; // average of the 5 highest values x 0,4 to create the threshold ledblue = !ledblue; } /* * filter functions */ bool emg0Filter(void){ double emg0filteredAbs = fabs( emg0bqc.step(emg0.read())); // Filter and make absolute, /* this is the threshhold */ if (emg0filteredAbs > Calibration0) { // when above threshold set bool to 1, here can the parameters be changed using global variables emg0Bool = 1; emg0Ignore = IGNORECOUNT; // here is the counter reset to catch sudden jolts } 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 } return emg0Bool; } bool emg1Filter(void){ double emg1filteredAbs = fabs( emg1bqc.step(emg1.read())); // Filter and make absolute /* this is the threshhold */ if (emg1filteredAbs > Calibration1) { // 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 > Calibration2) { // 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() { //run the filter functions for each emg signal emg0Filter(); emg1Filter(); emg2Filter(); } /* * main function for the emg calibration */ void emgCalibration() { // combining biquad chains is done in main, before the ticker emg0bqc.add( &emg0bq1 ).add( &emg0bq2 ).add ( &emg0bq3 ); emg1bqc.add( &emg1bq1 ).add( &emg1bq2 ).add ( &emg1bq3 ); emg2bqc.add( &emg2bq1 ).add( &emg2bq2 ).add ( &emg2bq3 ); bool u = true; CalibrationState = EMG0; while (u){ // !!! this is a place holder for the calibration!!! switch(CalibrationState) { case EMG0: if(!next_switch) { CalibrateEMG0(); // calibration function CalibrationState = EMG1; } break; case EMG1: if(!next_switch) { CalibrateEMG1(); // calibration function CalibrationState = EMG2; } break; case EMG2: if(!next_switch) { CalibrateEMG2(); // calibration function CurrentState = PositionCalibration; sample_timer.attach(&sample, 0.001); //ticker at 1000Hz ledred = false; // to indicate filter is working ledgreen = false; ledblue = false; u = false; } break; } } } /* * function that runs the Kinematics and PID controller */ void movement() { } void move_motors() { // assign the caltulates pwm values to the pwm pins pwmpin1 = pwm_value1; pwmpin2 = pwm_value2; } /* * the "main" function that runs the state machine */ void StateFunction() { switch(CurrentState) { case EmgCalibration: ledgreen = false; //turn on the green light emgCalibration(); break; case PositionCalibration: ledgreen = true; ledblue = false; //turn on the blue light positionCalibration(); break; case Movement: ledred = true; ledgreen = false; //turn on a cyan light ledblue = false; movement(); break; } } void KillInterrupt() { //turn off the motors pwm_value1 = 0; pwm_value2 = 0; //detach all the tickers MotorsTicker.detach(); sample_timer.detach(); //burn the red light ledred = false; ledgreen = true; ledblue = true; wait(2.0f); //give the person time to release the button bool b = true; //have the program get stuck in a while loop //to be sure that when an interrupt changes the pwmvalues somehow, they get turned off immediately while(b){ pwm_value1 = 0; pwm_value2 = 0; if (!next_switch){ CurrentState = PositionCalibration; b = false; } } StateFunction(); //get back to the state function } int main() { pwmpin1.period_us(60); pwmpin2.period_us(60); directionpin1 = true; directionpin2 = true; ledred = true; ledgreen = true; ledblue = true; kill_switch.fall(KillInterrupt); //attach the kill switch to the KillInterrupt function MotorsTicker.attach(&move_motors, 0.02f); //ticker at 50Hz CurrentState = EmgCalibration; //start at the calibration state while (true) { StateFunction(); //keep running the state machine } }