Diff: main.cpp

Revision:

22:93d32c57bac8

Parent:

21:6412e24cef74

Child:

23:29428dd42769

--- a/main.cpp	Fri Oct 10 12:09:34 2014 +0000
+++ b/main.cpp	Fri Oct 10 12:38:01 2014 +0000
@@ -49,27 +49,28 @@
         
         if (motor1.getPosition()<= ANGLEMIN && toestand != SLAAN)
         {
-            toestand = WACHTEN; 
-            if (EMG(readbiceps, readtriceps) != 0)
-            {
-                toestand = SLAAN;
+            toestand = WACHTEN;
+        }
+        
+        if (EMG(readbiceps, readtriceps) != 0 && toestand = WACHTEN) //door de ifs uit elkaar te halen en && condities toe te voegen kan het minder snel fout gaan!
+        {
+            toestand = SLAAN;
                // potmeter.read() wordt waarschijnlijk output EMG functie (0,1,2,3)
-                if ( EMG(readbiceps, readtriceps)=3)
-                {
-                    setpoint = v3; //experimenteel bepaald, snelheid(?)>>>=pulsewidthmodulation! geclampt tussen -1/1. om bovenste goal te raken 
-                }
-                else if ( EMG(readbiceps, readtriceps)=2)
-                {
-                    setpoint = v2; //experimenteel bepaald, snelheid om middelste goal te raken
-                }
-                else if (EMG(readbiceps, readtriceps)=1)
-                {
-                    setpoint = v1; //experimenteel bepaald, snelheid om bovenste goal te raken
-                }
-                else //waarschijnlijk niet nodig want hij kan hier niet komen als readEMG nul is
-                {
-                    setpoint = 0;  // toestand = TERGUGKEREN?
-                }
+            if ( EMG(readbiceps, readtriceps)=3)
+            {
+                setpoint = v3; //experimenteel bepaald, snelheid(?)>>>=pulsewidthmodulation! geclampt tussen -1/1. om bovenste goal te raken 
+            }
+            else if ( EMG(readbiceps, readtriceps)=2)
+            {
+                setpoint = v2; //experimenteel bepaald, snelheid om middelste goal te raken
+            }
+            else if (EMG(readbiceps, readtriceps)=1)
+            {
+                setpoint = v1; //experimenteel bepaald, snelheid om bovenste goal te raken
+            }
+            else //waarschijnlijk niet nodig want hij kan hier niet komen als readEMG nul is
+            {
+                toestand = TERUGKEREN                }
             }
         }
         
@@ -77,6 +78,10 @@
     // 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 =motor1.getPosition()
     //----------------------------------------------------------------------new_pwm = getal, opletten dat deze tijdens het slaan niet kan veranderen!
+        if (toestand != SLAAN)//is dit de beste manier en plaats voor dit?
+        {
+            setpoint = 0
+        }
         new_pwm = pid(setpoint, motor1.getPosition());
         clamp(&new_pwm, -1,1);
     //-------------------------------------------------------------------------------------output motor richting-------------------------------------
@@ -84,10 +89,11 @@
             motordir = 0;
         else
             motordir = 1;
-        pwm_motor.write(abs(new_pwm));
+        pwm_motor.write(abs(new_pwm)); 
         if(motor1.getPosition()>= ANGLEMAX && toestand=SLAAN)
         { 
             toestand = TERUGKEREN;
+            //setpoint = 0 //misschien -5 oid... (hij moet 0 wel echt berijken)
             // pc.printf ("Toestand = terugkeren!");
         }
     }