Jordan Brack / SmartWheelsPublic

SmartWheels self-driving race car. Designed for NXP Cup. Uses FRDM-KL25Z, area-scan camera, and simple image processing to detect and navigate any NXP spec track.

Dependencies:   TSI USBDevice mbed-dev

Fork of SmartWheels by haofan Zheng

Diff: main.cpp

Revision:
63:d9a81b3d69f5
Parent:
62:bc5caf59fe39
Child:
64:43ab429a37e0
diff -r bc5caf59fe39 -r d9a81b3d69f5 main.cpp
--- a/main.cpp	Sun Apr 09 18:20:57 2017 +0000
+++ b/main.cpp	Sun Apr 09 22:08:34 2017 +0000
@@ -3,7 +3,7 @@
 #include <math.h>
 #define PI 3.14159265f
 
-#define SW_DEBUG
+//#define SW_DEBUG
 #include "SWCommon.h"
 #include "GlobalVariable.h"
 
@@ -54,6 +54,8 @@
     
     uint8_t IMUInitResult = imu_manager_init();
     LOGI("IMU Init: %#x", IMUInitResult);
+    imu_manager_calibrate();
+    imu_manager_begin_tick();
     //wait(0.5);
     
     //timer.reset();
@@ -145,11 +147,19 @@
         //LOGI("FPS: %f", 1 / deltaTime);
         
         //imu_manager_init();
-        imu_manager_update();
+        //imu_manager_update();
+        
+        //const volatile struct imu_vec3* AccelV = imu_manager_get_accl();
+        
+        //LOGI("A: %5.3f, %5.3f, %5.3f ", AccelV->x, AccelV->y, AccelV->z);
         
-        const volatile struct imu_vec3* AccelV = imu_manager_get_accl();
+        //const volatile struct imu_vec3* VelocityV = imu_manager_get_velocity();
+        
+        //LOGI("V: %5.3f, %5.3f, %5.3f ", VelocityV->x, VelocityV->y, VelocityV->z);
         
-        LOGI("A: %5.3f, %5.3f, %5.3f ", AccelV->x, AccelV->y, AccelV->z);
+        const volatile struct imu_vec3* PositionV = imu_manager_get_position();
+        
+        LOGI("P: %5.3f, %5.3f, %5.3f ", PositionV->x, PositionV->y, PositionV->z);
         
         counter.Update();