Tommie Verouden / Mbed 2 deprecated StateMachine

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   biquadFilter FastPWM MODSERIAL QEI mbed

main.cpp.orig@16:c2986e890040, 2018-11-01 (annotated)

Committer:
tverouden
Date:
Thu Nov 01 10:16:10 2018 +0000
Revision:
16:c2986e890040
Merged LED & EMG stuff

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tverouden 16:c2986e890040 1 // ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ PREPARATION ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
tverouden 16:c2986e890040 2 // Libraries
tverouden 16:c2986e890040 3 #include "mbed.h"
tverouden 16:c2986e890040 4 #include "BiQuad.h"
tverouden 16:c2986e890040 5 #include "FastPWM.h"
tverouden 16:c2986e890040 6 #include "HIDScope.h"
tverouden 16:c2986e890040 7 #include "MODSERIAL.h"
tverouden 16:c2986e890040 8
tverouden 16:c2986e890040 9 // LEDs
tverouden 16:c2986e890040 10 DigitalOut ledRed(LED_RED,1); // red LED K64F
tverouden 16:c2986e890040 11 DigitalOut ledGreen(LED_GREEN,1); // green LED K64F
tverouden 16:c2986e890040 12 DigitalOut ledBlue(LED_BLUE,1); // blue LED K64F
tverouden 16:c2986e890040 13 // DigitalOut ledBio1(,1); // led 1 Biorobotics shield // LED pins
tverouden 16:c2986e890040 14 // DigitalOut ledBio2(,1); // led 2 Biorobotics shield
tverouden 16:c2986e890040 15
tverouden 16:c2986e890040 16 Ticker blinkTimer; // LED ticker
tverouden 16:c2986e890040 17
tverouden 16:c2986e890040 18 // Buttons/inputs
tverouden 16:c2986e890040 19 InterruptIn buttonBio1(D0); // button 1 BioRobotics shield
tverouden 16:c2986e890040 20 InterruptIn buttonBio2(D1); // button 2 BioRobotics shield
tverouden 16:c2986e890040 21 InterruptIn buttonK64F(SW3); // button on K64F
tverouden 16:c2986e890040 22 InterruptIn buttonEmergency(SW2); // emergency button on K64F
tverouden 16:c2986e890040 23
tverouden 16:c2986e890040 24 // Motor pins
tverouden 16:c2986e890040 25
tverouden 16:c2986e890040 26
tverouden 16:c2986e890040 27 // PC communication
tverouden 16:c2986e890040 28 MODSERIAL pc(USBTX, USBRX); // communication with pc
tverouden 16:c2986e890040 29
tverouden 16:c2986e890040 30 // Define & initialise state machine
tverouden 16:c2986e890040 31 enum states { calibratingMotors, calibratingEMG,
tverouden 16:c2986e890040 32 homing, operating, reading, failing, demoing
tverouden 16:c2986e890040 33 };
tverouden 16:c2986e890040 34 states currentState = calibratingMotors;// start in waiting mode
tverouden 16:c2986e890040 35 bool changeState = true; // initialise the first state
tverouden 16:c2986e890040 36
tverouden 16:c2986e890040 37
tverouden 16:c2986e890040 38
tverouden 16:c2986e890040 39 // ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ FUNCTIONS ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
tverouden 16:c2986e890040 40 // ============================ GENERAL FUNCTIONS =============================
tverouden 16:c2986e890040 41 void stopProgram(void)
tverouden 16:c2986e890040 42 {
tverouden 16:c2986e890040 43 // Error message
tverouden 16:c2986e890040 44 pc.printf("[ERROR] emergency button pressed\r\n");
tverouden 16:c2986e890040 45 currentState = failing; // change to state
tverouden 16:c2986e890040 46 changeState = true; // next loop, switch states
tverouden 16:c2986e890040 47 }
tverouden 16:c2986e890040 48
tverouden 16:c2986e890040 49 void blinkLedRed(void)
tverouden 16:c2986e890040 50 {
tverouden 16:c2986e890040 51 ledRed =! ledRed; // toggle LED
tverouden 16:c2986e890040 52 }
tverouden 16:c2986e890040 53 void blinkLedGreen(void)
tverouden 16:c2986e890040 54 {
tverouden 16:c2986e890040 55 ledGreen =! ledGreen; // toggle LED
tverouden 16:c2986e890040 56 }
tverouden 16:c2986e890040 57 void blinkLedBlue(void)
tverouden 16:c2986e890040 58 {
tverouden 16:c2986e890040 59 ledBlue =! ledBlue; // toggle LED
tverouden 16:c2986e890040 60 }
tverouden 16:c2986e890040 61
tverouden 16:c2986e890040 62 // ============================ MOTOR FUNCTIONS ==============================
tverouden 16:c2986e890040 63
tverouden 16:c2986e890040 64
tverouden 16:c2986e890040 65 // ============================= EMG FUNCTIONS ===============================
tverouden 16:c2986e890040 66
tverouden 16:c2986e890040 67
tverouden 16:c2986e890040 68 // ========================= KINEMATICS FUNCTIONS ============================
tverouden 16:c2986e890040 69
tverouden 16:c2986e890040 70
tverouden 16:c2986e890040 71 // ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ STATE MACHINE ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
tverouden 16:c2986e890040 72 void stateMachine(void)
tverouden 16:c2986e890040 73 {
tverouden 16:c2986e890040 74 switch (currentState) { // determine which state Odin is in // Van een aantal if statements moeten de conditions nog bepaald worden, niet vergeten
tverouden 16:c2986e890040 75
tverouden 16:c2986e890040 76 // ========================= MOTOR CALIBRATION MODE ==========================
tverouden 16:c2986e890040 77 case calibratingMotors:
tverouden 16:c2986e890040 78 // ------------------------- initialisation --------------------------
tverouden 16:c2986e890040 79 if (changeState) { // when entering the state
tverouden 16:c2986e890040 80 pc.printf("[MODE] calibrating motors...\r\n");
tverouden 16:c2986e890040 81 // print current state
tverouden 16:c2986e890040 82 changeState = false; // stay in this state
tverouden 16:c2986e890040 83
tverouden 16:c2986e890040 84 // Actions when entering state
tverouden 16:c2986e890040 85 ledRed = 1; // cyan-green blinking LED
tverouden 16:c2986e890040 86 ledGreen = 0;
tverouden 16:c2986e890040 87 ledBlue = 0;
tverouden 16:c2986e890040 88 blinkTimer.attach(&blinkLedBlue,0.5);
tverouden 16:c2986e890040 89
tverouden 16:c2986e890040 90 }
tverouden 16:c2986e890040 91 // ----------------------------- action ------------------------------
tverouden 16:c2986e890040 92 // Actions for each loop iteration
tverouden 16:c2986e890040 93 /* */
tverouden 16:c2986e890040 94
tverouden 16:c2986e890040 95 // --------------------------- transition ----------------------------
tverouden 16:c2986e890040 96 // Transition condition #1: when all motor errors smaller than 0.01,
tverouden 16:c2986e890040 97 // start calibrating EMG
tverouden 16:c2986e890040 98 // if (errorMotorL < 0.01 && errorMotorR < 0.01
tverouden 16:c2986e890040 99 // && errorMotorF < 0.01) {
tverouden 16:c2986e890040 100 // // Actions when leaving state
tverouden 16:c2986e890040 101 // blinkTimer.detach();
tverouden 16:c2986e890040 102 //
tverouden 16:c2986e890040 103 // currentState = calibratingEMG; // change to state
tverouden 16:c2986e890040 104 // changeState = true; // next loop, switch states
tverouden 16:c2986e890040 105 // }
tverouden 16:c2986e890040 106
tverouden 16:c2986e890040 107 break; // end case
tverouden 16:c2986e890040 108
tverouden 16:c2986e890040 109 // =========================== EMG CALIBRATION MODE ===========================
tverouden 16:c2986e890040 110 case calibratingEMG:
tverouden 16:c2986e890040 111 // ------------------------- initialisation --------------------------
tverouden 16:c2986e890040 112 if (changeState) { // when entering the state
tverouden 16:c2986e890040 113 pc.printf("[MODE] calibrating EMG 1 (x-direction)...\r\n");
tverouden 16:c2986e890040 114 // print current state
tverouden 16:c2986e890040 115 changeState = false; // stay in this state
tverouden 16:c2986e890040 116
tverouden 16:c2986e890040 117 // Actions when entering state
tverouden 16:c2986e890040 118 ledRed = 1; // cyan-blue blinking LED
tverouden 16:c2986e890040 119 ledGreen = 0;
tverouden 16:c2986e890040 120 ledBlue = 0;
tverouden 16:c2986e890040 121 blinkTimer.attach(&blinkLedGreen,0.5);
tverouden 16:c2986e890040 122
tverouden 16:c2986e890040 123
tverouden 16:c2986e890040 124 }
tverouden 16:c2986e890040 125 // ----------------------------- action ------------------------------
tverouden 16:c2986e890040 126 // Actions for each loop iteration
tverouden 16:c2986e890040 127 /* */
tverouden 16:c2986e890040 128
tverouden 16:c2986e890040 129 // --------------------------- transition ----------------------------
tverouden 16:c2986e890040 130 // Transition condition #1: after 20 sec in state
tverouden 16:c2986e890040 131 if (1) { // CONDITION
tverouden 16:c2986e890040 132 // Actions when leaving state
tverouden 16:c2986e890040 133 blinkTimer.detach();
tverouden 16:c2986e890040 134
tverouden 16:c2986e890040 135 currentState = homing; // change to state
tverouden 16:c2986e890040 136 changeState = true; // next loop, switch states
tverouden 16:c2986e890040 137 }
tverouden 16:c2986e890040 138 break; // end case
tverouden 16:c2986e890040 139
tverouden 16:c2986e890040 140 // ============================== HOMING MODE ================================
tverouden 16:c2986e890040 141 case homing:
tverouden 16:c2986e890040 142 // ------------------------- initialisation -------------------------- // Evt. ook checken op snelheid als mensen zo dom zijn om tijdens homing op random knopjes te drukken
tverouden 16:c2986e890040 143 if (changeState) { // when entering the state
tverouden 16:c2986e890040 144 pc.printf("[MODE] homing...\r\n");
tverouden 16:c2986e890040 145 // print current state
tverouden 16:c2986e890040 146 changeState = false; // stay in this state
tverouden 16:c2986e890040 147
tverouden 16:c2986e890040 148
tverouden 16:c2986e890040 149 // Actions when entering state
tverouden 16:c2986e890040 150 ledRed = 1; // cyan LED on
tverouden 16:c2986e890040 151 ledGreen = 0;
tverouden 16:c2986e890040 152 ledBlue = 0;
tverouden 16:c2986e890040 153
tverouden 16:c2986e890040 154 }
tverouden 16:c2986e890040 155 // ----------------------------- action ------------------------------
tverouden 16:c2986e890040 156 // Actions for each loop iteration
tverouden 16:c2986e890040 157 /* */
tverouden 16:c2986e890040 158
tverouden 16:c2986e890040 159 // --------------------------- transition ---------------------------- // Volgorde veranderen voor logica?
tverouden 16:c2986e890040 160 // Transition condition #1: with button press, enter demo mode
tverouden 16:c2986e890040 161 if (buttonBio1 == true) { // Het is niet nodig om hier ook nog "&& currentState == homing" toe te voegen, want hij bereikt dit stuk code alleen in homing mode
tverouden 16:c2986e890040 162 // Actions when leaving state
tverouden 16:c2986e890040 163 /* */
tverouden 16:c2986e890040 164
tverouden 16:c2986e890040 165 currentState = demoing; // change to state
tverouden 16:c2986e890040 166 changeState = true; // next loop, switch states
tverouden 16:c2986e890040 167 }
tverouden 16:c2986e890040 168 // Transition condition #2: with button press, enter operation mode
tverouden 16:c2986e890040 169 if (buttonBio2 == true) {
tverouden 16:c2986e890040 170 // Actions when leaving state
tverouden 16:c2986e890040 171 /* */
tverouden 16:c2986e890040 172
tverouden 16:c2986e890040 173 currentState = operating; // change to state
tverouden 16:c2986e890040 174 changeState = true; // next loop, switch states
tverouden 16:c2986e890040 175 }
tverouden 16:c2986e890040 176 break; // end case
tverouden 16:c2986e890040 177
tverouden 16:c2986e890040 178 // ============================= OPERATING MODE ==============================
tverouden 16:c2986e890040 179 case operating:
tverouden 16:c2986e890040 180 // ------------------------- initialisation --------------------------
tverouden 16:c2986e890040 181 if (changeState) { // when entering the state
tverouden 16:c2986e890040 182 pc.printf("[MODE] operating...\r\n");
tverouden 16:c2986e890040 183 // print current state
tverouden 16:c2986e890040 184 changeState = false; // stay in this state
tverouden 16:c2986e890040 185
tverouden 16:c2986e890040 186 // Actions when entering state
tverouden 16:c2986e890040 187 ledRed = 1; // blue fast blinking LED
tverouden 16:c2986e890040 188 ledGreen = 1;
tverouden 16:c2986e890040 189 ledBlue = 1;
tverouden 16:c2986e890040 190 blinkTimer.attach(&blinkLedBlue,0.25);
tverouden 16:c2986e890040 191
tverouden 16:c2986e890040 192 }
tverouden 16:c2986e890040 193 // ----------------------------- action ------------------------------
tverouden 16:c2986e890040 194 // Actions for each loop iteration
tverouden 16:c2986e890040 195 /* */
tverouden 16:c2986e890040 196
tverouden 16:c2986e890040 197 // --------------------------- transition ----------------------------
tverouden 16:c2986e890040 198 // Transition condition #1: with button press, back to homing mode // Is het nodig om vanuit operating mode naar homing mode terug te kunnen gaan? -> ja, voor als je een demo wilt starten
tverouden 16:c2986e890040 199 if (buttonBio2 == true) {
tverouden 16:c2986e890040 200 // Actions when leaving state
tverouden 16:c2986e890040 201 blinkTimer.detach();
tverouden 16:c2986e890040 202
tverouden 16:c2986e890040 203 currentState = homing; // change to state
tverouden 16:c2986e890040 204 changeState = true; // next loop, switch states
tverouden 16:c2986e890040 205 }
tverouden 16:c2986e890040 206 // Transition condition #2: motor angle error < certain value,
tverouden 16:c2986e890040 207 // start reading
tverouden 16:c2986e890040 208 if (1) { // CONDITION
tverouden 16:c2986e890040 209 // Actions when leaving state
tverouden 16:c2986e890040 210 blinkTimer.detach();
tverouden 16:c2986e890040 211
tverouden 16:c2986e890040 212 currentState = homing; // change to state
tverouden 16:c2986e890040 213 changeState = true; // next loop, switch states
tverouden 16:c2986e890040 214 }
tverouden 16:c2986e890040 215 break; // end case
tverouden 16:c2986e890040 216
tverouden 16:c2986e890040 217 // ============================== READING MODE =============================== // Beweegt deze modus zowel heen als weer en zo ja, hoe bepaalt hij wat moet gebeuren?
tverouden 16:c2986e890040 218 case reading:
tverouden 16:c2986e890040 219 // ------------------------- initialisation --------------------------
tverouden 16:c2986e890040 220 if (changeState) { // when entering the state
tverouden 16:c2986e890040 221 pc.printf("[MODE] reading...\r\n");
tverouden 16:c2986e890040 222 // print current state
tverouden 16:c2986e890040 223 changeState = false; // stay in this state
tverouden 16:c2986e890040 224
tverouden 16:c2986e890040 225 // Actions when entering state
tverouden 16:c2986e890040 226 ledRed = 1; // blue LED on
tverouden 16:c2986e890040 227 ledGreen = 1;
tverouden 16:c2986e890040 228 ledBlue = 0;
tverouden 16:c2986e890040 229
tverouden 16:c2986e890040 230 }
tverouden 16:c2986e890040 231 // ----------------------------- action ------------------------------
tverouden 16:c2986e890040 232 // Actions for each loop iteration
tverouden 16:c2986e890040 233 /* */
tverouden 16:c2986e890040 234
tverouden 16:c2986e890040 235 // --------------------------- transition ----------------------------
tverouden 16:c2986e890040 236 // Transition condition #1: with button press, back to homing mode // Hier automatisch terug naar operating mode!
tverouden 16:c2986e890040 237 if (1) { // En hij gaat nu terug naar homing mode, maar dan moet je dus elke keer weer kiezen voor demo of operation mode?
tverouden 16:c2986e890040 238 // Actions when leaving state // CONDITION
tverouden 16:c2986e890040 239 /* */
tverouden 16:c2986e890040 240
tverouden 16:c2986e890040 241 currentState = homing; // change to state
tverouden 16:c2986e890040 242 changeState = true; // next loop, switch states
tverouden 16:c2986e890040 243 }
tverouden 16:c2986e890040 244 break; // end case
tverouden 16:c2986e890040 245
tverouden 16:c2986e890040 246 // ============================== DEMOING MODE ===============================
tverouden 16:c2986e890040 247 case demoing:
tverouden 16:c2986e890040 248 // ------------------------- initialisation --------------------------
tverouden 16:c2986e890040 249 if (changeState) { // when entering the state
tverouden 16:c2986e890040 250 pc.printf("[MODE] demoing...\r\n");
tverouden 16:c2986e890040 251 // print current state
tverouden 16:c2986e890040 252 changeState = false; // stay in this state
tverouden 16:c2986e890040 253
tverouden 16:c2986e890040 254 // Actions when entering state
tverouden 16:c2986e890040 255 ledRed = 0; // yellow LED on
tverouden 16:c2986e890040 256 ledGreen = 0;
tverouden 16:c2986e890040 257 ledBlue = 1;
tverouden 16:c2986e890040 258
tverouden 16:c2986e890040 259 }
tverouden 16:c2986e890040 260 // ----------------------------- action ------------------------------
tverouden 16:c2986e890040 261 // Actions for each loop iteration
tverouden 16:c2986e890040 262 /* */
tverouden 16:c2986e890040 263
tverouden 16:c2986e890040 264 // --------------------------- transition ----------------------------
tverouden 16:c2986e890040 265 // Transition condition #1: with button press, back to homing mode
tverouden 16:c2986e890040 266 if (1) {
tverouden 16:c2986e890040 267 // Actions when leaving state
tverouden 16:c2986e890040 268 /* */
tverouden 16:c2986e890040 269
tverouden 16:c2986e890040 270 currentState = homing; // change to state
tverouden 16:c2986e890040 271 changeState = true; // next loop, switch states
tverouden 16:c2986e890040 272 }
tverouden 16:c2986e890040 273 // Transition condition #2: after 3 sec relaxation, start reading
tverouden 16:c2986e890040 274 if (1) {
tverouden 16:c2986e890040 275 // Actions when leaving state
tverouden 16:c2986e890040 276 /* */
tverouden 16:c2986e890040 277
tverouden 16:c2986e890040 278 currentState = reading; // change to state
tverouden 16:c2986e890040 279 changeState = true; // next loop, switch states
tverouden 16:c2986e890040 280 }
tverouden 16:c2986e890040 281 break; // end case
tverouden 16:c2986e890040 282
tverouden 16:c2986e890040 283 // =============================== FAILING MODE ================================
tverouden 16:c2986e890040 284 case failing:
tverouden 16:c2986e890040 285 // ------------------------- initialisation --------------------------
tverouden 16:c2986e890040 286 if (changeState) { // when entering the state
tverouden 16:c2986e890040 287 pc.printf("[ERROR] entering failure mode\r\n");
tverouden 16:c2986e890040 288 // print current state
tverouden 16:c2986e890040 289 changeState = false; // stay in this state
tverouden 16:c2986e890040 290
tverouden 16:c2986e890040 291 // Actions when entering state
tverouden 16:c2986e890040 292 ledGreen = 1; // fast blinking red LED
tverouden 16:c2986e890040 293 ledBlue = 1;
tverouden 16:c2986e890040 294 ledRed = 0;
tverouden 16:c2986e890040 295 blinkTimer.attach(&blinkLedRed,0.25);
tverouden 16:c2986e890040 296
tverouden 16:c2986e890040 297 // pin3 = 0; // all motor forces to zero // Pins nog niet gedefiniëerd
tverouden 16:c2986e890040 298 // pin5 = 0;
tverouden 16:c2986e890040 299 // pin6 = 0;
tverouden 16:c2986e890040 300 exit (0); // abort mission
tverouden 16:c2986e890040 301 }
tverouden 16:c2986e890040 302 break; // end case
tverouden 16:c2986e890040 303
tverouden 16:c2986e890040 304 // ============================== DEFAULT MODE =================================
tverouden 16:c2986e890040 305 default:
tverouden 16:c2986e890040 306 // ---------------------------- enter failing mode -----------------------------
tverouden 16:c2986e890040 307 currentState = failing; // change to state
tverouden 16:c2986e890040 308 changeState = true; // next loop, switch states
tverouden 16:c2986e890040 309 // print current state
tverouden 16:c2986e890040 310 pc.printf("[ERROR] unknown or unimplemented state reached\r\n");
tverouden 16:c2986e890040 311
tverouden 16:c2986e890040 312 } // end switch
tverouden 16:c2986e890040 313 } // end stateMachine
tverouden 16:c2986e890040 314
tverouden 16:c2986e890040 315
tverouden 16:c2986e890040 316
tverouden 16:c2986e890040 317 // ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ MAIN LOOP ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
tverouden 16:c2986e890040 318
tverouden 16:c2986e890040 319 int main()
tverouden 16:c2986e890040 320 {
tverouden 16:c2986e890040 321 // ================================ EMERGENCY ================================
tverouden 16:c2986e890040 322 //If the emergency button is pressed, stop program via failing state
tverouden 16:c2986e890040 323 buttonEmergency.rise(stopProgram); // Automatische triggers voor failure mode?
tverouden 16:c2986e890040 324
tverouden 16:c2986e890040 325 // ================================ EMERGENCY ================================
tverouden 16:c2986e890040 326 pc.baud(115200); // communication with terminal // Baud rate
tverouden 16:c2986e890040 327
tverouden 16:c2986e890040 328 // ==================================== LOOP ===================================
tverouden 16:c2986e890040 329 while (true) { // loop forever
tverouden 16:c2986e890040 330 stateMachine();
tverouden 16:c2986e890040 331 }
tverouden 16:c2986e890040 332 }