Group 9 BioRobotics / Mbed 2 deprecated the_code

The code without functions and variables filled in

Dependencies:   HIDScope Servo mbed QEI biquadFilter

THE.cpp@11:79311abb2bc2, 2018-10-31 (annotated)

Committer:
s1725696
Date:
Wed Oct 31 13:29:06 2018 +0000
Revision:
11:79311abb2bc2
Parent:
main.cpp@10:56136a0da8c1
Child:
12:b2b082e73ef1
Servo included, not used yet

Who changed what in which revision?

UserRevisionLine numberNew contents of line
s1725696 5:3581013d4505 1 #include "mbed.h" // Use revision 119!!
s1725696 5:3581013d4505 2 #include "HIDScope.h" // For displaying data, select MBED - HID device, restart for every new code
s1725696 5:3581013d4505 3 #include "QEI.h" // For reading the encoder of the motors
s1725696 9:d7a6a3619576 4 #include <ctime> // for the timer during the process (if needed)
s1725696 11:79311abb2bc2 5 #include "Servo.h" // For controlling the servo
s1725696 0:cb8857cf3ea4 6
s1725696 5:3581013d4505 7 #define SERIAL_BAUD 115200
s1725696 5:3581013d4505 8
s1725696 5:3581013d4505 9 // In- en outputs
s1725696 4:8183e7b228f0 10 // -----------------------------------------------------------------------------
s1725696 4:8183e7b228f0 11
s1725696 0:cb8857cf3ea4 12 // EMG related
s1725696 5:3581013d4505 13 AnalogIn emg1(); // EMG signal 1
s1725696 5:3581013d4505 14 AnalogIn emg2(); // EMG signal 2
s1725696 5:3581013d4505 15 // if needed
s1725696 5:3581013d4505 16 AnalogIn emg3(); // EMG signal 3
s1725696 5:3581013d4505 17 AnalogIn emg4(); // EMG signal 4
s1725696 10:56136a0da8c1 18
s1725696 0:cb8857cf3ea4 19
s1725696 0:cb8857cf3ea4 20 // Motor related
s1725696 0:cb8857cf3ea4 21 DigitalOut dirpin_1(D4); // direction of motor 1
s1725696 0:cb8857cf3ea4 22 PwmOut pwmpin_1(D5); // PWM pin of motor 1
s1725696 5:3581013d4505 23 DigitalOut dirpin_2(D7); // direction of motor 2
s1725696 5:3581013d4505 24 PwmOut pwmpin_2(D6); // PWM pin of motor 2
s1725696 0:cb8857cf3ea4 25
s1725696 0:cb8857cf3ea4 26 // Extra stuff
s1725696 0:cb8857cf3ea4 27 // Like LED lights, buttons etc
s1725696 9:d7a6a3619576 28
s1725696 9:d7a6a3619576 29 DigitalIn button_motorcal(SW2); // button for motor calibration, on mbed
s1725696 8:5ad8a7892693 30 DigitalIn button_emergency(D7); // button for emergency mode, on bioshield
s1725696 9:d7a6a3619576 31 DigitalIn button_wait(SW3); // button for wait mode, on mbed
s1725696 8:5ad8a7892693 32 DigitalIn button_demo(D6); // button for demo mode, on bioshield
s1725696 9:d7a6a3619576 33
s1725696 4:8183e7b228f0 34 DigitalIn led_red(LED_RED); // red led
s1725696 4:8183e7b228f0 35 DigitalIn led_green(LED_GREEN); // green led
s1725696 4:8183e7b228f0 36 DigitalIn led_blue(LED_BLUE); // blue led
s1725696 9:d7a6a3619576 37
s1725696 5:3581013d4505 38 AnalogIn pot_1(A1); // potmeter for simulating EMG input
s1725696 5:3581013d4505 39
s1725696 0:cb8857cf3ea4 40
s1725696 0:cb8857cf3ea4 41 // Other stuff
s1725696 4:8183e7b228f0 42 // -----------------------------------------------------------------------------
s1725696 0:cb8857cf3ea4 43 // Define stuff like tickers etc
s1725696 11:79311abb2bc2 44 Servo myservo(D7); // Define the servo to control (in penholder)
s1725696 10:56136a0da8c1 45 Ticker ticker; // Name a ticker, use each ticker only for 1 function!
s1725696 0:cb8857cf3ea4 46 HIDScope scope(2); // Number of channels in HIDScope
s1725696 0:cb8857cf3ea4 47 QEI Encoder(D13,D12,NC,64,QEI::X4_ENCODING); // Define the type of encoding: X4 encoding(default is X2)
s1725696 5:3581013d4505 48 Serial pc(USBTX,USBRX);
s1725696 6:f495a77c2c95 49 Timer t; // For timing the time in each state (https://os.mbed.com/handbook/Timer)
s1725696 0:cb8857cf3ea4 50
s1725696 0:cb8857cf3ea4 51 // Variables
s1725696 4:8183e7b228f0 52 // -----------------------------------------------------------------------------
s1725696 0:cb8857cf3ea4 53 // Define here all variables needed throughout the whole code
s1725696 5:3581013d4505 54 int counts;
s1725696 6:f495a77c2c95 55 float potmeter_value;
s1725696 9:d7a6a3619576 56 double time_overall;
s1725696 6:f495a77c2c95 57 float time_in_state;
s1725696 9:d7a6a3619576 58 double motor_velocity = 0;
s1725696 9:d7a6a3619576 59 double EMG = 0;
s1725696 9:d7a6a3619576 60 double errors = 0;
s1725696 6:f495a77c2c95 61
s1725696 6:f495a77c2c95 62 // states
s1725696 9:d7a6a3619576 63 enum states {WAIT, MOTOR_CAL, EMG_CAL, START, OPERATING, FAILURE, DEMO}; // states the robot can be in
s1725696 7:ec5add330cb3 64 states CurrentState = WAIT; // the CurrentState to start with is the WAIT state
s1725696 6:f495a77c2c95 65 bool StateChanged = true; // the state must be changed to go into the next state
s1725696 0:cb8857cf3ea4 66
s1725696 0:cb8857cf3ea4 67 // Functions
s1725696 4:8183e7b228f0 68 // -----------------------------------------------------------------------------
s1725696 0:cb8857cf3ea4 69
s1725696 4:8183e7b228f0 70 // Encoder
s1725696 4:8183e7b228f0 71 // Getting encoder information from motors
s1725696 4:8183e7b228f0 72 int encoder()
s1725696 4:8183e7b228f0 73 {
s1725696 5:3581013d4505 74 int counts = Encoder.getPulses();
s1725696 4:8183e7b228f0 75 return counts;
s1725696 5:3581013d4505 76 }
s1725696 5:3581013d4505 77
s1725696 11:79311abb2bc2 78 // Potmeter for controlling motor
s1725696 5:3581013d4505 79 float potmeter()
s1725696 5:3581013d4505 80 {
s1725696 5:3581013d4505 81 float out_1 = pot_1 * 2.0f;
s1725696 5:3581013d4505 82 float out_2 = out_1 - 1.0f;
s1725696 5:3581013d4505 83
s1725696 5:3581013d4505 84 dirpin_1.write(out_2 < 0);
s1725696 5:3581013d4505 85 dirpin_2.write(out_2 < 0);
s1725696 5:3581013d4505 86
s1725696 5:3581013d4505 87 pwmpin_1 = fabs (out_2); // Has to be positive value
s1725696 5:3581013d4505 88 pwmpin_2 = fabs (out_2);
s1725696 5:3581013d4505 89
s1725696 5:3581013d4505 90 return out_2;
s1725696 6:f495a77c2c95 91 }
s1725696 9:d7a6a3619576 92
s1725696 11:79311abb2bc2 93 // Servo control
s1725696 11:79311abb2bc2 94 // To lift the pen up, with a push of button 2
s1725696 11:79311abb2bc2 95 void servocontrol()
s1725696 11:79311abb2bc2 96 {
s1725696 11:79311abb2bc2 97 if (but_1)
s1725696 11:79311abb2bc2 98 {
s1725696 11:79311abb2bc2 99 myservo = 0.1;
s1725696 11:79311abb2bc2 100 }
s1725696 11:79311abb2bc2 101 else
s1725696 11:79311abb2bc2 102 {
s1725696 11:79311abb2bc2 103 myservo = 0.0;
s1725696 11:79311abb2bc2 104 }
s1725696 11:79311abb2bc2 105 }
s1725696 11:79311abb2bc2 106
s1725696 9:d7a6a3619576 107 // Send information to HIDScope
s1725696 6:f495a77c2c95 108 void hidscope() // Attach this to a ticker!
s1725696 0:cb8857cf3ea4 109 {
s1725696 6:f495a77c2c95 110 scope.set(0, counts); // send EMG 1 to channel 1 (=0)
s1725696 6:f495a77c2c95 111 scope.set(1, potmeter_value); // sent EMG 2 to channel 2 (=1)
s1725696 5:3581013d4505 112
s1725696 1:1a8211e1f3f3 113 // Ensure that enough channels are available (HIDScope scope(2))
s1725696 0:cb8857cf3ea4 114 scope.send(); // Send all channels to the PC at once
s1725696 6:f495a77c2c95 115 }
s1725696 9:d7a6a3619576 116
s1725696 10:56136a0da8c1 117 // EMG filter
s1725696 10:56136a0da8c1 118 // To process the EMG signal before information can be caught from it
s1725696 10:56136a0da8c1 119 // Kees mee bezig
s1725696 10:56136a0da8c1 120
s1725696 10:56136a0da8c1 121 // WAIT
s1725696 10:56136a0da8c1 122 // To do nothing
s1725696 10:56136a0da8c1 123 void wait_mode()
s1725696 10:56136a0da8c1 124 {
s1725696 10:56136a0da8c1 125 // go back to the initial values
s1725696 10:56136a0da8c1 126 // Copy here the variables list with initial values
s1725696 10:56136a0da8c1 127 motor_velocity = 0;
s1725696 10:56136a0da8c1 128 }
s1725696 10:56136a0da8c1 129
s1725696 10:56136a0da8c1 130 // MOTOR_CAL
s1725696 10:56136a0da8c1 131 // To calibrate the motor angle to some mechanical boundaries
s1725696 10:56136a0da8c1 132 // Kenneth mee bezig
s1725696 10:56136a0da8c1 133 void motor_calibration()
s1725696 10:56136a0da8c1 134 {
s1725696 10:56136a0da8c1 135 // Kenneths code here
s1725696 10:56136a0da8c1 136 }
s1725696 10:56136a0da8c1 137
s1725696 10:56136a0da8c1 138 // EMG_CAL
s1725696 10:56136a0da8c1 139 // To calibrate the EMG signal to some boundary values
s1725696 10:56136a0da8c1 140 void emg_calibration()
s1725696 10:56136a0da8c1 141 {
s1725696 10:56136a0da8c1 142 // code
s1725696 10:56136a0da8c1 143 }
s1725696 10:56136a0da8c1 144
s1725696 9:d7a6a3619576 145 // PID controller
s1725696 9:d7a6a3619576 146 // To control the input signal before it goes into the motor control
s1725696 9:d7a6a3619576 147 // Simon mee bezig
s1725696 10:56136a0da8c1 148 void pid_controller()
s1725696 10:56136a0da8c1 149 {
s1725696 10:56136a0da8c1 150 // Simons code here
s1725696 10:56136a0da8c1 151 }
s1725696 0:cb8857cf3ea4 152
s1725696 10:56136a0da8c1 153 // START
s1725696 0:cb8857cf3ea4 154 // To move the robot to the starting position: middle
s1725696 10:56136a0da8c1 155 void start_mode()
s1725696 4:8183e7b228f0 156 {
s1725696 4:8183e7b228f0 157 // move to middle
s1725696 4:8183e7b228f0 158 }
s1725696 0:cb8857cf3ea4 159
s1725696 10:56136a0da8c1 160 // OPERATING
s1725696 0:cb8857cf3ea4 161 // To control the robot with EMG signals
s1725696 11:79311abb2bc2 162 // Kenneth bezig met motor aansturen
s1725696 0:cb8857cf3ea4 163
s1725696 10:56136a0da8c1 164 // DEMO
s1725696 0:cb8857cf3ea4 165 // To control the robot with a written code and write 'HELLO'
s1725696 4:8183e7b228f0 166 // Voor het laatst bewaren
s1725696 10:56136a0da8c1 167 void demo_mode()
s1725696 10:56136a0da8c1 168 {
s1725696 10:56136a0da8c1 169 // code here
s1725696 10:56136a0da8c1 170 }
s1725696 0:cb8857cf3ea4 171
s1725696 10:56136a0da8c1 172 // FAILURE
s1725696 0:cb8857cf3ea4 173 // To shut down the robot after an error etc
s1725696 10:56136a0da8c1 174 void failure()
s1725696 10:56136a0da8c1 175 {
s1725696 10:56136a0da8c1 176 // code here
s1725696 10:56136a0da8c1 177 }
s1725696 0:cb8857cf3ea4 178
s1725696 0:cb8857cf3ea4 179 // Main function
s1725696 4:8183e7b228f0 180 // -----------------------------------------------------------------------------
s1725696 0:cb8857cf3ea4 181 int main()
s1725696 0:cb8857cf3ea4 182 {
s1725696 0:cb8857cf3ea4 183 pc.baud(115200); // For TeraTerm, the baudrate, set also in TeraTerm itself!
s1725696 0:cb8857cf3ea4 184 pc.printf("Start code\r\n"); // To check if the program starts
s1725696 9:d7a6a3619576 185 pwmpin_1.period_us(60); // Setting period for PWM
s1725696 10:56136a0da8c1 186 ticker.attach(&hidscope,0.002f); // Send information to HIDScope
s1725696 1:1a8211e1f3f3 187
s1725696 1:1a8211e1f3f3 188 while(true){
s1725696 6:f495a77c2c95 189 // timer
s1725696 6:f495a77c2c95 190 clock_t start; // start the timer
s1725696 6:f495a77c2c95 191 start = clock();
s1725696 9:d7a6a3619576 192 time_overall = (clock() - start) / (double) CLOCKS_PER_SEC;
s1725696 5:3581013d4505 193
s1725696 5:3581013d4505 194 counts = encoder();
s1725696 6:f495a77c2c95 195 potmeter_value = potmeter();
s1725696 6:f495a77c2c95 196
s1725696 5:3581013d4505 197
s1725696 6:f495a77c2c95 198 //pc.printf("potmeter value = %f ", potmeter_value);
s1725696 6:f495a77c2c95 199 //pc.printf("counts = %i\r\n", counts);
s1725696 5:3581013d4505 200
s1725696 6:f495a77c2c95 201 // With the help of a switch loop and states we can switch between states and the robot knows what to do
s1725696 9:d7a6a3619576 202 switch(CurrentState)
s1725696 6:f495a77c2c95 203 {
s1725696 6:f495a77c2c95 204 case WAIT:
s1725696 6:f495a77c2c95 205
s1725696 6:f495a77c2c95 206 if(StateChanged) // so if StateChanged is true
s1725696 6:f495a77c2c95 207 {
s1725696 10:56136a0da8c1 208 pc.printf("State is WAIT\r\n");
s1725696 10:56136a0da8c1 209
s1725696 6:f495a77c2c95 210 // Execute WAIT mode
s1725696 10:56136a0da8c1 211 wait_mode();
s1725696 10:56136a0da8c1 212
s1725696 6:f495a77c2c95 213 StateChanged = false; // the state is still WAIT
s1725696 6:f495a77c2c95 214 }
s1725696 6:f495a77c2c95 215
s1725696 7:ec5add330cb3 216 if(button_motorcal == true) // condition for WAIT --> MOTOR_CAl; button_motorcal press
s1725696 6:f495a77c2c95 217 {
s1725696 6:f495a77c2c95 218 CurrentState = MOTOR_CAL;
s1725696 9:d7a6a3619576 219 pc.printf("State is MOTOR_CAL\r\n");
s1725696 6:f495a77c2c95 220 StateChanged = true;
s1725696 6:f495a77c2c95 221 }
s1725696 7:ec5add330cb3 222
s1725696 7:ec5add330cb3 223 if (button_emergency == true) // condition for WAIT --> FAILURE; button_emergency press
s1725696 7:ec5add330cb3 224 {
s1725696 7:ec5add330cb3 225 CurrentState = FAILURE;
s1725696 9:d7a6a3619576 226 pc.printf("State is FAILURE\r\n");
s1725696 7:ec5add330cb3 227 StateChanged = true;
s1725696 7:ec5add330cb3 228 }
s1725696 6:f495a77c2c95 229
s1725696 6:f495a77c2c95 230 break;
s1725696 6:f495a77c2c95 231
s1725696 6:f495a77c2c95 232 case MOTOR_CAL:
s1725696 6:f495a77c2c95 233
s1725696 6:f495a77c2c95 234 if(StateChanged) // so if StateChanged is true
s1725696 6:f495a77c2c95 235 {
s1725696 9:d7a6a3619576 236 t.reset();
s1725696 6:f495a77c2c95 237 t.start();
s1725696 10:56136a0da8c1 238
s1725696 6:f495a77c2c95 239 // Execute MOTOR_CAL mode
s1725696 10:56136a0da8c1 240 motor_calibration();
s1725696 10:56136a0da8c1 241
s1725696 6:f495a77c2c95 242 t.stop();
s1725696 6:f495a77c2c95 243 time_in_state = t.read();
s1725696 9:d7a6a3619576 244 pc.printf("Time here = %f\r\n", time_in_state);
s1725696 10:56136a0da8c1 245
s1725696 6:f495a77c2c95 246 StateChanged = false; // the state is still MOTOR_CAL
s1725696 6:f495a77c2c95 247 }
s1725696 6:f495a77c2c95 248
s1725696 9:d7a6a3619576 249 if((time_in_state > 3.0) && (motor_velocity < 0.01)) // condition for MOTOR_CAL --> EMG_CAL; 3s en motors stopped moving
s1725696 6:f495a77c2c95 250 {
s1725696 6:f495a77c2c95 251 CurrentState = EMG_CAL;
s1725696 9:d7a6a3619576 252 pc.printf("State is EMG_CAL\r\n");
s1725696 6:f495a77c2c95 253 StateChanged = true;
s1725696 6:f495a77c2c95 254 }
s1725696 6:f495a77c2c95 255
s1725696 7:ec5add330cb3 256 if (button_emergency == true) // condition for MOTOR_CAL --> FAILURE; button_emergency press
s1725696 7:ec5add330cb3 257 {
s1725696 7:ec5add330cb3 258 CurrentState = FAILURE;
s1725696 9:d7a6a3619576 259 pc.printf("State is FAILURE\r\n");
s1725696 7:ec5add330cb3 260 StateChanged = true;
s1725696 7:ec5add330cb3 261 }
s1725696 7:ec5add330cb3 262
s1725696 6:f495a77c2c95 263 break;
s1725696 6:f495a77c2c95 264
s1725696 6:f495a77c2c95 265 case EMG_CAL:
s1725696 6:f495a77c2c95 266
s1725696 6:f495a77c2c95 267 if(StateChanged) // so if StateChanged is true
s1725696 6:f495a77c2c95 268 {
s1725696 6:f495a77c2c95 269 t.reset();
s1725696 6:f495a77c2c95 270 t.start();
s1725696 10:56136a0da8c1 271
s1725696 6:f495a77c2c95 272 // Execute EMG_CAL mode
s1725696 10:56136a0da8c1 273 emg_calibration();
s1725696 10:56136a0da8c1 274
s1725696 6:f495a77c2c95 275 t.stop();
s1725696 6:f495a77c2c95 276 time_in_state = t.read();
s1725696 9:d7a6a3619576 277 pc.printf("Time here = %f\r\n", time_in_state);
s1725696 10:56136a0da8c1 278
s1725696 6:f495a77c2c95 279 StateChanged = false; // state is still EMG_CAL
s1725696 6:f495a77c2c95 280 }
s1725696 6:f495a77c2c95 281
s1725696 9:d7a6a3619576 282 if((time_in_state > 5) && (EMG < 0.01)) // condition for EMG_CAL --> START; 5s and EMG is low
s1725696 6:f495a77c2c95 283 {
s1725696 6:f495a77c2c95 284 CurrentState = START;
s1725696 9:d7a6a3619576 285 pc.printf("State is START\r\n");
s1725696 6:f495a77c2c95 286 StateChanged = true;
s1725696 6:f495a77c2c95 287 }
s1725696 6:f495a77c2c95 288
s1725696 7:ec5add330cb3 289 if (button_emergency == true) // condition for EMG_CAL --> FAILURE; button_emergency press
s1725696 7:ec5add330cb3 290 {
s1725696 7:ec5add330cb3 291 CurrentState = FAILURE;
s1725696 9:d7a6a3619576 292 pc.printf("State is FAILURE\r\n");
s1725696 7:ec5add330cb3 293 StateChanged = true;
s1725696 7:ec5add330cb3 294 }
s1725696 7:ec5add330cb3 295
s1725696 6:f495a77c2c95 296 break;
s1725696 6:f495a77c2c95 297
s1725696 6:f495a77c2c95 298 case START:
s1725696 6:f495a77c2c95 299
s1725696 6:f495a77c2c95 300 if(StateChanged) // so if StateChanged is true
s1725696 6:f495a77c2c95 301 {
s1725696 6:f495a77c2c95 302 t.reset();
s1725696 6:f495a77c2c95 303 t.start();
s1725696 10:56136a0da8c1 304
s1725696 6:f495a77c2c95 305 // Execute START mode
s1725696 10:56136a0da8c1 306 start_mode();
s1725696 10:56136a0da8c1 307
s1725696 6:f495a77c2c95 308 t.stop();
s1725696 6:f495a77c2c95 309 time_in_state = t.read();
s1725696 9:d7a6a3619576 310 pc.printf("Time here = %f\r\n", time_in_state);
s1725696 10:56136a0da8c1 311
s1725696 6:f495a77c2c95 312 StateChanged = false; // state is still START
s1725696 6:f495a77c2c95 313 }
s1725696 6:f495a77c2c95 314
s1725696 9:d7a6a3619576 315 if((time_in_state > 5) && (errors < 0.01)) // condition for START --> OPERATING; 5s and error is small
s1725696 6:f495a77c2c95 316 {
s1725696 6:f495a77c2c95 317 CurrentState = OPERATING;
s1725696 9:d7a6a3619576 318 pc.printf("State is OPERATING\r\n");
s1725696 6:f495a77c2c95 319 StateChanged = true;
s1725696 6:f495a77c2c95 320 }
s1725696 6:f495a77c2c95 321
s1725696 7:ec5add330cb3 322 if (button_emergency == true) // condition for START --> FAILURE; button_emergency press
s1725696 7:ec5add330cb3 323 {
s1725696 7:ec5add330cb3 324 CurrentState = FAILURE;
s1725696 9:d7a6a3619576 325 pc.printf("State is FAILURE\r\n");
s1725696 7:ec5add330cb3 326 StateChanged = true;
s1725696 7:ec5add330cb3 327 }
s1725696 7:ec5add330cb3 328
s1725696 6:f495a77c2c95 329 break;
s1725696 6:f495a77c2c95 330
s1725696 6:f495a77c2c95 331 case OPERATING:
s1725696 6:f495a77c2c95 332
s1725696 6:f495a77c2c95 333 if(StateChanged) // so if StateChanged is true
s1725696 6:f495a77c2c95 334 {
s1725696 6:f495a77c2c95 335 // Execute OPERATING mode
s1725696 10:56136a0da8c1 336
s1725696 6:f495a77c2c95 337 StateChanged = false; // state is still OPERATING
s1725696 6:f495a77c2c95 338 }
s1725696 6:f495a77c2c95 339
s1725696 7:ec5add330cb3 340 if(button_emergency == true) // condition for OPERATING --> FAILURE; button_emergency press
s1725696 6:f495a77c2c95 341 {
s1725696 6:f495a77c2c95 342 CurrentState = FAILURE;
s1725696 9:d7a6a3619576 343 pc.printf("State is FAILURE\r\n");
s1725696 6:f495a77c2c95 344 StateChanged = true;
s1725696 6:f495a77c2c95 345 }
s1725696 6:f495a77c2c95 346
s1725696 7:ec5add330cb3 347 if(button_demo == true) // condition for OPERATING --> DEMO; button_demo press
s1725696 6:f495a77c2c95 348 {
s1725696 6:f495a77c2c95 349 CurrentState = DEMO;
s1725696 9:d7a6a3619576 350 pc.printf("State is DEMO\r\n");
s1725696 9:d7a6a3619576 351 StateChanged = true;
s1725696 9:d7a6a3619576 352 }
s1725696 9:d7a6a3619576 353
s1725696 9:d7a6a3619576 354 if(button_wait == true) // condition OPERATING --> WAIT; button_wait press
s1725696 9:d7a6a3619576 355 {
s1725696 9:d7a6a3619576 356 CurrentState = WAIT;
s1725696 9:d7a6a3619576 357 pc.printf("State is WAIT\r\n");
s1725696 6:f495a77c2c95 358 StateChanged = true;
s1725696 6:f495a77c2c95 359 }
s1725696 6:f495a77c2c95 360
s1725696 6:f495a77c2c95 361 break;
s1725696 6:f495a77c2c95 362
s1725696 6:f495a77c2c95 363 case FAILURE:
s1725696 6:f495a77c2c95 364
s1725696 6:f495a77c2c95 365 if(StateChanged) // so if StateChanged is true
s1725696 6:f495a77c2c95 366 {
s1725696 6:f495a77c2c95 367 // Execute FAILURE mode
s1725696 10:56136a0da8c1 368 failure_mode();
s1725696 10:56136a0da8c1 369
s1725696 6:f495a77c2c95 370 StateChanged = false; // state is still FAILURE
s1725696 6:f495a77c2c95 371 }
s1725696 6:f495a77c2c95 372
s1725696 7:ec5add330cb3 373 if(button_wait == true) // condition for FAILURE --> WAIT; button_wait press (IF THAT IS EVEN POSSIBLE IN THIS STATE?)
s1725696 6:f495a77c2c95 374 {
s1725696 6:f495a77c2c95 375 CurrentState = WAIT;
s1725696 9:d7a6a3619576 376 pc.printf("State is WAIT\r\n");
s1725696 6:f495a77c2c95 377 StateChanged = true;
s1725696 6:f495a77c2c95 378 }
s1725696 6:f495a77c2c95 379
s1725696 6:f495a77c2c95 380 break;
s1725696 6:f495a77c2c95 381
s1725696 6:f495a77c2c95 382 case DEMO:
s1725696 6:f495a77c2c95 383
s1725696 6:f495a77c2c95 384 if(StateChanged) // so if StateChanged is true
s1725696 6:f495a77c2c95 385 {
s1725696 6:f495a77c2c95 386 // Execute DEMO mode
s1725696 10:56136a0da8c1 387 demo_mode();
s1725696 10:56136a0da8c1 388
s1725696 6:f495a77c2c95 389 StateChanged = false; // state is still DEMO
s1725696 6:f495a77c2c95 390 }
s1725696 6:f495a77c2c95 391
s1725696 7:ec5add330cb3 392 if(button_wait == true) // condition for DEMO --> WAIT; button_wait press
s1725696 6:f495a77c2c95 393 {
s1725696 6:f495a77c2c95 394 CurrentState = WAIT;
s1725696 9:d7a6a3619576 395 pc.printf("State is WAIT\r\n");
s1725696 6:f495a77c2c95 396 StateChanged = true;
s1725696 6:f495a77c2c95 397 }
s1725696 6:f495a77c2c95 398
s1725696 7:ec5add330cb3 399 if (button_emergency == true) // condition for DEMO --> FAILURE; button_emergency press
s1725696 6:f495a77c2c95 400 {
s1725696 6:f495a77c2c95 401 CurrentState = FAILURE;
s1725696 9:d7a6a3619576 402 pc.printf("State is FAILURE\r\n");
s1725696 6:f495a77c2c95 403 StateChanged = true;
s1725696 6:f495a77c2c95 404 }
s1725696 6:f495a77c2c95 405
s1725696 6:f495a77c2c95 406 break;
s1725696 6:f495a77c2c95 407
s1725696 6:f495a77c2c95 408 // no default
s1725696 4:8183e7b228f0 409 }
s1725696 6:f495a77c2c95 410
s1725696 6:f495a77c2c95 411 // while loop does not have to loop every time
s1725696 5:3581013d4505 412 }
s1725696 5:3581013d4505 413
s1725696 0:cb8857cf3ea4 414 }