State machine with EMG functions and parameters
Dependencies: biquadFilter FastPWM HIDScope MODSERIAL mbed
Fork of
StateMachine
by 
Tommie Verouden
main.cpp
- Committer:
- tverouden
- Date:
- 2018-10-31
- Revision:
- 5:04b26b2f536a
- Parent:
- 4:5ce2c8864908
- Child:
- 6:f32352bc5078
File content as of revision 5:04b26b2f536a:
// ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ PREPARATION ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
// Libraries
#include "mbed.h"
#include "BiQuad.h"
#include "FastPWM.h"
#include "HIDScope.h"
#include "MODSERIAL.h"
// Inputs & outputs
DigitalOut redled(LED_RED,1); // red LED K64F
DigitalOut greenled(LED_GREEN,1); // green LED K64F
DigitalOut blueled(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 ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
// ============================= 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
/* */
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
/* */
}
// ----------------------------- action ------------------------------
// Actions for each loop iteration
/* */
// --------------------------- transition ----------------------------
// Transition condition #1
if (1) {
// Actions when leaving state
/* */
currentState = failing; // change to state
changeState = true; // next loop, switch states
}
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()
{
// ==================================== LOOP ===================================
while (true) { // loop forever
}
}