Martijn Grootens
/
mbr_2016_demo_state_machine
Minor Biorobotics 2016 Demo of a possible state machine implementation.
main.cpp@0:87e63b5aef8c, 2016-10-28 (annotated)
- Committer:
- megrootens
- Date:
- Fri Oct 28 13:55:21 2016 +0000
- Revision:
- 0:87e63b5aef8c
Minor Biorobotics example State Machine
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
megrootens | 0:87e63b5aef8c | 1 | /** |
megrootens | 0:87e63b5aef8c | 2 | * Minor BioRobotics 2016 |
megrootens | 0:87e63b5aef8c | 3 | * M.E. Grootens [at] utwente.nl, |
megrootens | 0:87e63b5aef8c | 4 | * |
megrootens | 0:87e63b5aef8c | 5 | * Example code for a 'state machine' with four states: |
megrootens | 0:87e63b5aef8c | 6 | * - 'Off' (LEDs off) |
megrootens | 0:87e63b5aef8c | 7 | * - 'Calibration' (Blinking red LED) |
megrootens | 0:87e63b5aef8c | 8 | * - 'Ready' (Continues green LED) |
megrootens | 0:87e63b5aef8c | 9 | * - 'Motion control' (Blinking blue LED) |
megrootens | 0:87e63b5aef8c | 10 | * |
megrootens | 0:87e63b5aef8c | 11 | * The user can use SW2 to switch the state: |
megrootens | 0:87e63b5aef8c | 12 | * - from 'Off' we switch to 'Calibration' |
megrootens | 0:87e63b5aef8c | 13 | * - from 'Ready' we go to 'Motion control' |
megrootens | 0:87e63b5aef8c | 14 | * - If the user presses SW2 during another state (i.e. either 'Calibration' or |
megrootens | 0:87e63b5aef8c | 15 | * 'Motion control'), we switch to 'Off' |
megrootens | 0:87e63b5aef8c | 16 | * |
megrootens | 0:87e63b5aef8c | 17 | * The 'Calibration' state is finished automatically, after which the machine |
megrootens | 0:87e63b5aef8c | 18 | * goes into 'Ready' state. |
megrootens | 0:87e63b5aef8c | 19 | * |
megrootens | 0:87e63b5aef8c | 20 | * This is just an example of a _possible_ way to create a state machine; there |
megrootens | 0:87e63b5aef8c | 21 | * are endless possibilities |
megrootens | 0:87e63b5aef8c | 22 | */ |
megrootens | 0:87e63b5aef8c | 23 | |
megrootens | 0:87e63b5aef8c | 24 | #include "mbed.h" |
megrootens | 0:87e63b5aef8c | 25 | |
megrootens | 0:87e63b5aef8c | 26 | // Serial communication |
megrootens | 0:87e63b5aef8c | 27 | #define SERIAL_TX USBTX |
megrootens | 0:87e63b5aef8c | 28 | #define SERIAL_RX USBRX |
megrootens | 0:87e63b5aef8c | 29 | #define SERIAL_BAUD 115200 |
megrootens | 0:87e63b5aef8c | 30 | |
megrootens | 0:87e63b5aef8c | 31 | // Interrupt switches |
megrootens | 0:87e63b5aef8c | 32 | #define SW_STATE SW2 |
megrootens | 0:87e63b5aef8c | 33 | |
megrootens | 0:87e63b5aef8c | 34 | // LEDs |
megrootens | 0:87e63b5aef8c | 35 | #define LED_READY LED_GREEN |
megrootens | 0:87e63b5aef8c | 36 | #define LED_CALIBRATION LED_RED |
megrootens | 0:87e63b5aef8c | 37 | #define LED_MOTION_CTRL LED_BLUE |
megrootens | 0:87e63b5aef8c | 38 | #define LED_ON false |
megrootens | 0:87e63b5aef8c | 39 | |
megrootens | 0:87e63b5aef8c | 40 | /*__ USER IO _________________________________________________________________*/ |
megrootens | 0:87e63b5aef8c | 41 | |
megrootens | 0:87e63b5aef8c | 42 | // Serial communication |
megrootens | 0:87e63b5aef8c | 43 | Serial serial(SERIAL_TX, SERIAL_RX); |
megrootens | 0:87e63b5aef8c | 44 | |
megrootens | 0:87e63b5aef8c | 45 | // User interrupt to witch state |
megrootens | 0:87e63b5aef8c | 46 | InterruptIn sw_state(SW_STATE); |
megrootens | 0:87e63b5aef8c | 47 | |
megrootens | 0:87e63b5aef8c | 48 | // LEDs to display state |
megrootens | 0:87e63b5aef8c | 49 | DigitalOut led_ready(LED_READY); |
megrootens | 0:87e63b5aef8c | 50 | DigitalOut led_calibration(LED_CALIBRATION); |
megrootens | 0:87e63b5aef8c | 51 | DigitalOut led_motion_ctrl(LED_MOTION_CTRL); |
megrootens | 0:87e63b5aef8c | 52 | |
megrootens | 0:87e63b5aef8c | 53 | /*__ TIMING / SETTINGS _______________________________________________________*/ |
megrootens | 0:87e63b5aef8c | 54 | |
megrootens | 0:87e63b5aef8c | 55 | // 'Control' |
megrootens | 0:87e63b5aef8c | 56 | const float kPeriodControl = 0.5f; // 2 Hz 'control' loop |
megrootens | 0:87e63b5aef8c | 57 | Ticker tick_control; // Control ticker |
megrootens | 0:87e63b5aef8c | 58 | |
megrootens | 0:87e63b5aef8c | 59 | // 'Calibration' |
megrootens | 0:87e63b5aef8c | 60 | const int kNumCalibrations = 6; // Number of times Calibration() is executed |
megrootens | 0:87e63b5aef8c | 61 | int i_calibration = 0; // Counter |
megrootens | 0:87e63b5aef8c | 62 | |
megrootens | 0:87e63b5aef8c | 63 | |
megrootens | 0:87e63b5aef8c | 64 | |
megrootens | 0:87e63b5aef8c | 65 | /*__ SYSTEM STATES ___________________________________________________________*/ |
megrootens | 0:87e63b5aef8c | 66 | |
megrootens | 0:87e63b5aef8c | 67 | // Definition of system states |
megrootens | 0:87e63b5aef8c | 68 | enum State {OFF, CALIBRATION, READY, MOTION_CONTROL}; |
megrootens | 0:87e63b5aef8c | 69 | |
megrootens | 0:87e63b5aef8c | 70 | // Initial state is OFF |
megrootens | 0:87e63b5aef8c | 71 | State state_current = OFF; |
megrootens | 0:87e63b5aef8c | 72 | |
megrootens | 0:87e63b5aef8c | 73 | /** |
megrootens | 0:87e63b5aef8c | 74 | * Get the name of the State |
megrootens | 0:87e63b5aef8c | 75 | * @param state, State of which we want to know the name |
megrootens | 0:87e63b5aef8c | 76 | * @return char array that contains the name of the State |
megrootens | 0:87e63b5aef8c | 77 | */ |
megrootens | 0:87e63b5aef8c | 78 | char* StateName(State state) |
megrootens | 0:87e63b5aef8c | 79 | { |
megrootens | 0:87e63b5aef8c | 80 | switch(state) { |
megrootens | 0:87e63b5aef8c | 81 | case OFF: return "Off"; |
megrootens | 0:87e63b5aef8c | 82 | case CALIBRATION: return "Calibration"; |
megrootens | 0:87e63b5aef8c | 83 | case READY: return "Ready"; |
megrootens | 0:87e63b5aef8c | 84 | case MOTION_CONTROL: return "Motion Control"; |
megrootens | 0:87e63b5aef8c | 85 | default: return "_UNKNOWN_"; |
megrootens | 0:87e63b5aef8c | 86 | } |
megrootens | 0:87e63b5aef8c | 87 | } |
megrootens | 0:87e63b5aef8c | 88 | |
megrootens | 0:87e63b5aef8c | 89 | |
megrootens | 0:87e63b5aef8c | 90 | |
megrootens | 0:87e63b5aef8c | 91 | /** |
megrootens | 0:87e63b5aef8c | 92 | * Set the new state and print the change to terminal |
megrootens | 0:87e63b5aef8c | 93 | * |
megrootens | 0:87e63b5aef8c | 94 | * Also do 'resets' if needed |
megrootens | 0:87e63b5aef8c | 95 | * - If we go into calibration mode, we need to reset the counter |
megrootens | 0:87e63b5aef8c | 96 | * |
megrootens | 0:87e63b5aef8c | 97 | * __Only allow to change the state through this function__ |
megrootens | 0:87e63b5aef8c | 98 | * |
megrootens | 0:87e63b5aef8c | 99 | * @param state_new new state to be set |
megrootens | 0:87e63b5aef8c | 100 | * @ensure state_current == state_new |
megrootens | 0:87e63b5aef8c | 101 | */ |
megrootens | 0:87e63b5aef8c | 102 | void SetNewState(State state_new) |
megrootens | 0:87e63b5aef8c | 103 | { |
megrootens | 0:87e63b5aef8c | 104 | serial.printf("\r\n[New State: %s (previous: %s)]\r\n", |
megrootens | 0:87e63b5aef8c | 105 | StateName(state_new), StateName(state_current)); |
megrootens | 0:87e63b5aef8c | 106 | state_current = state_new; |
megrootens | 0:87e63b5aef8c | 107 | |
megrootens | 0:87e63b5aef8c | 108 | switch (state_new) { |
megrootens | 0:87e63b5aef8c | 109 | case CALIBRATION: { |
megrootens | 0:87e63b5aef8c | 110 | i_calibration = 0; |
megrootens | 0:87e63b5aef8c | 111 | break; |
megrootens | 0:87e63b5aef8c | 112 | } |
megrootens | 0:87e63b5aef8c | 113 | default: break; |
megrootens | 0:87e63b5aef8c | 114 | } |
megrootens | 0:87e63b5aef8c | 115 | } |
megrootens | 0:87e63b5aef8c | 116 | |
megrootens | 0:87e63b5aef8c | 117 | /** |
megrootens | 0:87e63b5aef8c | 118 | * Switches the state; called by user interrupt from SW2 |
megrootens | 0:87e63b5aef8c | 119 | * The new state depends on the current state; read the switch statements |
megrootens | 0:87e63b5aef8c | 120 | */ |
megrootens | 0:87e63b5aef8c | 121 | void ToggleState() |
megrootens | 0:87e63b5aef8c | 122 | { |
megrootens | 0:87e63b5aef8c | 123 | serial.printf("\r\n<User interrupt>\r\n"); |
megrootens | 0:87e63b5aef8c | 124 | switch (state_current) { |
megrootens | 0:87e63b5aef8c | 125 | case OFF: { |
megrootens | 0:87e63b5aef8c | 126 | // If the machine is off, then the user wants to start the machine, |
megrootens | 0:87e63b5aef8c | 127 | // but it first needs to be 'calibrated'. |
megrootens | 0:87e63b5aef8c | 128 | SetNewState(CALIBRATION); |
megrootens | 0:87e63b5aef8c | 129 | break; |
megrootens | 0:87e63b5aef8c | 130 | } |
megrootens | 0:87e63b5aef8c | 131 | case READY: { |
megrootens | 0:87e63b5aef8c | 132 | // If the system is ready (after 'calibration'), the user wants to |
megrootens | 0:87e63b5aef8c | 133 | // start 'motion control' |
megrootens | 0:87e63b5aef8c | 134 | SetNewState(MOTION_CONTROL); |
megrootens | 0:87e63b5aef8c | 135 | break; |
megrootens | 0:87e63b5aef8c | 136 | } |
megrootens | 0:87e63b5aef8c | 137 | default: { |
megrootens | 0:87e63b5aef8c | 138 | // If the user presses the button during 'calibration' _or_ 'motion |
megrootens | 0:87e63b5aef8c | 139 | // control', he wants to turn the machine off. |
megrootens | 0:87e63b5aef8c | 140 | SetNewState(OFF); |
megrootens | 0:87e63b5aef8c | 141 | break; |
megrootens | 0:87e63b5aef8c | 142 | } |
megrootens | 0:87e63b5aef8c | 143 | } |
megrootens | 0:87e63b5aef8c | 144 | } |
megrootens | 0:87e63b5aef8c | 145 | |
megrootens | 0:87e63b5aef8c | 146 | /*__ FAKE CALIBRATION AND CONTROL FUNCTIONS __________________________________*/ |
megrootens | 0:87e63b5aef8c | 147 | |
megrootens | 0:87e63b5aef8c | 148 | /** |
megrootens | 0:87e63b5aef8c | 149 | * Fake calibration function. |
megrootens | 0:87e63b5aef8c | 150 | * - Blinks the LED for a specified number of times |
megrootens | 0:87e63b5aef8c | 151 | * - Then switches to 'ready' mode. |
megrootens | 0:87e63b5aef8c | 152 | */ |
megrootens | 0:87e63b5aef8c | 153 | void Calibration() |
megrootens | 0:87e63b5aef8c | 154 | { |
megrootens | 0:87e63b5aef8c | 155 | // LED display |
megrootens | 0:87e63b5aef8c | 156 | led_calibration = !led_calibration; |
megrootens | 0:87e63b5aef8c | 157 | led_ready = !LED_ON; |
megrootens | 0:87e63b5aef8c | 158 | led_motion_ctrl = !LED_ON; |
megrootens | 0:87e63b5aef8c | 159 | |
megrootens | 0:87e63b5aef8c | 160 | // Printing calibration status |
megrootens | 0:87e63b5aef8c | 161 | if (i_calibration==0) { |
megrootens | 0:87e63b5aef8c | 162 | serial.printf("\r\nCalibrating..."); |
megrootens | 0:87e63b5aef8c | 163 | } |
megrootens | 0:87e63b5aef8c | 164 | |
megrootens | 0:87e63b5aef8c | 165 | ++i_calibration; // count number of calibration steps |
megrootens | 0:87e63b5aef8c | 166 | serial.printf("%d...",i_calibration); |
megrootens | 0:87e63b5aef8c | 167 | |
megrootens | 0:87e63b5aef8c | 168 | if (i_calibration>=kNumCalibrations) { |
megrootens | 0:87e63b5aef8c | 169 | // finished, so switch state |
megrootens | 0:87e63b5aef8c | 170 | serial.printf("ready\r\n"); |
megrootens | 0:87e63b5aef8c | 171 | SetNewState(READY); |
megrootens | 0:87e63b5aef8c | 172 | } |
megrootens | 0:87e63b5aef8c | 173 | |
megrootens | 0:87e63b5aef8c | 174 | } |
megrootens | 0:87e63b5aef8c | 175 | |
megrootens | 0:87e63b5aef8c | 176 | /** |
megrootens | 0:87e63b5aef8c | 177 | * Fake motion control function; blinks a LED |
megrootens | 0:87e63b5aef8c | 178 | */ |
megrootens | 0:87e63b5aef8c | 179 | void MotionControl() |
megrootens | 0:87e63b5aef8c | 180 | { |
megrootens | 0:87e63b5aef8c | 181 | led_calibration = not LED_ON; |
megrootens | 0:87e63b5aef8c | 182 | led_ready = not LED_ON; |
megrootens | 0:87e63b5aef8c | 183 | led_motion_ctrl = !led_motion_ctrl; |
megrootens | 0:87e63b5aef8c | 184 | } |
megrootens | 0:87e63b5aef8c | 185 | |
megrootens | 0:87e63b5aef8c | 186 | /*__ MACHINE FUNCTION DEPENDS ON CURRENT STATE _______________________________*/ |
megrootens | 0:87e63b5aef8c | 187 | |
megrootens | 0:87e63b5aef8c | 188 | /** |
megrootens | 0:87e63b5aef8c | 189 | * 'Control' function that is called by a Ticker. |
megrootens | 0:87e63b5aef8c | 190 | * Depending on the current state, the machine carries out a different task |
megrootens | 0:87e63b5aef8c | 191 | */ |
megrootens | 0:87e63b5aef8c | 192 | void Control() |
megrootens | 0:87e63b5aef8c | 193 | { |
megrootens | 0:87e63b5aef8c | 194 | switch(state_current) { |
megrootens | 0:87e63b5aef8c | 195 | case CALIBRATION: { |
megrootens | 0:87e63b5aef8c | 196 | Calibration(); |
megrootens | 0:87e63b5aef8c | 197 | break; |
megrootens | 0:87e63b5aef8c | 198 | } |
megrootens | 0:87e63b5aef8c | 199 | case MOTION_CONTROL: { |
megrootens | 0:87e63b5aef8c | 200 | MotionControl(); |
megrootens | 0:87e63b5aef8c | 201 | break; |
megrootens | 0:87e63b5aef8c | 202 | } |
megrootens | 0:87e63b5aef8c | 203 | case READY: { |
megrootens | 0:87e63b5aef8c | 204 | // Ready state is shown by continuous green LED |
megrootens | 0:87e63b5aef8c | 205 | led_calibration = !LED_ON; |
megrootens | 0:87e63b5aef8c | 206 | led_ready = LED_ON; |
megrootens | 0:87e63b5aef8c | 207 | led_motion_ctrl = !LED_ON; |
megrootens | 0:87e63b5aef8c | 208 | break; |
megrootens | 0:87e63b5aef8c | 209 | } |
megrootens | 0:87e63b5aef8c | 210 | default: { |
megrootens | 0:87e63b5aef8c | 211 | // Off state is shown by all LEDs off |
megrootens | 0:87e63b5aef8c | 212 | led_calibration = !LED_ON; |
megrootens | 0:87e63b5aef8c | 213 | led_ready = !LED_ON; |
megrootens | 0:87e63b5aef8c | 214 | led_motion_ctrl = !LED_ON; |
megrootens | 0:87e63b5aef8c | 215 | break; |
megrootens | 0:87e63b5aef8c | 216 | } |
megrootens | 0:87e63b5aef8c | 217 | } |
megrootens | 0:87e63b5aef8c | 218 | } |
megrootens | 0:87e63b5aef8c | 219 | |
megrootens | 0:87e63b5aef8c | 220 | /*__ MAIN FUNCTION ___________________________________________________________*/ |
megrootens | 0:87e63b5aef8c | 221 | |
megrootens | 0:87e63b5aef8c | 222 | /** |
megrootens | 0:87e63b5aef8c | 223 | * Main / initialization function |
megrootens | 0:87e63b5aef8c | 224 | * - Switch off LEDs |
megrootens | 0:87e63b5aef8c | 225 | * - Serial comm |
megrootens | 0:87e63b5aef8c | 226 | * - Attach interrupts |
megrootens | 0:87e63b5aef8c | 227 | * - Start control ticker |
megrootens | 0:87e63b5aef8c | 228 | */ |
megrootens | 0:87e63b5aef8c | 229 | int main() |
megrootens | 0:87e63b5aef8c | 230 | { |
megrootens | 0:87e63b5aef8c | 231 | // Turn off LEDs |
megrootens | 0:87e63b5aef8c | 232 | state_current = OFF; |
megrootens | 0:87e63b5aef8c | 233 | Control(); |
megrootens | 0:87e63b5aef8c | 234 | |
megrootens | 0:87e63b5aef8c | 235 | // Serial baud |
megrootens | 0:87e63b5aef8c | 236 | serial.baud(SERIAL_BAUD); |
megrootens | 0:87e63b5aef8c | 237 | |
megrootens | 0:87e63b5aef8c | 238 | serial.printf("\r\n**STARTING STATE MACHINE DEMO**\r\n"); |
megrootens | 0:87e63b5aef8c | 239 | |
megrootens | 0:87e63b5aef8c | 240 | // Interrupt |
megrootens | 0:87e63b5aef8c | 241 | sw_state.fall(&ToggleState); |
megrootens | 0:87e63b5aef8c | 242 | |
megrootens | 0:87e63b5aef8c | 243 | serial.printf("- Interrupts attached\r\n"); |
megrootens | 0:87e63b5aef8c | 244 | |
megrootens | 0:87e63b5aef8c | 245 | // Ticker |
megrootens | 0:87e63b5aef8c | 246 | tick_control.attach(&Control, kPeriodControl); |
megrootens | 0:87e63b5aef8c | 247 | |
megrootens | 0:87e63b5aef8c | 248 | serial.printf("- Tickers attached\r\n"); |
megrootens | 0:87e63b5aef8c | 249 | serial.printf("Ready. Awaiting user input through SW2...\r\n\r\n"); |
megrootens | 0:87e63b5aef8c | 250 | |
megrootens | 0:87e63b5aef8c | 251 | // Infinite loop |
megrootens | 0:87e63b5aef8c | 252 | while (true); |
megrootens | 0:87e63b5aef8c | 253 | } |