Diff: main.cpp

Revision:

19:b8d959e02e5d

Parent:

18:0f7f57228901

Child:

20:473735947e52

--- a/main.cpp	Wed Oct 07 20:06:15 2015 +0000
+++ b/main.cpp	Wed Oct 07 20:22:21 2015 +0000
@@ -29,7 +29,7 @@
 double Errorp = 0;
 const double Kp = 0.2; //Moet berekend worden aan de hand van Control concept slides
 const double Kd = 10;
-const double Ki = 0.1;
+const double Ki = 0.2;
 double v = 0; //snelheid van de motor (0-0.1
 double upperlimit; //max aantal rotaties
 bool Excecute = false;
@@ -72,7 +72,7 @@
             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 = PotMeter.read()*upperlimit; // Het doel waar hij naar toe moet
+            Goal = 0.5*twopi; //PotMeter.read()*upperlimit; // Het doel waar hij naar toe moet
             Error = Goal-Rotatieup; // De error die het motortje maakt ten opzichte van je Goal
             Errord = (Error-Errorp)/Fs;
             Errorp = Error;
@@ -103,6 +103,13 @@
             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
+            Errord = (Error-Errorp)/Fs;
+            Errorp = Error;
+            if (fabs(Error) <= 0.5) {
+                Errori = Errori + Error*1/Fs;
+            } else {
+                Errori = 0;
+            }
             pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
             if (fabs(Error)<=0.0015) {
                 timer.start();
@@ -111,7 +118,8 @@
                 timer.reset();
             }
             if (timer.read() >= 5) {
-                Excecute2=false;
+                Excecute=false;
+                Excecute2 = false;
                 Error = 0;
                 Errori = 0;
                 Errord = 0;