Diff: main.cpp

Revision:

8:a0760acdc59e

Parent:

7:6f5cb6377bd4

Child:

9:1d8e4da058cd

--- a/main.cpp	Fri May 05 00:08:59 2017 +0000
+++ b/main.cpp	Fri May 05 01:21:33 2017 +0000
@@ -13,10 +13,12 @@
 #define I_CONSTANT 0.0000025
 #define D_CONSTANT 0.25
 
+#include "QEI.h"
+#define PULSES 880
 
 int main()
 {
-    enableMotors();
+    //enableMotors();
     //Set highest bandwidth.
     gyro.setLpBandwidth(LPFBW_42HZ);
     serial.baud(9600);
@@ -33,13 +35,21 @@
     greenLed.write(0);
     blueLed.write(1);
     
-    rightWheelForward(0.1);
-    leftWheelForward(0.1);
+    //rightWheelForward(0.1);
+    //leftWheelForward(0.1);
+
+    // PA_1 is A of right
+    // PA_0 is B of right
+    // PA_5 is A of left
+    // PB_3 is B of left
+    QEI encoder0( PA_5, PB_3, NC, PULSES, QEI::X4_ENCODING );
+    QEI encoder1( PA_1, PA_0, NC, PULSES, QEI::X4_ENCODING );
 
     while (1) {
-
-    wait(0.1);
-        serial.printf("%i, %i, %i\n", gyro.getGyroX(), gyro.getGyroY(), gyro.getGyroZ());
+        
+        wait(0.1);
+        //serial.printf("%i, %i, %i\n", gyro.getGyroX(), gyro.getGyroY(), gyro.getGyroZ());
+        serial.printf("Pulse Count=> e0:%d, e1:%d      \r\n", encoder0.getPulses(),encoder1.getPulses());
 
         //reading = Rec_4.read();
 //        serial.printf("reading: %f\n", reading);