Diff: main.cpp

Revision:

10:e210675cbe71

Parent:

9:4743f3bb39b2

Child:

11:a9a23042fc9e

--- a/main.cpp	Mon Sep 28 20:13:03 2015 +0000
+++ b/main.cpp	Mon Sep 28 21:01:31 2015 +0000
@@ -9,92 +9,54 @@
 Serial pc(USBTX, USBRX);
 Ticker Pot;
 
-double z=0;
+double z=0; //initiele waarde potmeter
 const double twopi = 6.2831853071795;
 const double pi = twopi/2;
 int Pulses;
-double Rotatie;
-double Rotatietwopi;
-double Goal = 0;
+double Rotatie; //aantal graden dat de motor is gedraaid
+double Rotatieup; //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 K = 2;
-double v;
+double Errorv = 0;
+double Kp = 2; //Moet berekend worden aan de hand van Control concept slides
+double Kd = 0.1; 
+double v = 0; //snelheid van de motor (0-0.1
+double upperlimit; //max aantal rotaties
+
+
+double n = 2;
 
 void readpot()
 {
-    z = PotMeter.read()/10;
+    z = PotMeter.read(); //uitlezen Potmeter of voor EMG bijvoorbeeld
 }
 
 
 int main()
 {
+    upperlimit = n*twopi; 
     pc.baud(115200);
     PowerMotor.write(0);
     Pot.attach(readpot,0.1); // Deze ticker moet de waarde uitlezen van de PotMeter 10 keer per seconde
     while (true) {
         Pulses = Encoder.getPulses();
-        Rotatie = (Pulses*twopi)/4192;
-        Rotatietwopi = fmod(Rotatie,twopi);
-        
-        pc.printf ("Potmeter = %f\n", z);return 0; pc.printf ("Rotatie = %f [radialen] \n", Rotatietwopi);
-//        if (z > 0.05) {
-//            Goal = pi;
-//        }
-        Error = Goal-Rotatietwopi;
-        if (Error >= 0) {
+        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 ("Potmeter = %f\n", z); 
+        pc.printf ("Rotatie = %f [radialen] \n", Rotatieup);
+        Goal = z*upperlimit; // Het doel waar hij naar toe moet
+        Error = Goal-Rotatieup; // De error die het motortje maakt ten opzichte van je Goal
+        Errorv = 0-v;
+        if (Error >= 0) { //Bepalen van de rotatie richting
             Direction =0;
         } else {
-                        Direction =1;
+            Direction =1;
         }
         pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
-        v = K*fabs(Error); 
-        PowerMotor.write(v);
+        v = Kp*fabs(Error)+Kd*fabs(Errorv); // Snelheid van motor 
+        PowerMotor.write(v); // Snelheid die naar de motor geschreven wordt (wordt alleen in een schaal van 0-1 weergegeven). (uit ervaring van Motor4_Setpoint loopt dit maar tussen 0-0.1).
     }
 }
 
 
 
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-