Dependencies: biquadFilter FastPWM HIDScope MODSERIAL mbed
Fork of
StateMachine
by 
Tommie Verouden
main.cpp
- Committer:
- tverouden
- Date:
- 2018-11-01
- Revision:
- 12:323ac4e27a0d
- Parent:
- 11:2d1dfebae948
- Child:
- 13:abee61d15b5f
File content as of revision 12:323ac4e27a0d:
// ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ 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 blinkLED(DigitalOut led) // blinkTimer.attach(&blinkLED,0.5) aanroepen bij initialisation, bij verlaten state:
{ // blinkTimer.detach
led =! led; // 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
/* */
}
// ----------------------------- 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 = 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
/* */
}
// ----------------------------- 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
}
}