Diff: main.cpp

Revision:

17:c5eea26e171d

Parent:

16:b0ec8e6a8ad4

Child:

18:0f7f57228901

--- a/main.cpp	Wed Oct 07 18:17:58 2015 +0000
+++ b/main.cpp	Wed Oct 07 18:25:02 2015 +0000
@@ -28,6 +28,7 @@
 double v = 0; //snelheid van de motor (0-0.1
 double upperlimit; //max aantal rotaties
 bool Excecute = false;
+bool Excecute2 = false;
 double Fs=100;
 
 double n = 3;
@@ -57,9 +58,10 @@
     Sender.attach(Send,0.5/Fs);
     MotorWrite.attach(MotorSet,1/Fs); // Deze ticker moet de waarde uitlezen van de PotMeter Fs keer per seconde
     while (true) {
+        Encoder.reset();
         if (Button == 0) {
             Excecute =! Excecute;
-            }
+        }
         while (Excecute ) {
             Pulses = Encoder.getPulses();
             Rotatie = (Pulses*twopi)/4192; // Aantal graden draaien in radialen
@@ -76,8 +78,28 @@
             }
             if (timer.read() >= 5) {
                 Excecute=false;
-                Goal = 0;
+                Excecute2 = true;
+                Error = 0;
+            }
+        }
+        while (Excecute2) {
+            Pulses = Encoder.getPulses();
+            Rotatie = (Pulses*twopi)/4192; // Aantal graden draaien in radialen
+            Rotatieup = fmod(Rotatie,upperlimit); //Aantal graden draaien in radialen binnen het bereik van upperlimit
+            pc.printf ("Rotatie = %f [radialen] \n", Rotatieup);
+            Goal = 0; // Het doel waar hij naar toe moet
+            Error = Goal-Rotatieup; // De error die het motortje maakt ten opzichte van je Goal
+            pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
+            if (fabs(Error)<=0.15) {
+                timer.start();
+            } else {
+                timer.stop();
+                timer.reset();
+            }
+            if (timer.read() >= 5) {
+                Excecute2=false;
+                Error = 0;
             }
         }
     }
-}
+}
\ No newline at end of file