Diff: main.cpp

Revision:

18:1e40778ad1aa

Parent:

17:71c5c9bfb7ba

Child:

19:0754c9563e01

--- a/main.cpp	Fri Oct 31 10:26:50 2014 +0000
+++ b/main.cpp	Fri Oct 31 11:09:37 2014 +0000
@@ -624,9 +624,9 @@
 void motor1aansturing()
 {
 
-    if (motor1.getPosition()>= ANGLEMAX && toestand != SLAAN) {
+    if (motor1.getPosition()>= ANGLEMIN && toestand != SLAAN) {
         toestand = WACHTEN;
-        motor1.setPosition(0);
+        //motor1.setPosition(0);
         pid(0,0,true); 
         pc.printf("if1\n");
     }
@@ -645,19 +645,12 @@
     }
     if (toestand == SLAAN && motor1.getPosition() <= ANGLEMAX) {
         toestand = TERUGKEREN;
-        motor1.setPosition(0);
+        //motor1.setPosition(0);
         pwm_motor1.write(0);
         pid(0,0,true); 
         stop = 1; //zorgt dat hij niet weer gaat slaan tot er gereset is...
         pc.printf("toestand = terugkeren\n\r");
     }
-    /*if (toestand == TERUGKEREN) {
-        new_pwm = pid(setspeed, motor1.getSpeed(),false); 
-        pwm_motor1.write(new_pwm);
-        motordir1 = 0;
-        pc.printf("motor gaat terugkeren\n\r");
-        pc.printf("new pwm %f\r\n",new_pwm);
-    }*/
     if (toestand == WACHTEN) {
         pwm_motor1.write(0);
         pc.printf("ifwachten\n");
@@ -668,7 +661,14 @@
         motordir1 = 1;
         pc.printf("SLAAN\n");
         
+    if (toestand == TERUGKEREN) {
+        new_pwm = pid(setspeed, motor1.getSpeed(),false); 
+        pwm_motor1.write(new_pwm);
+        motordir1 = 0;
+        pc.printf("motor gaat terugkeren\n\r");
+        pc.printf("new pwm %f\r\n",new_pwm);
     }
+    
 }
 
 void motor1aansturingdeel2()
@@ -679,26 +679,6 @@
         pid(0,0,true); 
         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;
-        pc.printf("slaan \n");
-        if ( snelheidsstand==3) {
-            setspeed = V3; pc.printf("Snel 3 \n");
-        }
-        if ( snelheidsstand==2) {
-            setspeed = V2; pc.printf("Snel 2\n");
-        }
-        if ( snelheidsstand==1) {
-            setspeed = V1; pc.printf("Snel 1 \n");
-        }
-    }*/
-    /*if (toestand == SLAAN && motor1.getPosition() <= ANGLEMAX) {
-        toestand = TERUGKEREN;
-        motor1.setPosition(0);
-        pid(0,0,true); 
-        stop = 1; //zorgt dat hij niet weer gaat slaan tot er gereset is...
-        pc.printf("toestand = terugkeren\n\r");
-    }*/
     if (toestand == TERUGKEREN) {
         new_pwm = pid(setspeed, motor1.getSpeed(),false); 
         pwm_motor1.write(new_pwm);
@@ -710,11 +690,4 @@
         pwm_motor1.write(0);
         pc.printf("ifwachten\n");
     }
-    /*if (toestand == SLAAN) {
-        new_pwm = pid(setspeed, motor1.getSpeed(),false);
-        pwm_motor1.write(new_pwm);
-        motordir1 = 1;
-        pc.printf("SLAAN\n");
-        
-    }*/  
 }
\ No newline at end of file