Diff: main.cpp

Revision:

17:71c5c9bfb7ba

Parent:

16:8e56aa6f4b7a

Child:

18:1e40778ad1aa

--- a/main.cpp	Fri Oct 31 09:45:07 2014 +0000
+++ b/main.cpp	Fri Oct 31 10:26:50 2014 +0000
@@ -26,6 +26,7 @@
 float pid(float setspeed, float measurement, bool reset = false);
 void motor2aansturing();
 void motor1aansturing();
+void motor1aansturingdeel2();
 
 //alle initiaties voor EMG
 MODSERIAL pc(USBTX,USBRX);
@@ -381,9 +382,16 @@
                 motordir1 = 1;
                 stop = 0;
                 looptimer1.attach(motor1aansturing,TSAMP1);
-                wait(8); ////is aan te passen (tijd die nodig is om balletje te slaan
+                wait(1); ////is aan te passen (tijd die nodig is om balletje te slaan
                 looptimer1.detach();
                 pc.printf("detachMotor1\n");
+                
+                Ticker looptimer3;
+                looptimer3.attach(motor1aansturingdeel2,TSAMP1);
+                wait(1); ////is aan te passen (tijd die nodig is om balletje te slaan
+                looptimer3.detach();
+                pc.printf("detachMotor1\n");
+                
                 pwm_motor1.write(0);
                 
                 myled1=1;
@@ -638,10 +646,59 @@
     if (toestand == SLAAN && motor1.getPosition() <= ANGLEMAX) {
         toestand = TERUGKEREN;
         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");
+    }
+    if (toestand == SLAAN) {
+        new_pwm = pid(setspeed, motor1.getSpeed(),false);
+        pwm_motor1.write(new_pwm);
+        motordir1 = 1;
+        pc.printf("SLAAN\n");
+        
+    }
+}
+
+void motor1aansturingdeel2()
+{
+    if (motor1.getPosition()>= ANGLEMAX && toestand != SLAAN) {
+        toestand = WACHTEN;
+        motor1.setPosition(0);
+        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);
@@ -653,11 +710,11 @@
         pwm_motor1.write(0);
         pc.printf("ifwachten\n");
     }
-    if (toestand == SLAAN) {
+    /*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