Group 9 BioRobotics / Mbed 2 deprecated the_code

Dependencies:   HIDScope Servo mbed QEI biquadFilter

Committer:
s1725696
Date:
Fri Oct 26 10:09:13 2018 +0000
Revision:
10:56136a0da8c1
Parent:
9:d7a6a3619576
minor changes in switch and script lay-out

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