Diff: main.cpp

Revision:

44:130fa54388ef

Parent:

43:af480e6823ab

Child:

45:d359aad22a8a

--- a/main.cpp	Sun Nov 02 13:06:18 2014 +0000
+++ b/main.cpp	Sun Nov 02 13:16:07 2014 +0000
@@ -698,20 +698,22 @@
     }
     switch(toestand) {
         case SLAAN:
-              if (motor1.getPosition() <= ANGLEMAX) {
-                    toestand = TERUGKEREN;
-                    pc.printf("toestand = terugkeren\n\r");
-                    //motor1.setPosition(0);
-                    pid(0,0,true);
-                    pwm_motor1.write(0);//arvid had hier 0,0 gezet?!
-                    stop = 1;
-                }
-            pidpositie(ANGLEMAX, motor1.getPosition());//dit is nu alleen een waarde, moet vervolgens in een functie terugkomen toch
-            //?? motor1.getPosition(nieuwe positie);
+            pc.printf("SLAAN\n");
             new_pwm = pid(setspeed, motor1.getSpeed(),false);
             pwm_motor1.write(new_pwm);
             motordir1 = 1;
-            pc.printf("SLAAN\n");
+            pidpositie(ANGLEMAX, motor1.getPosition());//dit is nu alleen een waarde, moet vervolgens in een functie terugkomen toch
+            //?? motor1.getPosition(nieuwe positie);
+            
+            if (motor1.getPosition() <= ANGLEMAX) {
+                toestand = TERUGKEREN;
+                pc.printf("toestand = terugkeren\n\r");
+                //motor1.setPosition(0);
+                pid(0,0,true);
+                pwm_motor1.write(0);//arvid had hier 0,0 gezet?!
+                stop = 1;
+            }
+            
             break;
         case WACHTEN:
             pidpositie(ANGLEMIN,motor1.getPosition()); //dit is nu alleen een waarde, moet vervolgens in een functie terugkomen toch
@@ -783,7 +785,7 @@
             pc.printf("new position %f\r\n", new_pos);
             //pc.printf("ifwachten2\n");
             break;
-    }
+    } //end switch
     scope.set(0,motor1.getPosition());        //ruwe data
     scope.set(2,motor1.getPosition());     //filtered
     scope.send();