Opzetje
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
Diff: OpzetjeBrechje.cpp
- Revision:
- 9:19682cb1d8ee
- Parent:
- 8:2fc7a3a7f09a
diff -r 2fc7a3a7f09a -r 19682cb1d8ee OpzetjeBrechje.cpp --- a/OpzetjeBrechje.cpp Thu Oct 03 12:58:44 2019 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,191 +0,0 @@ -#include "mbed.h" -//#include "HIDScope.h" -//#include "QEI.h" -#include "MODSERIAL.h" -//#include "BiQuad.h" -//#include "FastPWM.h" - -MODSERIAL pc(USBTX, USBRX); - -//TIMER -timer - -//MOTORSPEED -Motorspeed - -//LED -DigitalOut GreenLED(LED1); -DigitalOut RedLED(LED2); - -//Buttons -Button1 //to start ME calibration -Button2 - -// declare a new variable type (I called it states) -enum states {STATE_START, STATE_MECALIBRATION,STATE_EMGCALIBRATION,STATE_MOVETOSTART, STATE_READYTOSTART, STATE_DEMO, STATE_MOVE, STATE_WAIT, STATE_OFF}; - -// create a variable (of type ‘states’) called ‘mystate', initialize it -states mystate = STATE_START; -bool stateChanged = true; // Make sure the initialization of first state is executed - -void ProcessStateMachine(void) -{ -case STATE_START : // Start Robot -if (stateChanged) // -{ printf("Robot is on, ready to play. Press button to calibrate"); - RedLED = 1 ; //Turn red led on - stateChanged = false; - } -//State Me Calibration -if (Button1.Pressed()) -{currentState = STATE_MECALIBRATION; -stateChanged = true; -} -break; // end of STATE_START - -case STATE_MECALIBRATION: -if (stateChanged) // -{ printf("Robot is calibrating ME"); - RedLED = SLOW BLINK ; //Blinking SLOW RedLED - //actions of ME CALIBRATION - Move motor to Mechanical stop - Stop motor - Start timer - stateChanged = false; - } - -//State EMG calibration -if (Motorspeed = 0 && timer t > 2) -{currentState = STATE_EMGCALIBRATION; -stateChanged = true; -} -break; // end of STATE_MECALIBRATION - -case STATE_EMGCALIBRATION: -if (stateChanged) // -{ printf("Robot is calibrating EMG"); - RedLED = FAST BLINK ; //Blinking FAST RedLED - //actions of EMG CALIBRATION - Calculating EMG_max - Calibrating - Measuring current EMG_signal - stateChanged = false; - } - -//State EMG calibration -if (EMG_signal < 10% of EMG_max && timer t > 2) -{currentState = STATE_MOVETOSTART; -stateChanged = true; -} -break; // end of STATE_EMGCALIBRATION - - -case STATE_MOVETOSTART : -if (stateChanged) // -{ printf("Robot is moving to start position"); - GreenLED = BLINKING FAST ; //Blinking FastRedLED - //actions of Robot moving to start - stateChanged = false; - } -//State Ready to start -if (Position is x,y,z ? && timer t > 2) //hoe gaan we dit bepalen? -{currentState = STATE_READYTOSTART; -stateChanged = true; -} -break; // end of STATE_MOVETOSTART - -case STATE_READYTOSTART : -if (stateChanged) // -{ printf("Robot is ready to start, press button 1 for starting DEMO or press button 2 or contract muscle with EMG electrode to start Game Mode "); - GreenLED = ON ; //GreenLed is on - //Wait for input user - stateChanged = false; - } -//State Ready to start -if (Button2.pressed() && timer t > 2 or EMG_signal > 20% EMG_max) -{currentState = STATE_MOVE; -stateChanged = true; -} -else if (Button1.pressed()) -{currentState = STATE_DEMO; -stateChanged = true; -} -break; // end of STATE_READYTOSTART - -case STATE_DEMO : -if (stateChanged) // -{ printf("Robot is performing DEMO"); - //actions of DEMO - stateChanged = false; - } - -//State Move to Start -if ( Position is x,y,z ? && timer t > 2) -{currentState = STATE_MOVETOSTART; -stateChanged = true; -} -break; // end of State DEMO - - -case STATE_MOVE : -if (stateChanged) // -{ printf("Time to play!"); - //actions of game mode - stateChanged = false; - } - -//State Wait -if (EMG_signal < 20% EMG_max for t > 5) -{currentState = STATE_WAIT; -stateChanged = true; -} -break; // end of State MOVE - -case STATE_WAIT : -if (stateChanged) // -{ printf("Robot is waiting for your actions. Press button 2 or contract muscle to continue playing. Press button 1 to Restart. Hold button 1 pressed to shut down the game"); - //Waiting for input actions - stateChanged = false; - } - -//State Move -if (Button2.pressed() or EMG_signal > 20% EMG_max) -{currentState = STATE_MOVE; -stateChanged = true; -} - -else if (Button1.pressed()) -{currentState = STATE_MOVETOSTART; -stateChanged = true; -} - -else if (Button1.pressed() for timer t > 5) -{currentState = STATE_OFF; -stateChanged = true; -} -break; // end of State WAIT - -case STATE_OFF: -if (stateChanged) // -{ printf("No Signal. Restart by putting out and back in USB cable" - stateChanged = false; - } - - - -default: -TurnMotorsOff(); // Safety is important!! -pc.printf(“Unknown or unimplemented state reached!\n”); - -} //end of switch - - - - - - -int main() -{ - - } -}