Diff: main.cpp

Revision:

11:4bc0304978e2

Parent:

10:83f3cec8dd1c

Child:

12:93ad9781eeef

--- a/main.cpp	Mon Oct 14 13:34:03 2019 +0000
+++ b/main.cpp	Mon Oct 14 14:07:30 2019 +0000
@@ -7,7 +7,7 @@
 #include <math.h>
 #include "Servo.h"
 
-//definieer objecten
+// Definieer objecten
 Serial pc(USBTX, USBRX);
 
 PwmOut motor1(D6);  
@@ -18,21 +18,27 @@
 
 Ticker loop_ticker;
     
-// Motoren uitzetten
+// Motoren aan-/uitzetten
 void motors_on_off()
     {
           motor1=!motor1;
           motor2=!motor2;
           pc.printf("Motoren aan/uit functie\r\n");
     }    
-    
+
+// Emergency    
 void emergency()
     {
+        loop_ticker.detach(); 
         motor1.write(0);
         motor2.write(0);
-        pc.printf("Noodgeval functie\r\n");
+        pc.printf("Ik ga exploderen!!!\r\n");
+        // Alles moet uitgaan (evt. een rood LEDje laten branden), moet 
+        // opnieuw worden opgestart. Mogelijk kan dit door de ticker de 
+        // detachen
     }    
-    
+
+// Motoren uitzetten    
 void motors_off()
     {
         motor1.write(0);
@@ -56,24 +62,19 @@
             if (stateChanged) 
             {
                 motors_off(); // functie waarbij motoren uitgaan
+                stateChanged = false;
                 pc.printf("Motors off state\r\n");
-                stateChanged = false;
             }          
-            if  (Power_button_pressed.read() == false)
+            if  (Power_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false
             {      
-                stateChanged = true;
-                pc.printf("Moving to Calib_motor state\r\n");
                 motors_on_off();
                 currentState = Calib_motor;
+                stateChanged = true;
+                pc.printf("Moving to Calib_motor state\r\n");
             }
-            if  (Emergency_button_pressed.read() == false) 
+            if  (Emergency_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false
             { 
-                pc.printf("Ik ga exploderen!!!\r\n");
-                loop_ticker.detach(); 
                 emergency();
-                // Alles moet uitgaan (evt. een rood LEDje laten branden), moet 
-                // opnieuw worden opgestart. Mogelijk kan dit door de ticker de 
-                // detachen
             } 
             break;
             
@@ -82,35 +83,85 @@
             if (stateChanged)
             {       
                 // Hier wordt een kalibratie uitgevoerd, waarbij de motorhoeken worden bepaald
+                currentState = Calib_EMG;
+                stateChanged = true;
                 pc.printf("Moving to Calib_EMG state\r\n");
             }    
-           
+            if  (Emergency_button_pressed.read() == false) 
+            { 
+                emergency();
+            } 
+            break;
+            
+         case Calib_EMG:
+            
+            if (stateChanged)
+            {
+                // Hierbij wordt een een kalibratie uitgevoerd, waarbij de maximale EMG-amplitude waarde wordt bepaald
+                currentState = Homing;
+                stateChanged = true;
+                pc.printf("Moving to Homing state\r\n");
+            }
             if  (Emergency_button_pressed.read() == false) 
             { 
-                pc.printf("Ik ga exploderen!!!\r\n");
-                loop_ticker.detach(); 
+                emergency();
+            } 
+            break; 
+            
+         case Homing:
+            
+            if (stateChanged)
+            {   
+                // Ervoor zorgen dat de motoren zo bewegen dat de robotarm 
+                // (inclusief de end-effector) in de juiste home positie wordt gezet
+                currentState = Operation_mode;
+                stateChanged = true;
+                pc.printf("Moving to operation mode\n");
+            }
+            if  (Emergency_button_pressed.read() == false) 
+            { 
                 emergency();
-                // Alles moet uitgaan (evt. een rood LEDje laten branden), moet 
-                // opnieuw worden opgestart. Mogelijk kan dit door de ticker de 
-                // detachen
             } 
-            break;
+            break; 
+            
+         case Operation_mode:
+        
+            if (stateChanged)
+            { 
+                // Hier moet een functie worden aangeroepen die ervoor zorgt dat 
+                // aan de hand van EMG-signalen de motoren kunnen worden aangestuurd, 
+                // zodat de robotarm kan bewegen   
+                
+                if (Power_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false
+                {
+                    motors_on_off()
+                    currentState = Motors_off;
+                    stateChanged = true;
+                    pc.printf("Terug naar de state Motors_off");
+                }
+                if  (Emergency_button_pressed.read() == false) 
+                { 
+                    emergency();
+                } 
+                else 
+                { 
+                    currentState = Homing; 
+                    stateChanged = true; 
+                    pc.printf("Terug naar de state Homing");
+                }
+                break;
                      
         default:
             // Zelfde functie als die eerder is toegepast om motoren uit te schakelen -> safety!
             pc.printf("Unknown or uninplemented state reached!\r\n");
             
-        }
     }
+}
 
 int main(void)
     {
         pc.printf("Opstarten\r\n");
-        loop_ticker.attach(&ProcessStateMachine, 3.0f);  
+        loop_ticker.attach(&ProcessStateMachine, 1.0f);  
         while(true) 
-        { 
-            // pc.printf("powerbutton: %d\r\n", Power_button_pressed.read());
-            // pc.printf("emergencybutton: %d\r\n", Emergency_button_pressed.read());
-            // wait(0.5);
-            /* do nothing */}
+        { /* do nothing */}
     }
\ No newline at end of file