File content as of revision 15:6566c5dedeeb:

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

// LEDs
DigitalOut ledRed(LED_RED,1);       // red LED K64F
DigitalOut ledGreen(LED_GREEN,1);   // green LED K64F
DigitalOut ledBlue(LED_BLUE,1);     // blue LED K64F
// DigitalOut ledBio1(,1);          // led 1 Biorobotics shield                 // LED pins
// DigitalOut ledBio2(,1);          // led 2 Biorobotics shield

Ticker blinkTimer;                  // LED ticker

// Buttons/inputs
InterruptIn buttonBio1(D0);         // button 1 BioRobotics shield
InterruptIn buttonBio2(D1);         // button 2 BioRobotics shield
InterruptIn buttonK64F(SW3);        // button on K64F
InterruptIn buttonEmergency(SW2);   // emergency button on K64F

// Motor pins


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

// Define & initialise state machine
enum states {   calibratingMotors, calibratingEMG,
                homing, operating, reading, failing, demoing
            };
states currentState = calibratingMotors;// 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
}

void blinkLedRed(void)
{
    ledRed =! ledRed;                   // toggle LED
}
void blinkLedGreen(void)
{
    ledGreen =! ledGreen;               // toggle LED
}
void blinkLedBlue(void)
{
    ledBlue =! ledBlue;                 // toggle LED
}

//  ============================ MOTOR FUNCTIONS ==============================


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


//  ========================= KINEMATICS FUNCTIONS ============================


// ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ STATE MACHINE ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
void stateMachine(void)
{
    switch (currentState) {         // determine which state Odin is in         // Van een aantal if statements moeten de conditions nog bepaald worden, niet vergeten

//  ========================= 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
                ledRed = 1;                     // cyan-green blinking LED
                ledGreen = 0;
                ledBlue = 0;
                blinkTimer.attach(&blinkLedBlue,0.5);

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

//      --------------------------- transition ----------------------------
            // Transition condition #1: when all motor errors smaller than 0.01,
            //      start calibrating EMG
//            if (errorMotorL < 0.01 && errorMotorR < 0.01
//                    && errorMotorF < 0.01) {
//                // Actions when leaving state
//                blinkTimer.detach();
//
//                currentState = calibratingEMG;    // change to state
//                changeState = true;               // next loop, switch states
//            }

            break; // end case

//  =========================== EMG CALIBRATION MODE ===========================
        case calibratingEMG:
//      ------------------------- 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
                ledRed = 1;                     // cyan-blue blinking LED
                ledGreen = 0;
                ledBlue = 0;
                blinkTimer.attach(&blinkLedGreen,0.5);


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

//      --------------------------- transition ----------------------------
            // Transition condition #1: after 20 sec in state
            if (1) {                                                            // CONDITION
                // Actions when leaving state
                blinkTimer.detach();

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

//  ============================== HOMING MODE ================================
        case homing:
//      ------------------------- initialisation --------------------------     // Evt. ook checken op snelheid als mensen zo dom zijn om tijdens homing op random knopjes te drukken
            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
                ledRed = 1;                     // cyan LED on
                ledGreen = 0;
                ledBlue = 0;

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

//      --------------------------- transition ----------------------------     // Volgorde veranderen voor logica?
            // 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
                ledRed = 1;                     // blue fast blinking LED
                ledGreen = 1;
                ledBlue = 1;
                blinkTimer.attach(&blinkLedBlue,0.25);

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

//      --------------------------- transition ----------------------------
            // Transition condition #1: with button press, back to homing mode  // Is het nodig om vanuit operating mode naar homing mode terug te kunnen gaan? -> ja, voor als je een demo wilt starten
            if (buttonBio2 == true) {
                // Actions when leaving state
                blinkTimer.detach();

                currentState = homing;              // change to state
                changeState = true;                 // next loop, switch states
            }
            // Transition condition #2: motor angle error < certain value,
            //      start reading
            if (1) {                                                            // CONDITION
                // Actions when leaving state
                blinkTimer.detach();

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

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

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

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

//      --------------------------- transition ----------------------------
            // Transition condition #1: with button press, back to homing mode  // Hier automatisch terug naar operating mode!
            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                                   // CONDITION
                /* */

                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
                ledRed = 0;                     // yellow LED on
                ledGreen = 0;
                ledBlue = 1;

            }
//      ----------------------------- 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 reading   
            if (1) {
                // Actions when leaving state
                /* */

                currentState = reading;             // 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;                   // fast blinking red LED
                ledBlue = 1;
                ledRed = 0;
                blinkTimer.attach(&blinkLedRed,0.25);

//                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 ================================
    //If the emergency button is pressed, stop program via failing state
    buttonEmergency.rise(stopProgram);                                          // Automatische triggers voor failure mode?

//  ================================ EMERGENCY ================================
    pc.baud(115200); // communication with terminal                             // Baud rate

// ==================================== LOOP ===================================
    while (true) {                  // loop forever
        stateMachine();
    }
}