Diff: main.cpp

Revision:

7:6f5cb6377bd4

Parent:

6:3d68fedd6fd9

Child:

8:a0760acdc59e

--- a/main.cpp	Thu May 04 21:38:20 2017 +0000
+++ b/main.cpp	Fri May 05 00:08:59 2017 +0000
@@ -6,6 +6,7 @@
 
 #include <stdlib.h>
 #include "ITG3200.h"
+#include "QEI.h"
 
 // PID
 #define P_CONSTANT 0.0025
@@ -15,12 +16,14 @@
 
 int main()
 {
+    enableMotors();
     //Set highest bandwidth.
     gyro.setLpBandwidth(LPFBW_42HZ);
     serial.baud(9600);
-       
+    serial.printf("The gyro's address is %s", gyro.getWhoAmI());
+
     wait (0.1);
-    
+
 //    IR_1.write(1);
 //    IR_2.write(1);
 //    IR_3.write(1);
@@ -30,11 +33,14 @@
     greenLed.write(0);
     blueLed.write(1);
     
+    rightWheelForward(0.1);
+    leftWheelForward(0.1);
+
     while (1) {
-        
-        wait(0.1);
+
+    wait(0.1);
         serial.printf("%i, %i, %i\n", gyro.getGyroX(), gyro.getGyroY(), gyro.getGyroZ());
-            
+
         //reading = Rec_4.read();
 //        serial.printf("reading: %f\n", reading);
 //        redLed.write(0);