Diff: main.cpp

Revision:

30:37e778f27fce

Parent:

29:7653adbbb101

Child:

31:85d3b4db5e2b

--- a/main.cpp	Fri Oct 16 10:35:24 2015 +0000
+++ b/main.cpp	Tue Oct 20 12:03:30 2015 +0000
@@ -155,14 +155,13 @@
     PowerServo.write(0.27);
     wait (1);
     PowerServo.write(0.04);
-    wait (1);
     Fired=Fired+1;
-    if (Fired >= 3) {
+    pc.printf("Fire = %i", Fired);
+    if (Fired == 3) {
         wait (1);
         Excecute = false;
         Home = true;
-        Fired = 0;
-    }
+     }
 }
 
 
@@ -188,8 +187,6 @@
         }
         while (Excecute ) {
             // Eerst wordt motor 1 aangestuurd
-            pc.printf("Rotatie = %f \n", Rotatie); 
-            pc.printf("Rotatie2 = %f \n", Rotatie2);
             if (Rotatie >= upperlimit) { //Als hij buiten bereik is
                 Goal = upperlimit - margin;
                 OutRange = true;
@@ -231,11 +228,11 @@
             if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit && OutRange2 == false) { // EMG aansturing
                 if (emg_value2 >= T2 ) {
                     Direction2 = 1;
-                    v2 = 0.05;
+                    v2 = 0.1;
                 }
                 if (emg_value2 > T1 && emg_value2 < T2) {
                     Direction2 = 0;
-                    v2 = 0.05;
+                    v2 = 0.1;
                 }
                 if (emg_value2 <= T1) {
                     Direction2 = 0;
@@ -248,10 +245,11 @@
         }
 
         while (Home) { //Terugkeren naar vaste positie
+            pc.printf("Home\n");
             OutRange = true; //Hiermee wordt het PID gedeelte van de motor control aangestuurd.
             Goal = 0;
             Goal2 = 0;
-            if (fabs(Error)<=0.0015 && fabs(Error2)<=0.0015) {
+            if (fabs(Error)<=0.015 && fabs(Error2)<=0.015) {
                 timer.start();
             } else {
                 timer.stop();
@@ -265,6 +263,7 @@
                 Errori2 = 0;
                 Errord2 = 0;
                 Errorp2 = 0;
+                Fired = 0;
             }
         }
     }