Diff: main.cpp

Revision:

16:b0ec8e6a8ad4

Parent:

15:f7e2b20f4dca

Child:

17:c5eea26e171d

--- a/main.cpp	Wed Oct 07 17:24:39 2015 +0000
+++ b/main.cpp	Wed Oct 07 18:17:58 2015 +0000
@@ -1,24 +1,24 @@
 #include "mbed.h"
-#include "encoder.h"
+#include "QEI.h"
 #include "math.h"
 #include "HIDScope.h"
 
 DigitalOut Direction(D4); //1 = CCW - 0 = CW
 PwmOut PowerMotor(D5); //van 0 tot 1
 PwmOut PowerServo(D7);
-DigitalIn Button(D3);
-DigitalIn Button2(D2);
+DigitalIn Button(PTC6);
+//DigitalIn Button2(D2);
 AnalogIn PotMeter(A1);
-Encoder MotorEncoder(D12,D13); //Encoder
+QEI Encoder(D13,D12,NC,32,QEI::X2_ENCODING); //Encoder
 Serial pc(USBTX, USBRX);
 Ticker MotorWrite;
 Ticker Sender;
+Timer timer;
 HIDScope scope(3);
 
 
 double z=0; //initiele waarde potmeter
 const double twopi = 6.2831853071795;
-const double pi = twopi/2;
 int Pulses;
 double Rotatie; //aantal graden dat de motor is gedraaid
 double Rotatieup=0; //aantal graden dat de motor is gedraaid in een bereik van n*pi
@@ -47,33 +47,37 @@
     scope.set(0,Goal);
     scope.set(1,Rotatieup);
     scope.set(2,Error);
-//  scope.set(2,v);
-    scope.send();   
-    }
+    scope.send();
+}
 int main()
 {
     upperlimit = n*twopi;
     pc.baud(115200);
     PowerMotor.write(0);
-    Sender.attach(Send,1/Fs/2);
+    Sender.attach(Send,0.5/Fs);
     MotorWrite.attach(MotorSet,1/Fs); // Deze ticker moet de waarde uitlezen van de PotMeter Fs keer per seconde
     while (true) {
         if (Button == 0) {
             Excecute =! Excecute;
-        }
+            }
         while (Excecute ) {
-            Pulses = MotorEncoder.getPosition();
+            Pulses = Encoder.getPulses();
             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 = PotMeter.read()*upperlimit; // 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 (Error>=-0.001 && Error<=0.001) {
+            if (fabs(Error)<=0.15) {
+                timer.start();
+            } else {
+                timer.stop();
+                timer.reset();
+            }
+            if (timer.read() >= 5) {
                 Excecute=false;
-                v=0;
+                Goal = 0;
             }
         }
     }
-}
\ No newline at end of file
+}