Martijn Grootens / Mbed 2 deprecated mbr_2016_demo_state_machine

Minor Biorobotics 2016 Demo of a possible state machine implementation.

Dependencies:   mbed

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?

UserRevisionLine numberNew 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 }