Diff: main.cpp

Revision:

15:1cfe58aea10d

Parent:

14:a861ba49107c

Child:

16:2f89d6e25782

--- a/main.cpp	Wed Nov 01 21:29:46 2017 +0000
+++ b/main.cpp	Thu Nov 02 09:10:41 2017 +0000
@@ -5,11 +5,22 @@
 #include "encoder.h"
 #include "MODSERIAL.h"
 
-//State Machine
-enum States (Cal1, Cal2, CalEMG, Home, EMG, Rest, Demo);
-int State, Counter;
+//State Machine en calibratie
+enum States {Cal1, Cal2, CalEMG, SelectDevice, EMG, Rest, Demo};
+States State;
+int Counter;
 bool Position_controller_on;
-double Looptime = 0.002f;
+double looptime = 0.002f;
+double value;
+double home1;
+DigitalIn button (D4);
+DigitalOut led(LED_RED);
+
+//Encoder/motor
+double Huidigepositie1;
+double Huidigepositie2;
+double motorValue1;
+double motorValue2;
 
 //globalvariables Motor
 Ticker Treecko;             //We make a awesome ticker for our control system
@@ -99,7 +110,7 @@
 
 
 
-Timer looptime; //moetuiteindelijk weg
+Timer LooptimeEMG; //moetuiteindelijk weg
 
 //filters
 double emgNotchLB;
@@ -140,8 +151,8 @@
 
 void Filteren() 
 {
-    looptime.reset();
-    looptime.start();
+   // LooptimeEMG.reset();
+   // LooptimeEMG.start();
     
     //EMG 1
     
@@ -284,7 +295,7 @@
   //return maxi;
 }
        
-double changePosition ()
+void changePosition ()    // DIT MOET NOG HEEL ERG GETUNED WORDEN !!!
 {
     if (RBF>0.3) {
         goalx++;    // hoe veel verder gaat hij? 1 cm? 10 cm?
@@ -299,7 +310,6 @@
         goaly--;
     }
     pc.printf("goalx = %i, goaly = %i\r\n",goalx, goaly);
-    // DIT MOET NOG HEEL ERG GETUNED WORDEN !!!
 } 
 
 /*
@@ -349,7 +359,7 @@
     }
 }
 */
-void MeasureAndControl ()
+/* void MeasureAndControl ()
 {
         // hier the control of the control system
      
@@ -367,66 +377,67 @@
        changePosition();
         //rest
     }
-    
+ */ 
     //double Huidigepositie = Encoder(); 
     //double error = (refP - Huidigepositie);// make an error
     //double motorValue = FeedBackControl(error, e_prev, e_int);
     //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;
+            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
+                    if (fabs(Huidigepositie1-home1)<0.01) {
+                        State=Cal2;
                     }
                 }
                 else {
                     SetMotor1(0);
-                    Loop_function();
+                    Loop_funtion();
                 }
             break;
             
         case Cal2: //Calibration motor 2
                  if (Huidigepositie2== 0)
                  {
-                       if (encoder2.read)<0.01){
-                       state=CalEMG;
+                       if (Huidigepositie2<0.01){
+                       State=CalEMG;
                  }
                  else {
                      SetMotor2(0);
-                     Loop_function();
+                     Loop_funtion();
                  }
             break;
         case CalEMG: // Calibration EMG
-            calibrationEMG();   //calculates average EMGFiltered at rest and measures max signal EMGFiltered.
-            state=SelectDevice;
+            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==1) {
+                State=EMG;
             }
-            if button=0; {
-                state=Demo;
+            if (button==0) {
+                State=Demo;
             }
             break;
         case EMG: //Aansturen met EMG
             Filteren();
             changePosition();
             break;
-        case Demo: // Aansturen met toetsenbord
+         case Demo: // Aansturen met toetsenbord
             break;
+        }
 } 
 
-void Tickerfunctie()
+/*void Tickerfunctie()
 {
     //if(caldone == false)
     //{
@@ -436,8 +447,8 @@
     pc.printf("emgreadLT = %f , emgFilteredLT = %f, maxiLT = %f meanrest = %f \r\n",emgLT.read(), LTF, MVCLT,RESTMEANLT);
     //}
 }
-
-int main()
+*/
+int main();//deze moet ooit nog weg --> pas op voor errors
 {
     //voor EMG filteren
     //Left Bicep
@@ -466,18 +477,18 @@
     //motor
    // M1E.period(PwmPeriod); //set PWMposition at 5000hz
     //Ticker
-    Treecko.attach(MeasureAndControl, tijdstap);   //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende 
+    //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);
+  //  printer.attach(Tickerfunctie,0.4);
     
     //State Machine
     State = Cal1;
     Position_controller_on = false;
-    Main_loop.attach(&loop_function, looptime);
+    Treecko.attach(&Loop_funtion, looptime);
     while(true)
     {
         
         }
     
-    
+} //is deze wel nodig?  
 }
\ No newline at end of file