Diff: main.cpp

Revision:

48:5b1b90260239

Parent:

47:8cef7b0badea

Child:

49:7ae21c58ba92

--- a/main.cpp	Sun Nov 02 13:49:12 2014 +0000
+++ b/main.cpp	Sun Nov 02 14:13:22 2014 +0000
@@ -81,7 +81,7 @@
 
 /*bool stop;*/
 float pwm;
-float new_pos;
+float new_pwmpos;
 float new_pwm;
 float PWM2 = 0.3; //PWM voor instellen hoek batje
 int toestand = WACHTEN; //terugkeren?
@@ -440,6 +440,7 @@
             pc.printf("detachMotor1\n");
 
             pwm_motor1.write(0);
+            toestand = WACHTEN; //hierheen verplaatst vanaf motor1aansturingdeel2. Belangrijk!! niet weghalen!!
 
             myled1=1;
             myled2=1;
@@ -692,19 +693,12 @@
 
 void motor1aansturing()
 {
-    /*if (motor1.getPosition()>= ANGLEMIN && toestand != SLAAN) {
-        toestand = WACHTEN;
-        pc.printf("if1\n");
-    }--------------------------misschien komt hij niet op 0 pos met gebruik stok! dan zal hij niet gaan wachten en slaan!!, moet dus aan eind 2e ticker toestand wachten weer terugkrijgen!*/
     switch(toestand) {
         case SLAAN:
             pc.printf("SLAAN\n");
-            new_pwm = pid(setspeed, motor1.getSpeed(),false);
+            new_pwm = pid(setspeed, motor1.getSpeed());
             pwm_motor1.write(new_pwm);
             motordir1 = 1;
-            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;
                 pwm_motor1.write(0);//arvid had hier 0,0 gezet?!
@@ -737,6 +731,9 @@
                 }//end switch
             } //end if
             break;
+        case TERUGKEREN:
+            //positieregeling rond anglemax!!!
+            break;
     } //end switch
     scope.set(0,motor1.getPosition());
     scope.set(1,motor1.getPosition());
@@ -759,32 +756,22 @@
 void motor1aansturingdeel2()
 {
     switch(toestand) {
-        case TERUGKEREN:
-            if (motor1.getPosition()>= ANGLEMIN) {//moet arm hier niet naar beginpositie? als het te ver naar achteren is gegaan bijvoorbeeld
-                //pidpositie(ANGLEMIN, motor1.getPosition())
-                //?? motor1.getPosition(nieuwe positie);
-                toestand = WACHTEN;
-                //motor1.setPosition(0);
-                pid(0,0,true);
+        case TERUGKEREN: //deze case moet blijven ookal is het de enige case
+            if (motor1.getPosition()<=ANGLEMIN) {
+                new_pwmpos = pidpositie(ANGLEMIN, motor1.getPosition());//new_PWM benaming zorgt mogelijk voor problemen.
+                pwm_motor1.write(new_pwmpos); //mogelijk moet dit -new_pwm zijn???
+                motordir1 = 0;
+                //pc.printf("motor2.getPosition %d\r\n", motor2.getPosition());
+            }
+            if (motor1.getPosition()>= ANGLEMIN) {
+                new_pwmpos = pidpositie(ANGLEMIN, motor1.getPosition());
+                pwm_motor1.write(new_pwmpos); //mogelijk moet dit -new_pwm zijn???
+                motordir1 = 1;
                 //pc.printf("if2\n");
             }
-            if (motor1.getPosition()<=ANGLEMIN) {
-                pidpositie(ANGLEMIN, motor1.getPosition());//dit is nu alleen een waarde, moet vervolgens in een functie terugkomen toch
-                //?? motor1.getPosition(nieuwe positie);
-                pwm_motor1.write(0.3);
-                motordir1 = 0;
-        
-                pc.printf("motor2.getPosition %d\r\n", motor2.getPosition());
-            }
-            break;
-        case WACHTEN:
-            pwm_motor1.write(0);
-            //pidpositie(ANGLEMIN, motor1.getPosition());
-            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();
-}
\ No newline at end of file
+}//let op. Geen pidposition(0,0,true) deze moet zelf zorgen dat hij 0 wordt, en daar genoeg tijd voor hebben!!!
\ No newline at end of file