Diff: main.cpp

Revision:

4:e171c9fa5447

Parent:

3:c9f9db6581bc

Child:

5:46dae55f0ab0

--- a/main.cpp	Mon Sep 28 09:22:19 2015 +0000
+++ b/main.cpp	Mon Sep 28 10:22:38 2015 +0000
@@ -1,4 +1,11 @@
 #include "mbed.h"
+#include "MODSERIAL.h"
+#include "QEI.h"
+
+
+Serial pc(USBTX,USBRX);
+
+Ticker          mod;
 
 //MOTOR
 // motor 1 en 2 zijn omgedraait
@@ -15,11 +22,18 @@
 // einde motor 1
 //EINDE MOTOR
 
+// ENcoDER
+QEI motor1_enc(D12,D11,NC,64);
+QEI motor2_enc(D10,D9,NC,64);
+
 
 //POTMETERS
 AnalogIn potleft(A1);
 AnalogIn potright(A0);
 
+DigitalIn button(PTA4)
+int button_on = 0
+
 void move_mot1(float left)
 {
     if(left < 0.4)
@@ -65,16 +79,29 @@
 }  
 
 
+void send()
+{
+    int countsl = motor1_enc.getPulses();
+    int countsr = motor2_enc.getPulses();
+    pc.printf("motor 1 counts %d motor 2 counts %d \n", countsl, countsr);
+}
+
 
 int main()
 {
-
+    pc.baud(115200);
+    mod.attach(&send,1);
     while(true)
     {
-    float left = potleft.read();
-    float right = potright.read();
-    move_mot1(left);
-    move_mot2(right); 
-    wait(0.01f);
+        float left = potleft.read();
+        float right = potright.read();
+        move_mot1(left);
+        move_mot2(right);
+        if(button.read() == button_on)
+        {
+            motor1_enc.reset;
+            motor2_enc.reset;
+        }
+    
     }
 }