Diff: main.cpp

Revision:

0:87e63b5aef8c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Oct 28 13:55:21 2016 +0000
@@ -0,0 +1,253 @@
+/**
+ * Minor BioRobotics 2016
+ * M.E. Grootens [at] utwente.nl,
+ *
+ * Example code for a 'state machine' with four states:
+ *  - 'Off'                 (LEDs off)
+ *  - 'Calibration'         (Blinking red LED)
+ *  - 'Ready'               (Continues green LED)
+ *  - 'Motion control'      (Blinking blue LED)
+ *
+ * The user can use SW2 to  switch the state:
+ *  - from 'Off' we switch to 'Calibration'
+ *  - from 'Ready' we go to 'Motion control'
+ *  - If the user presses SW2 during another state (i.e. either 'Calibration' or 
+ *      'Motion control'), we switch to 'Off'
+ *  
+ * The 'Calibration' state is finished automatically, after which the machine
+ * goes into 'Ready' state.
+ *
+ * This is just an example of a _possible_ way to create a state machine; there
+ * are endless possibilities
+ */
+
+#include "mbed.h"
+
+// Serial communication
+#define SERIAL_TX           USBTX
+#define SERIAL_RX           USBRX
+#define SERIAL_BAUD         115200
+
+// Interrupt switches
+#define SW_STATE            SW2
+
+// LEDs
+#define LED_READY           LED_GREEN
+#define LED_CALIBRATION     LED_RED
+#define LED_MOTION_CTRL     LED_BLUE
+#define LED_ON              false
+
+/*__ USER IO _________________________________________________________________*/
+ 
+// Serial communication
+Serial serial(SERIAL_TX, SERIAL_RX);
+
+// User interrupt to witch state
+InterruptIn sw_state(SW_STATE);
+
+// LEDs to display state
+DigitalOut led_ready(LED_READY);
+DigitalOut led_calibration(LED_CALIBRATION);
+DigitalOut led_motion_ctrl(LED_MOTION_CTRL);
+
+/*__ TIMING / SETTINGS _______________________________________________________*/
+
+// 'Control'
+const float kPeriodControl = 0.5f;  // 2 Hz 'control' loop
+Ticker tick_control;                // Control ticker
+
+// 'Calibration'
+const int kNumCalibrations = 6;     // Number of times Calibration() is executed
+int i_calibration = 0;              // Counter
+
+
+
+/*__ SYSTEM STATES ___________________________________________________________*/
+
+// Definition of system states
+enum State {OFF, CALIBRATION, READY, MOTION_CONTROL};
+
+// Initial state is OFF
+State state_current = OFF;
+
+/**
+ * Get the name of the State
+ * @param state, State of which we want to know the name
+ * @return char array that contains the name of the State
+ */
+char* StateName(State state)
+{
+    switch(state) {
+        case OFF:               return "Off";
+        case CALIBRATION:       return "Calibration";
+        case READY:             return "Ready";
+        case MOTION_CONTROL:    return "Motion Control";
+        default:                return "_UNKNOWN_";
+    }
+}
+
+
+
+/**
+ * Set the new state and print the change to terminal
+ *
+ * Also do 'resets' if needed
+ *  - If we go into calibration mode, we need to reset the counter
+ *
+ * __Only allow to change the state through this function__
+ *
+ * @param state_new new state to be set
+ * @ensure state_current == state_new
+ */
+void SetNewState(State state_new)
+{
+    serial.printf("\r\n[New State: %s (previous: %s)]\r\n",
+        StateName(state_new), StateName(state_current));
+    state_current = state_new;
+    
+    switch (state_new) {
+        case CALIBRATION: {
+            i_calibration = 0;
+            break;
+        } 
+        default: break;
+    }
+}
+
+/**
+ * Switches the state; called by user interrupt from SW2
+ * The new state depends on the current state; read the switch statements
+ */
+void ToggleState()
+{
+    serial.printf("\r\n<User interrupt>\r\n");
+    switch (state_current) {
+        case OFF: {
+            // If the machine is off, then the user wants to start the machine,
+            // but it first needs to be 'calibrated'.
+            SetNewState(CALIBRATION);
+            break;
+        }
+        case READY: {
+            // If the system is ready (after 'calibration'), the user wants to
+            // start 'motion control'
+            SetNewState(MOTION_CONTROL);
+            break;
+        }
+        default: {
+            // If the user presses the button during 'calibration' _or_ 'motion
+            // control', he wants to turn the machine off.
+            SetNewState(OFF);
+            break;
+        }
+    }
+}
+
+/*__ FAKE CALIBRATION AND CONTROL FUNCTIONS __________________________________*/
+
+/**
+ * Fake calibration function.
+ * - Blinks the LED for a specified number of times
+ * - Then switches to 'ready' mode.
+ */
+void Calibration()
+{
+    // LED display
+    led_calibration = !led_calibration;
+    led_ready = !LED_ON;
+    led_motion_ctrl = !LED_ON;
+    
+    // Printing calibration status
+    if (i_calibration==0) {
+        serial.printf("\r\nCalibrating...");
+    }
+    
+    ++i_calibration; // count number of calibration steps
+    serial.printf("%d...",i_calibration);
+    
+    if (i_calibration>=kNumCalibrations) {
+        // finished, so switch state
+        serial.printf("ready\r\n");
+        SetNewState(READY);
+    }
+    
+}
+
+/**
+ * Fake motion control function; blinks a LED
+ */
+void MotionControl()
+{
+    led_calibration = not LED_ON;
+    led_ready = not LED_ON;
+    led_motion_ctrl = !led_motion_ctrl;
+}
+
+/*__ MACHINE FUNCTION DEPENDS ON CURRENT STATE _______________________________*/
+
+/**
+ * 'Control' function that is called by a Ticker.
+ * Depending on the current state, the machine carries out a different task
+ */
+void Control()
+{
+    switch(state_current) {
+        case CALIBRATION: {
+            Calibration();
+            break;
+        }
+        case MOTION_CONTROL: {
+            MotionControl();
+            break;
+        }
+        case READY: {
+            // Ready state is shown by continuous green LED
+            led_calibration = !LED_ON;
+            led_ready       =  LED_ON;
+            led_motion_ctrl = !LED_ON;
+            break;
+        }
+        default: {
+            // Off state is shown by all LEDs off
+            led_calibration = !LED_ON;
+            led_ready       = !LED_ON;
+            led_motion_ctrl = !LED_ON;
+            break;
+        }
+    }
+}
+
+/*__ MAIN FUNCTION ___________________________________________________________*/
+
+/**
+ * Main / initialization function
+ *  - Switch off LEDs
+ *  - Serial comm
+ *  - Attach interrupts
+ *  - Start control ticker
+ */
+int main()
+{
+    // Turn off LEDs
+    state_current = OFF;
+    Control();
+    
+    // Serial baud
+    serial.baud(SERIAL_BAUD);
+    
+    serial.printf("\r\n**STARTING STATE MACHINE DEMO**\r\n");
+    
+    // Interrupt
+    sw_state.fall(&ToggleState);
+    
+    serial.printf("- Interrupts attached\r\n");
+    
+    // Ticker
+    tick_control.attach(&Control, kPeriodControl);
+    
+    serial.printf("- Tickers attached\r\n");
+    serial.printf("Ready. Awaiting user input through SW2...\r\n\r\n");
+    
+    // Infinite loop
+    while (true);
+}
\ No newline at end of file