Brechje Hooglugt / Mbed 2 deprecated Bioroboticsgr15

Opzetje

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

OpzetjeBrechje.cpp@8:2fc7a3a7f09a, 2019-10-03 (annotated)

Committer:
s1868365
Date:
Thu Oct 03 12:58:44 2019 +0000
Revision:
8:2fc7a3a7f09a
Statemachine

Who changed what in which revision?

UserRevisionLine numberNew contents of line
s1868365 8:2fc7a3a7f09a 1 #include "mbed.h"
s1868365 8:2fc7a3a7f09a 2 //#include "HIDScope.h"
s1868365 8:2fc7a3a7f09a 3 //#include "QEI.h"
s1868365 8:2fc7a3a7f09a 4 #include "MODSERIAL.h"
s1868365 8:2fc7a3a7f09a 5 //#include "BiQuad.h"
s1868365 8:2fc7a3a7f09a 6 //#include "FastPWM.h"
s1868365 8:2fc7a3a7f09a 7
s1868365 8:2fc7a3a7f09a 8 MODSERIAL pc(USBTX, USBRX);
s1868365 8:2fc7a3a7f09a 9
s1868365 8:2fc7a3a7f09a 10 //TIMER
s1868365 8:2fc7a3a7f09a 11 timer
s1868365 8:2fc7a3a7f09a 12
s1868365 8:2fc7a3a7f09a 13 //MOTORSPEED
s1868365 8:2fc7a3a7f09a 14 Motorspeed
s1868365 8:2fc7a3a7f09a 15
s1868365 8:2fc7a3a7f09a 16 //LED
s1868365 8:2fc7a3a7f09a 17 DigitalOut GreenLED(LED1);
s1868365 8:2fc7a3a7f09a 18 DigitalOut RedLED(LED2);
s1868365 8:2fc7a3a7f09a 19
s1868365 8:2fc7a3a7f09a 20 //Buttons
s1868365 8:2fc7a3a7f09a 21 Button1 //to start ME calibration
s1868365 8:2fc7a3a7f09a 22 Button2
s1868365 8:2fc7a3a7f09a 23
s1868365 8:2fc7a3a7f09a 24 // declare a new variable type (I called it states)
s1868365 8:2fc7a3a7f09a 25 enum states {STATE_START, STATE_MECALIBRATION,STATE_EMGCALIBRATION,STATE_MOVETOSTART, STATE_READYTOSTART, STATE_DEMO, STATE_MOVE, STATE_WAIT, STATE_OFF};
s1868365 8:2fc7a3a7f09a 26
s1868365 8:2fc7a3a7f09a 27 // create a variable (of type ‘states’) called ‘mystate', initialize it
s1868365 8:2fc7a3a7f09a 28 states mystate = STATE_START;
s1868365 8:2fc7a3a7f09a 29 bool stateChanged = true; // Make sure the initialization of first state is executed
s1868365 8:2fc7a3a7f09a 30
s1868365 8:2fc7a3a7f09a 31 void ProcessStateMachine(void)
s1868365 8:2fc7a3a7f09a 32 {
s1868365 8:2fc7a3a7f09a 33 case STATE_START : // Start Robot
s1868365 8:2fc7a3a7f09a 34 if (stateChanged) //
s1868365 8:2fc7a3a7f09a 35 { printf("Robot is on, ready to play. Press button to calibrate");
s1868365 8:2fc7a3a7f09a 36 RedLED = 1 ; //Turn red led on
s1868365 8:2fc7a3a7f09a 37 stateChanged = false;
s1868365 8:2fc7a3a7f09a 38 }
s1868365 8:2fc7a3a7f09a 39 //State Me Calibration
s1868365 8:2fc7a3a7f09a 40 if (Button1.Pressed())
s1868365 8:2fc7a3a7f09a 41 {currentState = STATE_MECALIBRATION;
s1868365 8:2fc7a3a7f09a 42 stateChanged = true;
s1868365 8:2fc7a3a7f09a 43 }
s1868365 8:2fc7a3a7f09a 44 break; // end of STATE_START
s1868365 8:2fc7a3a7f09a 45
s1868365 8:2fc7a3a7f09a 46 case STATE_MECALIBRATION:
s1868365 8:2fc7a3a7f09a 47 if (stateChanged) //
s1868365 8:2fc7a3a7f09a 48 { printf("Robot is calibrating ME");
s1868365 8:2fc7a3a7f09a 49 RedLED = SLOW BLINK ; //Blinking SLOW RedLED
s1868365 8:2fc7a3a7f09a 50 //actions of ME CALIBRATION
s1868365 8:2fc7a3a7f09a 51 Move motor to Mechanical stop
s1868365 8:2fc7a3a7f09a 52 Stop motor
s1868365 8:2fc7a3a7f09a 53 Start timer
s1868365 8:2fc7a3a7f09a 54 stateChanged = false;
s1868365 8:2fc7a3a7f09a 55 }
s1868365 8:2fc7a3a7f09a 56
s1868365 8:2fc7a3a7f09a 57 //State EMG calibration
s1868365 8:2fc7a3a7f09a 58 if (Motorspeed = 0 && timer t > 2)
s1868365 8:2fc7a3a7f09a 59 {currentState = STATE_EMGCALIBRATION;
s1868365 8:2fc7a3a7f09a 60 stateChanged = true;
s1868365 8:2fc7a3a7f09a 61 }
s1868365 8:2fc7a3a7f09a 62 break; // end of STATE_MECALIBRATION
s1868365 8:2fc7a3a7f09a 63
s1868365 8:2fc7a3a7f09a 64 case STATE_EMGCALIBRATION:
s1868365 8:2fc7a3a7f09a 65 if (stateChanged) //
s1868365 8:2fc7a3a7f09a 66 { printf("Robot is calibrating EMG");
s1868365 8:2fc7a3a7f09a 67 RedLED = FAST BLINK ; //Blinking FAST RedLED
s1868365 8:2fc7a3a7f09a 68 //actions of EMG CALIBRATION
s1868365 8:2fc7a3a7f09a 69 Calculating EMG_max
s1868365 8:2fc7a3a7f09a 70 Calibrating
s1868365 8:2fc7a3a7f09a 71 Measuring current EMG_signal
s1868365 8:2fc7a3a7f09a 72 stateChanged = false;
s1868365 8:2fc7a3a7f09a 73 }
s1868365 8:2fc7a3a7f09a 74
s1868365 8:2fc7a3a7f09a 75 //State EMG calibration
s1868365 8:2fc7a3a7f09a 76 if (EMG_signal < 10% of EMG_max && timer t > 2)
s1868365 8:2fc7a3a7f09a 77 {currentState = STATE_MOVETOSTART;
s1868365 8:2fc7a3a7f09a 78 stateChanged = true;
s1868365 8:2fc7a3a7f09a 79 }
s1868365 8:2fc7a3a7f09a 80 break; // end of STATE_EMGCALIBRATION
s1868365 8:2fc7a3a7f09a 81
s1868365 8:2fc7a3a7f09a 82
s1868365 8:2fc7a3a7f09a 83 case STATE_MOVETOSTART :
s1868365 8:2fc7a3a7f09a 84 if (stateChanged) //
s1868365 8:2fc7a3a7f09a 85 { printf("Robot is moving to start position");
s1868365 8:2fc7a3a7f09a 86 GreenLED = BLINKING FAST ; //Blinking FastRedLED
s1868365 8:2fc7a3a7f09a 87 //actions of Robot moving to start
s1868365 8:2fc7a3a7f09a 88 stateChanged = false;
s1868365 8:2fc7a3a7f09a 89 }
s1868365 8:2fc7a3a7f09a 90 //State Ready to start
s1868365 8:2fc7a3a7f09a 91 if (Position is x,y,z ? && timer t > 2) //hoe gaan we dit bepalen?
s1868365 8:2fc7a3a7f09a 92 {currentState = STATE_READYTOSTART;
s1868365 8:2fc7a3a7f09a 93 stateChanged = true;
s1868365 8:2fc7a3a7f09a 94 }
s1868365 8:2fc7a3a7f09a 95 break; // end of STATE_MOVETOSTART
s1868365 8:2fc7a3a7f09a 96
s1868365 8:2fc7a3a7f09a 97 case STATE_READYTOSTART :
s1868365 8:2fc7a3a7f09a 98 if (stateChanged) //
s1868365 8:2fc7a3a7f09a 99 { 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 ");
s1868365 8:2fc7a3a7f09a 100 GreenLED = ON ; //GreenLed is on
s1868365 8:2fc7a3a7f09a 101 //Wait for input user
s1868365 8:2fc7a3a7f09a 102 stateChanged = false;
s1868365 8:2fc7a3a7f09a 103 }
s1868365 8:2fc7a3a7f09a 104 //State Ready to start
s1868365 8:2fc7a3a7f09a 105 if (Button2.pressed() && timer t > 2 or EMG_signal > 20% EMG_max)
s1868365 8:2fc7a3a7f09a 106 {currentState = STATE_MOVE;
s1868365 8:2fc7a3a7f09a 107 stateChanged = true;
s1868365 8:2fc7a3a7f09a 108 }
s1868365 8:2fc7a3a7f09a 109 else if (Button1.pressed())
s1868365 8:2fc7a3a7f09a 110 {currentState = STATE_DEMO;
s1868365 8:2fc7a3a7f09a 111 stateChanged = true;
s1868365 8:2fc7a3a7f09a 112 }
s1868365 8:2fc7a3a7f09a 113 break; // end of STATE_READYTOSTART
s1868365 8:2fc7a3a7f09a 114
s1868365 8:2fc7a3a7f09a 115 case STATE_DEMO :
s1868365 8:2fc7a3a7f09a 116 if (stateChanged) //
s1868365 8:2fc7a3a7f09a 117 { printf("Robot is performing DEMO");
s1868365 8:2fc7a3a7f09a 118 //actions of DEMO
s1868365 8:2fc7a3a7f09a 119 stateChanged = false;
s1868365 8:2fc7a3a7f09a 120 }
s1868365 8:2fc7a3a7f09a 121
s1868365 8:2fc7a3a7f09a 122 //State Move to Start
s1868365 8:2fc7a3a7f09a 123 if ( Position is x,y,z ? && timer t > 2)
s1868365 8:2fc7a3a7f09a 124 {currentState = STATE_MOVETOSTART;
s1868365 8:2fc7a3a7f09a 125 stateChanged = true;
s1868365 8:2fc7a3a7f09a 126 }
s1868365 8:2fc7a3a7f09a 127 break; // end of State DEMO
s1868365 8:2fc7a3a7f09a 128
s1868365 8:2fc7a3a7f09a 129
s1868365 8:2fc7a3a7f09a 130 case STATE_MOVE :
s1868365 8:2fc7a3a7f09a 131 if (stateChanged) //
s1868365 8:2fc7a3a7f09a 132 { printf("Time to play!");
s1868365 8:2fc7a3a7f09a 133 //actions of game mode
s1868365 8:2fc7a3a7f09a 134 stateChanged = false;
s1868365 8:2fc7a3a7f09a 135 }
s1868365 8:2fc7a3a7f09a 136
s1868365 8:2fc7a3a7f09a 137 //State Wait
s1868365 8:2fc7a3a7f09a 138 if (EMG_signal < 20% EMG_max for t > 5)
s1868365 8:2fc7a3a7f09a 139 {currentState = STATE_WAIT;
s1868365 8:2fc7a3a7f09a 140 stateChanged = true;
s1868365 8:2fc7a3a7f09a 141 }
s1868365 8:2fc7a3a7f09a 142 break; // end of State MOVE
s1868365 8:2fc7a3a7f09a 143
s1868365 8:2fc7a3a7f09a 144 case STATE_WAIT :
s1868365 8:2fc7a3a7f09a 145 if (stateChanged) //
s1868365 8:2fc7a3a7f09a 146 { 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");
s1868365 8:2fc7a3a7f09a 147 //Waiting for input actions
s1868365 8:2fc7a3a7f09a 148 stateChanged = false;
s1868365 8:2fc7a3a7f09a 149 }
s1868365 8:2fc7a3a7f09a 150
s1868365 8:2fc7a3a7f09a 151 //State Move
s1868365 8:2fc7a3a7f09a 152 if (Button2.pressed() or EMG_signal > 20% EMG_max)
s1868365 8:2fc7a3a7f09a 153 {currentState = STATE_MOVE;
s1868365 8:2fc7a3a7f09a 154 stateChanged = true;
s1868365 8:2fc7a3a7f09a 155 }
s1868365 8:2fc7a3a7f09a 156
s1868365 8:2fc7a3a7f09a 157 else if (Button1.pressed())
s1868365 8:2fc7a3a7f09a 158 {currentState = STATE_MOVETOSTART;
s1868365 8:2fc7a3a7f09a 159 stateChanged = true;
s1868365 8:2fc7a3a7f09a 160 }
s1868365 8:2fc7a3a7f09a 161
s1868365 8:2fc7a3a7f09a 162 else if (Button1.pressed() for timer t > 5)
s1868365 8:2fc7a3a7f09a 163 {currentState = STATE_OFF;
s1868365 8:2fc7a3a7f09a 164 stateChanged = true;
s1868365 8:2fc7a3a7f09a 165 }
s1868365 8:2fc7a3a7f09a 166 break; // end of State WAIT
s1868365 8:2fc7a3a7f09a 167
s1868365 8:2fc7a3a7f09a 168 case STATE_OFF:
s1868365 8:2fc7a3a7f09a 169 if (stateChanged) //
s1868365 8:2fc7a3a7f09a 170 { printf("No Signal. Restart by putting out and back in USB cable"
s1868365 8:2fc7a3a7f09a 171 stateChanged = false;
s1868365 8:2fc7a3a7f09a 172 }
s1868365 8:2fc7a3a7f09a 173
s1868365 8:2fc7a3a7f09a 174
s1868365 8:2fc7a3a7f09a 175
s1868365 8:2fc7a3a7f09a 176 default:
s1868365 8:2fc7a3a7f09a 177 TurnMotorsOff(); // Safety is important!!
s1868365 8:2fc7a3a7f09a 178 pc.printf(“Unknown or unimplemented state reached!\n”);
s1868365 8:2fc7a3a7f09a 179
s1868365 8:2fc7a3a7f09a 180 } //end of switch
s1868365 8:2fc7a3a7f09a 181
s1868365 8:2fc7a3a7f09a 182
s1868365 8:2fc7a3a7f09a 183
s1868365 8:2fc7a3a7f09a 184
s1868365 8:2fc7a3a7f09a 185
s1868365 8:2fc7a3a7f09a 186
s1868365 8:2fc7a3a7f09a 187 int main()
s1868365 8:2fc7a3a7f09a 188 {
s1868365 8:2fc7a3a7f09a 189
s1868365 8:2fc7a3a7f09a 190 }
s1868365 8:2fc7a3a7f09a 191 }