Tommie Verouden
/
StateMachine
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: biquadFilter FastPWM MODSERIAL QEI mbed
Diff: main.cpp.orig
- Revision:
- 16:c2986e890040
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp.orig Thu Nov 01 10:16:10 2018 +0000
@@ -0,0 +1,332 @@
+// ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ 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();
+ }
+}
