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
- Revision:
- 15:6566c5dedeeb
- Parent:
- 12:323ac4e27a0d
- Child:
- 16:c2986e890040
--- a/main.cpp Thu Nov 01 09:02:56 2018 +0000
+++ b/main.cpp Thu Nov 01 10:12:16 2018 +0000
@@ -11,7 +11,7 @@
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
+// DigitalOut ledBio2(,1); // led 2 Biorobotics shield
Ticker blinkTimer; // LED ticker
@@ -46,11 +46,19 @@
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
+void blinkLedRed(void)
+{
+ ledRed =! ledRed; // toggle LED
}
-
+void blinkLedGreen(void)
+{
+ ledGreen =! ledGreen; // toggle LED
+}
+void blinkLedBlue(void)
+{
+ ledBlue =! ledBlue; // toggle LED
+}
+
// ============================ MOTOR FUNCTIONS ==============================
@@ -61,7 +69,7 @@
// ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ STATE MACHINE ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
-void stateMachine(void)
+void stateMachine(void)
{
switch (currentState) { // determine which state Odin is in // Van een aantal if statements moeten de conditions nog bepaald worden, niet vergeten
@@ -69,12 +77,15 @@
case calibratingMotors:
// ------------------------- initialisation --------------------------
if (changeState) { // when entering the state
- pc.printf("[MODE] calibrating motors...r\n");
+ 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 ------------------------------
@@ -82,15 +93,17 @@
/* */
// --------------------------- 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
- /* */
+ // 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
+// }
- currentState = calibratingEMG; // change to state
- changeState = true; // next loop, switch states
- }
break; // end case
// =========================== EMG CALIBRATION MODE ===========================
@@ -102,7 +115,11 @@
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 ------------------------------
@@ -113,7 +130,7 @@
// 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
@@ -130,7 +147,9 @@
// Actions when entering state
- /* */
+ ledRed = 1; // cyan LED on
+ ledGreen = 0;
+ ledBlue = 0;
}
// ----------------------------- action ------------------------------
@@ -165,7 +184,10 @@
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 ------------------------------
@@ -176,16 +198,16 @@
// 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
+ // start reading
if (1) { // CONDITION
// Actions when leaving state
- /* */
+ blinkTimer.detach();
currentState = homing; // change to state
changeState = true; // next loop, switch states
@@ -201,7 +223,9 @@
changeState = false; // stay in this state
// Actions when entering state
- /* */
+ ledRed = 1; // blue LED on
+ ledGreen = 1;
+ ledBlue = 0;
}
// ----------------------------- action ------------------------------
@@ -228,7 +252,9 @@
changeState = false; // stay in this state
// Actions when entering state
- /* */
+ ledRed = 0; // yellow LED on
+ ledGreen = 0;
+ ledBlue = 1;
}
// ----------------------------- action ------------------------------
@@ -244,7 +270,7 @@
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
+ // Transition condition #2: after 3 sec relaxation, start reading
if (1) {
// Actions when leaving state
/* */
@@ -259,13 +285,14 @@
// ------------------------- initialisation --------------------------
if (changeState) { // when entering the state
pc.printf("[ERROR] entering failure mode\r\n");
- // print current state
+ // print current state
changeState = false; // stay in this state
// Actions when entering state
- ledGreen = 1; // red LED on // Blijft dit aan?
+ 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;
@@ -292,14 +319,14 @@
int main()
{
// ================================ EMERGENCY ================================
- //If the emergency button is pressed, stop program via failing state
+ //If the emergency button is pressed, stop program via failing state
buttonEmergency.rise(stopProgram); // Automatische triggers voor failure mode?
-
-// ================================ EMERGENCY ================================
+
+// ================================ EMERGENCY ================================
pc.baud(115200); // communication with terminal // Baud rate
// ==================================== LOOP ===================================
while (true) { // loop forever
-
+ stateMachine();
}
}
