Diff: main.cpp

Revision:

2:455216d1b5ba

Parent:

1:5d30a2ea2e11

Child:

3:68b364036bc8

--- a/main.cpp	Thu Oct 30 13:18:42 2014 +0000
+++ b/main.cpp	Thu Oct 30 14:05:36 2014 +0000
@@ -611,8 +611,9 @@
 void motor1aansturing()
 {
 
-    if (motor1.getPosition()<= ANGLEMIN && toestand != SLAAN) {
+    if (abs(motor1.getPosition())<= ANGLEMIN && toestand != SLAAN) {
         toestand = WACHTEN; 
+        pc.printf("if1\n");
     }
     if (snelheidsstand != 0 && toestand == WACHTEN && stop == 0) { // stop = 0 in het begin let op dat dit na reset nog zo is
         toestand = SLAAN;
@@ -634,16 +635,20 @@
     }
     if (toestand == TERUGKEREN) {
         pc.printf("motor gaat terugkeren\n\r");
-        new_pwm = pid(Vreturn, motor1.getSpeed());
-        pwm_motor1.write(new_pwm);
+        //pid(Vreturn, motor1.getSpeed());
+        pwm_motor1.write(0.5);
+        pc.printf("new pwm %f\r\n",new_pwm);
         motordir1 = 0;
     }
     if (toestand == WACHTEN) {
         pwm_motor1.write(0);
+        pc.printf("ifwachten\n");
     }
     if (toestand == SLAAN) {
         new_pwm = pid(setspeed, motor1.getSpeed());
         motordir1 = 1;
         pwm_motor1.write(new_pwm);
+        pc.printf("SLAAN\n");
+        
     }
 }
\ No newline at end of file