Diff: main.cpp

Revision:

17:0116e2f17d0d

Parent:

16:bf4c5affd3e9

Child:

18:1188cdb7ccbe

--- a/main.cpp	Fri Oct 10 07:56:24 2014 +0000
+++ b/main.cpp	Fri Oct 10 08:07:06 2014 +0000
@@ -49,39 +49,36 @@
         if (angle<anglemin)
         {
             toestand = WACHTEN;
+            if (EMG(readbiceps, readtriceps) != 0)
+            {
+                toestand = SLAAN;
+               // potmeter.read() wordt waarschijnlijk output EMG functie (0,1,2,3)
+                if ( 0.75 < potmeter.read() && angle<anglemax) //angle<anglemax moet ergens anders staan (iets met toestand)
+                {
+                    setpoint = v3; //experimenteel bepaald, snelheid om bovenste goal te raken
+                }
+                else if ( 0.50 < potmeter.read())
+                {
+                    setpoint = v2; //experimenteel bepaald, snelheid om middelste goal te raken
+                }
+                else if (0.25 < potmeter.read())
+                {
+                    setpoint = v1; //experimenteel bepaald, snelheid om bovenste goal te raken
+                }
+                else 
+                {
+                    setpoint = 0;  // toestand = TERGUGKEREN?
+                }
+            }
         }
         
-        if (toestand = WACHTEN && EMG(readbiceps, readtriceps) != 0)
-        {
-            toestand = SLAAN
-        }
-    // De toestand moet vanaf wacht naar slaan gaan bij aanspanning van spier(1) Hier zit waarschijnlijk een delay in 
+    // De toestand moet vanaf wacht naar slaan gaan bij aanspanning van spier(1) Hier zit waarschijnlijk een !!!delay!!! in 
     // vanwege het middelen over de tijd van het signaal. van MK en Tanja stukje script met functie met return (0/1/2 of 3).
     // angle = ???? Encoder
-        if(toestand==SLAAN)
-        {
-            // potmeter.read() wordt waarschijnlijk output EMG functie (0,1,2,3)
-            if ( 0.75 < potmeter.read() && angle<anglemax) //angle<anglemax moet ergens anders staan (iets met toestand)
-            {
-                setpoint = v3; //experimenteel bepaald, snelheid om bovenste goal te raken
-            }
-            else if ( 0.50 < potmeter.read())
-            {
-                setpoint = v2; //experimenteel bepaald, snelheid om middelste goal te raken
-            }
-            else if (0.25 < potmeter.read())
-            {
-                setpoint = v1; //experimenteel bepaald, snelheid om bovenste goal te raken
-            }
-            else 
-            {
-                setpoint = 0;  // toestand = TERGUGKEREN?
-            }
-        }
-        //----------------------------------------------------------------------new_pwm = getal----------------------------------------------------
+    //----------------------------------------------------------------------new_pwm = getal----------------------------------------------------
         new_pwm = pid(setpoint, motor1.getPosition());
         clamp(&new_pwm, -1,1);
-        //-------------------------------------------------------------------------------------output motor richting-------------------------------------
+    //-------------------------------------------------------------------------------------output motor richting-------------------------------------
         if(new_pwm > 0)
             motordir = 0;
         else