Diff: main.cpp

Revision:

5:d47e6a96256b

Parent:

4:de94e1135968

Child:

6:cf20f04dbab4

--- a/main.cpp	Mon Sep 28 10:17:30 2015 +0000
+++ b/main.cpp	Mon Sep 28 18:39:43 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
@@ -7,21 +8,38 @@
 QEI Encoder(D13,D12,NC,32,QEI::X2_ENCODING); //Encoder
 Serial pc(USBTX, USBRX);
 Ticker Pot;
+Ticker PC;
 
 double z;
+const double twopi = 6.2831853071795;
+const double pi = pi/2;
+int Pulses;
+double Rotatie;
+double Rotatietwopi;
 
-void readpot ()
+void readpot()
 {
-    z = PotMeter.read();
+    z = PotMeter.read()/10;
+}
+
+void print()
+{
+    pc.printf ("Potmeter = %f\n", z); // het weergeven van de waarde waar z (PotMeter) op dat moment is
+    pc.printf ("Rotatie = %f [pi radialen] \n", Rotatietwopi);
 }
 
 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
+    Pot.attach(readpot,0.001); // Deze ticker moet de waarde uitlezen van de PotMeter 10 keer per seconde
+    PC.attach(print,0.5);
     while (true) {
-        pc.printf ("Potmeter = %f\n", z); // het weergeven van de waarde waar z (PotMeter) op dat moment is
+        PowerMotor.write(z);
+        Pulses = Encoder.getPulses();
+        Rotatie = (Pulses*twopi)/4192;
+        Rotatietwopi = fmod(Rotatie,twopi);
     }
 }
 
+