Diff: main.cpp

Revision:

14:a861ba49107c

Parent:

13:3351f4374885

Child:

15:1cfe58aea10d

--- a/main.cpp	Wed Nov 01 16:45:32 2017 +0000
+++ b/main.cpp	Wed Nov 01 21:29:46 2017 +0000
@@ -5,6 +5,12 @@
 #include "encoder.h"
 #include "MODSERIAL.h"
 
+//State Machine
+enum States (Cal1, Cal2, CalEMG, Home, EMG, Rest, Demo);
+int State, Counter;
+bool Position_controller_on;
+double Looptime = 0.002f;
+
 //globalvariables Motor
 Ticker Treecko;             //We make a awesome ticker for our control system
 Ticker printer;
@@ -293,6 +299,7 @@
         goaly--;
     }
     pc.printf("goalx = %i, goaly = %i\r\n",goalx, goaly);
+    // DIT MOET NOG HEEL ERG GETUNED WORDEN !!!
 } 
 
 /*
@@ -367,6 +374,57 @@
     //double motorValue = refP;
     //SetMotor1(motorValue);
 }
+void Loop_funtion()
+{   
+    switch(State){
+        case Cal1: //Calibration motor 1
+        // naar achteren bewegen( als voorbeeld Arvid), daarna deze waarde opslaan als offset. Dan bewegen naar home middels PID en verschil encodervalue uiterste stand en home1.
+       motorValue1 = 0.5f; motorValue2=0;
+        
+        
+               if (Huidigepositie1== 0)
+               {
+                    SetMotor1(value); //value is waarde encoder voor loodrechte hoeken,.
+                    if (fabs(huidigepositie1-home1)<0.01) {
+                        state=Cal2
+                    }
+                }
+                else {
+                    SetMotor1(0);
+                    Loop_function();
+                }
+            break;
+            
+        case Cal2: //Calibration motor 2
+                 if (Huidigepositie2== 0)
+                 {
+                       if (encoder2.read)<0.01){
+                       state=CalEMG;
+                 }
+                 else {
+                     SetMotor2(0);
+                     Loop_function();
+                 }
+            break;
+        case CalEMG: // Calibration EMG
+            calibrationEMG();   //calculates average EMGFiltered at rest and measures max signal EMGFiltered.
+            state=SelectDevice;
+            break;    
+        case SelectDevice: //Looks at the difference between current position and home. Select aansturen EMG or buttons
+            if button=1; {
+                state=EMG;
+            }
+            if button=0; {
+                state=Demo;
+            }
+            break;
+        case EMG: //Aansturen met EMG
+            Filteren();
+            changePosition();
+            break;
+        case Demo: // Aansturen met toetsenbord
+            break;
+} 
 
 void Tickerfunctie()
 {
@@ -411,8 +469,14 @@
     Treecko.attach(MeasureAndControl, tijdstap);   //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende 
                                             //functies en analoge signalen. Veranderingen worden elke 1 seconde doorgevoerd.
     printer.attach(Tickerfunctie,0.4);
+    
+    //State Machine
+    State = Cal1;
+    Position_controller_on = false;
+    Main_loop.attach(&loop_function, looptime);
     while(true)
     {
+        
         }