File content as of revision 0:87e63b5aef8c:

/**
 * 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);
}