File content as of revision 6:f32352bc5078:

// ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ PREPARATION ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
// Libraries
#include "mbed.h"
#include "BiQuad.h"
#include "FastPWM.h"
#include "HIDScope.h"
#include "MODSERIAL.h"

// Inputs & outputs
DigitalOut  ledRed(LED_RED,1);      // red LED K64F
DigitalOut  ledGreen(LED_GREEN,1);  // green LED K64F
DigitalOut  ledBlue(LED_BLUE,1);    // blue LED K64F
InterruptIn buttonBio1(D0);         // button 1 BioRobotics shield
InterruptIn buttonBio2(D1);         // button 2 BioRobotics shield
InterruptIn buttonK64F(SW3);        // button on K64F
InterruptIn emergencybutton(SW2);   // emergency button on K64F

MODSERIAL pc(USBTX, USBRX);         // communication with pc

// Define & initialise state machine
enum states {   waiting, calibratingMotors, calibratingEMGx, calibratingEMGy,
                homing, operating, flipping, failing, demoing
            };
states currentState = waiting;      // start in waiting mode
bool changeState = true;            // initialise the first state



// ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ FUNCTIONS ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
// ============================ GENERAL FUNCTIONS =============================
void stopProgram(void)
{
    // Error message
    pc.printf("[ERROR] emergency button pressed\r\n");
    currentState = failing;             // change to state
    changeState = true;                 // next loop, switch states
}
//  ============================ MOTOR FUNCTIONS ==============================


//  ============================= EMG FUNCTIONS ===============================


// ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ STATE MACHINE ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
void stateMachine(void)                                                         // Moet niet elke staat naar de failure mode kunnen komen met het emergency knopoje?
{
    switch (currentState) {         // determine which state Odin is in         // Van een aantal if statements moeten de conditions nog bepaald worden, niet vergeten

//  ============================== WAITING MODE ===============================
        case waiting:
//      ------------------------ initialisation --------------------------
            if (changeState) {                  // when entering the state
                pc.printf("[MODE] waiting...\r\n");
                // print current state
                changeState = false;            // stay in this state

                // Actions when entering state
                /* */

            }
//      ----------------------------- action ------------------------------
            // Actions for each loop iteration
            /* */

//      --------------------------- transition ----------------------------
            // Transition condition #1: with a button press, enter motor
            //      calibration mode
            if (emergencybutton == true) {                                      // Waarom drukken we op de noodknop om te beginnen? Lijkt me raar
                // Actions when leaving state                                   // Is hier niet de resetknop voor bedoeld?
                /* */

                currentState = calibratingMotors;   // change to state
                changeState = true;                 // next loop, switch states
            }
            break; // end case

//  ========================= MOTOR CALIBRATION MODE ==========================
        case calibratingMotors:
//      ------------------------- initialisation --------------------------
            if (changeState) {                  // when entering the state
                pc.printf("[MODE] calibrating motors...r\n");
                // print current state
                changeState = false;            // stay in this state

                // Actions when entering state
                /* */

            }
//      ----------------------------- action ------------------------------
            // Actions for each loop iteration
            /* */

//      --------------------------- transition ----------------------------
            // Transition condition #1: after 3 sec in state && all motor       // Als vóór het einde van de 3 seconden alle motoren al snelheid 0 hebben, zitten ze zo dus door te draaien terwijl dat niet kan
            //      velocities zero, start calibrating EMG-x                    // dat lijkt me niet de bedoeling
            if (1) {
                // Actions when leaving state
                /* */

                currentState = calibratingMotors;   // change to state
                changeState = true;                 // next loop, switch states
            }
            break; // end case

//  ================== EMG 1 (X-DIRECTION) CALIBRATION MODE ===================
        case calibratingEMGx:
//      ------------------------- initialisation --------------------------
            if (changeState) {                  // when entering the state
                pc.printf("[MODE] calibrating EMG 1 (x-direction)...\r\n");
                // print current state
                changeState = false;            // stay in this state

                // Actions when entering state
                /* */

            }
//      ----------------------------- action ------------------------------
            // Actions for each loop iteration
            /* */

//      --------------------------- transition ----------------------------
            // Transition condition #1: after 5 sec in state && relaxation
            if (1) {
                // Actions when leaving state
                /* */

                currentState = calibratingEMGy;     // change to state
                changeState = true;                 // next loop, switch states
            }
            break; // end case

//  ================== EMG 2 (Y-DIRECTION) CALIBRATION MODE ===================
        case calibratingEMGy:
//      ------------------------- initialisation --------------------------
            if (changeState) {                  // when entering the state
                pc.printf("[MODE] calibrating EMG 2 (y-direction)...\r\n");
                // print current state
                changeState = false;            // stay in this state

                // Actions when entering state
                /* */

            }
//      ----------------------------- action ------------------------------
            // Actions for each loop iteration
            /* */

//      --------------------------- transition ----------------------------
            // Transition condition #1: after 5 sec in state && relaxation
            if (1) {
                // Actions when leaving state
                /* */

                currentState = homing;              // change to state
                changeState = true;                 // next loop, switch states
            }
            break; // end case

//  ============================== HOMING MODE ================================
        case homing:
//      ------------------------- initialisation --------------------------
            if (changeState) {                  // when entering the state
                pc.printf("[MODE] homing...\r\n");
                // print current state
                changeState = false;            // stay in this state


                // Actions when entering state
                /* */

            }
//      ----------------------------- action ------------------------------
            // Actions for each loop iteration
            /* */

//      --------------------------- transition ----------------------------
            // Transition condition #1: with button press, enter demo mode
            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
                // Actions when leaving state
                /* */

                currentState = demoing;             // change to state
                changeState = true;                 // next loop, switch states
            }
            // Transition condition #2: with button press, enter operation mode
            if (buttonBio2 == true) {
                // Actions when leaving state
                /* */

                currentState = operating;           // change to state
                changeState = true;                 // next loop, switch states
            }
            break; // end case

//  ============================= OPERATING MODE ==============================
        case operating:
//      ------------------------- initialisation --------------------------
            if (changeState) {                  // when entering the state
                pc.printf("[MODE] operating...\r\n");
                // print current state
                changeState = false;            // stay in this state

                // Actions when entering state
                /* */

            }
//      ----------------------------- action ------------------------------
            // Actions for each loop iteration
            /* */

//      --------------------------- transition ----------------------------
            // Transition condition #1: with button press, back to homing mode
            if (buttonBio2 == true) {
                // Actions when leaving state
                /* */

                currentState = homing;              // change to state
                changeState = true;                 // next loop, switch states
            }
            // Transition condition #2: after 3 sec relaxation, start flipping  // In 3 seconden zijn de bladzijden uit zichzelf alweer helemaal teruggegaan. Misschien dit gewoon doen na het voorgedefinieerde pad
            if (1) {
                // Actions when leaving state
                /* */

                currentState = homing;              // change to state
                changeState = true;                 // next loop, switch states
            }
            break; // end case

//  ============================= FLIPPING MODE =============================== // Beweegt deze modus zowel heen als weer en zo ja, hoe bepaalt hij wat moet gebeuren?
        case flipping:
//      ------------------------- initialisation --------------------------
            if (changeState) {                  // when entering the state
                pc.printf("[MODE] flipping...\r\n");
                // print current state
                changeState = false;            // stay in this state

                // Actions when entering state
                /* */

            }
//      ----------------------------- action ------------------------------
            // Actions for each loop iteration
            /* */

//      --------------------------- transition ----------------------------
            // Transition condition #1: with button press, back to homing mode  // Moet ook dit niet automatisch gaan? Nu blijft hij vrolijk in die modus niks doen...
            if (1) {                                                            // En hij gaat nu terug naar homing mode, maar dan moet je dus elke keer weer kiezen voor demo of operation mode?
                // Actions when leaving state
                /* */

                currentState = homing;              // change to state
                changeState = true;                 // next loop, switch states
            }
            break; // end case

//  ============================== DEMOING MODE ===============================
        case demoing:
//      ------------------------- initialisation --------------------------
            if (changeState) {                  // when entering the state
                pc.printf("[MODE] demoing...\r\n");
                // print current state
                changeState = false;            // stay in this state

                // Actions when entering state
                /* */

            }
//      ----------------------------- action ------------------------------
            // Actions for each loop iteration
            /* */

//      --------------------------- transition ----------------------------
            // Transition condition #1: with button press, back to homing mode
            if (1) {
                // Actions when leaving state
                /* */

                currentState = homing;              // change to state
                changeState = true;                 // next loop, switch states
            }
            // Transition condition #2: after 3 sec relaxation, start flipping  // In 3 seconden zijn de bladzijden uit zichzelf alweer helemaal teruggegaan. Misschien dit gewoon doen na het voorgedefinieerde pad
            if (1) {
                // Actions when leaving state
                /* */

                currentState = flipping;            // change to state
                changeState = true;                 // next loop, switch states
            }
            break; // end case

// =============================== FAILING MODE ================================
        case failing:
//      ------------------------- initialisation --------------------------
            if (changeState) {                  // when entering the state
                pc.printf("[ERROR] entering failure mode\r\n");
                // print current state
                changeState = false;            // stay in this state

                // Actions when entering state
                ledGreen = 1;       // red LED on
                ledBlue = 1;
                ledRed = 0;

//                pin3 = 0;           // all motor forces to zero               // Pins nog niet gedefiniëerd
//                pin5 = 0;
//                pin6 = 0;
                exit (0);           // abort mission
            }
            break; // end case

// ============================== DEFAULT MODE =================================
        default:
// ---------------------------- enter failing mode -----------------------------
            currentState = failing;                 // change to state
            changeState = true;                     // next loop, switch states
            // print current state
            pc.printf("[ERROR] unknown or unimplemented state reached\r\n");

    }   // end switch
}       // end stateMachine



// ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ MAIN LOOP ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡

int main()
{
//  ================================ EMERGENCY ================================ // Dit hoeft dus niet in de while loop?
    //If the emergency button is pressed, stop program via failing state
    emergencybutton.rise(stopProgram);

// ==================================== LOOP ===================================
    while (true) {                  // loop forever

    }
}