Diff: main.cpp

Revision:

8:a2b725b502d8

Parent:

7:2042e359bfc3

Child:

9:4743f3bb39b2

--- a/main.cpp	Mon Sep 28 18:48:19 2015 +0000
+++ b/main.cpp	Mon Sep 28 19:56:53 2015 +0000
@@ -1,5 +1,6 @@
 #include "mbed.h"
 #include "QEI.h"
+#include "math.h"
 
 DigitalOut Direction(D4); //1 = CCW - 0 = CW
 PwmOut PowerMotor(D5); //van 0 tot 1
@@ -9,21 +10,48 @@
 Ticker Pot;
 
 double z=0;
-
+const double twopi = 6.2831853071795;
+const double pi = twopi/2;
+int Pulses;
+double Rotatie;
+double Rotatietwopi;
+double Goal = 0;
+double Error = 0;
+double K = 2;
+double v;
 
 void readpot()
 {
     z = PotMeter.read()/10;
 }
 
+
 int main()
 {
     pc.baud(115200);
     PowerMotor.write(0);
     Pot.attach(readpot,0.1); // Deze ticker moet de waarde uitlezen van de PotMeter 10 keer per seconde
+    //PC.attach(print,0.5);
     while (true) {
+        Pulses = Encoder.getPulses();
+        Rotatie = (Pulses*twopi)/4192;
+        Rotatietwopi = fmod(Rotatie,twopi);
         pc.printf ("Potmeter = %f\n", z); // het weergeven van de waarde waar z (PotMeter) op dat moment is
-        PowerMotor.write(z);
+        pc.printf ("Rotatie = %f [radialen] \n", Rotatietwopi);
+        if (z > 0.05) {
+            Goal = pi;
+        }
+        Error = Goal-Rotatietwopi;
+        if (Error >= 0) {
+            Direction =0;
+        } else {
+            Direction =1;
+        }
+        pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
+        v = K*fabs(Error); 
+        PowerMotor.write(v);
     }
 }
 
+
+