Diff: main.cpp

Revision:

6:f32352bc5078

Parent:

5:04b26b2f536a

Child:

7:ef5966469621

--- a/main.cpp	Wed Oct 31 16:09:44 2018 +0000
+++ b/main.cpp	Wed Oct 31 16:30:27 2018 +0000
@@ -7,9 +7,9 @@
 #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
+DigitalOut  ledRed(LED_RED,1);      // red LED K64F
+DigitalOut  ledGreen(LED_GREEN,1);  // green LED K64F
+DigitalOut  ledBlue(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
@@ -27,7 +27,15 @@
 
 
 // ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ FUNCTIONS ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
-//  ============================= MOTOR 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
+}
+//  ============================ MOTOR FUNCTIONS ==============================
 
 
 //  ============================= EMG FUNCTIONS ===============================
@@ -58,7 +66,7 @@
             // 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
+                // Actions when leaving state                                   // Is hier niet de resetknop voor bedoeld?
                 /* */
 
                 currentState = calibratingMotors;   // change to state
@@ -290,22 +298,14 @@
                 changeState = false;            // stay in this state
 
                 // Actions when entering state
-
-                /* */
-
-            }
-//      ----------------------------- action ------------------------------
-            // Actions for each loop iteration
-            /* */
+                ledGreen = 1;       // red LED on
+                ledBlue = 1;
+                ledRed = 0;
 
-//      --------------------------- transition ----------------------------
-            // Transition condition #1
-            if (1) {
-                // Actions when leaving state
-                /* */
-
-                currentState = failing;             // change to state
-                changeState = true;                 // next loop, switch states
+//                pin3 = 0;           // all motor forces to zero               // Pins nog niet gedefiniëerd
+//                pin5 = 0;
+//                pin6 = 0;
+                exit (0);           // abort mission
             }
             break; // end case
 
@@ -326,6 +326,10 @@
 
 int main()
 {
+//  ================================ EMERGENCY ================================ // Dit hoeft dus niet in de while loop?
+    //If the emergency button is pressed, stop program via failing state
+    emergencybutton.rise(stopProgram);
+
 // ==================================== LOOP ===================================
     while (true) {                  // loop forever