Diff: main.cpp

Revision:

18:0f7f57228901

Parent:

17:c5eea26e171d

Child:

19:b8d959e02e5d

--- a/main.cpp	Wed Oct 07 18:25:02 2015 +0000
+++ b/main.cpp	Wed Oct 07 20:06:15 2015 +0000
@@ -7,7 +7,7 @@
 PwmOut PowerMotor(D5); //van 0 tot 1
 PwmOut PowerServo(D7);
 DigitalIn Button(PTC6);
-//DigitalIn Button2(D2);
+DigitalIn Button2(PTC4);
 AnalogIn PotMeter(A1);
 QEI Encoder(D13,D12,NC,32,QEI::X2_ENCODING); //Encoder
 Serial pc(USBTX, USBRX);
@@ -24,19 +24,24 @@
 double Rotatieup=0; //aantal graden dat de motor is gedraaid in een bereik van n*pi
 double Goal = 0; //initele waarde goal waar de motor naar toe moet, dit wordt gedaan
 double Error = 0;
-double Kp = 0.2; //Moet berekend worden aan de hand van Control concept slides
+double Errord = 0;
+double Errori = 0;
+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;
 double v = 0; //snelheid van de motor (0-0.1
 double upperlimit; //max aantal rotaties
 bool Excecute = false;
 bool Excecute2 = false;
-double Fs=100;
+const double Fs=100;
 
 double n = 3;
 
 void MotorSet()
 {
-    v=Kp*Error;
-    if (v>=0) {
+    v=Kp*Error + Kd*Errord + Ki*Errori;
+    if (Error>=0) {
         Direction=1;
     } else {
         Direction=0;
@@ -69,8 +74,15 @@
             pc.printf ("Rotatie = %f [radialen] \n", Rotatieup);
             Goal = 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;
+            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.15) {
+            if (fabs(Error)<=0.0015) {
                 timer.start();
             } else {
                 timer.stop();
@@ -80,6 +92,8 @@
                 Excecute=false;
                 Excecute2 = true;
                 Error = 0;
+                Errori = 0;
+                Errord = 0;
             }
         }
         while (Excecute2) {
@@ -90,7 +104,7 @@
             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) {
+            if (fabs(Error)<=0.0015) {
                 timer.start();
             } else {
                 timer.stop();
@@ -99,6 +113,8 @@
             if (timer.read() >= 5) {
                 Excecute2=false;
                 Error = 0;
+                Errori = 0;
+                Errord = 0;
             }
         }
     }