File content as of revision 8:8cef1050ebd9:

// ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ 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 = 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
}

void blinkLED(DigitalOut led)                                                   // blinkTimer.attach(&blinkLED,0.5) aanroepen bij initialisation, bij verlaten state:
{                                                                               // blinkTimer.detach
    led =! led;                         // toggle LED
    
//  ============================ MOTOR FUNCTIONS ==============================


//  ============================= EMG 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

//  ============================== 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 (buttonEmergency == 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) {                                                            // CONDITION
                // Actions when leaving state
                /* */

                currentState = calibratingEMGx;     // 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
                /* */

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

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

                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
                /* */

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

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

                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
                /* */

                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
                /* */

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

            }
//      ----------------------------- 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   // 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 = 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;       // red LED on                               // Blijft dit aan?
                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 ================================
    //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

    }
}