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

Branch:
Drift
Revision:
79:bdbac82c979b
Parent:
78:9f20bf037db6
Child:
80:c85cb93713b3
--- a/main.cpp	Mon Apr 17 16:02:48 2017 +0000
+++ b/main.cpp	Tue Apr 18 17:43:12 2017 +0000
@@ -16,6 +16,7 @@
 #include "ArduCAM.h"
 #include "ArduUTFT.h"
 #include "IMUManager.h"
+#include "ArduTouch.h"
 
 #include "PinAssignment.h"
 
@@ -51,6 +52,8 @@
     
     ardu_cam_init();
     
+    ardu_touch_init();
+    
     
     //uint8_t IMUInitResult = imu_manager_init();
     //LOGI("IMU Init: %#x", IMUInitResult);
@@ -124,6 +127,9 @@
         //////////////////////////////////////
         
         const uint8_t lineFoundRefRow = ((2 * CAM_ROI_UPPER_LIMIT) - 1) - (CAM_ROI_UPPER_LIMIT / 2);
+        //const uint8_t lineFoundRefTop = ((2 * CAM_ROI_UPPER_LIMIT) - 1);
+        //const uint8_t lineFoundRefBut = (CAM_ROI_UPPER_LIMIT);
+        const uint8_t isBorderFound = ardu_cam_get_is_border_found();
 
         float totalAngleDegrees = (angleDegrees * 0.52f) + (offsetDegrees * (centerLine[lineFoundRefRow] != BOTH_FOUND ? 0.90f : 0.35f));
         
@@ -132,10 +138,10 @@
         else if(totalAngleDegrees < -SERVO_MAX_ANGLE)
             totalAngleDegrees = -SERVO_MAX_ANGLE;
         
-        if((totalAngleDegrees < 0.0f && lastAngle > 18.0f && lastLineFound != LEFT_FOUND && centerLine[lineFoundRefRow] == LEFT_FOUND) ||
-            (totalAngleDegrees > 0.0f && lastAngle < -18.0f && lastLineFound != RIGHT_FOUND  && centerLine[lineFoundRefRow] == RIGHT_FOUND))
+        if((totalAngleDegrees < 0.0f && (isBorderFound == LEFT_FOUND)) ||
+        (totalAngleDegrees > 0.0f && (isBorderFound == RIGHT_FOUND)))
         {
-            totalAngleDegrees = ((-0.1f) * totalAngleDegrees);// + ((0.2f) * lastAngle);
+            totalAngleDegrees = ((-1.0f) * totalAngleDegrees);// + ((0.2f) * lastAngle);
         }
         LOGI("%d %5.3f ", centerLine[lineFoundRefRow], totalAngleDegrees);
         servo_set_angle(totalAngleDegrees);
@@ -158,19 +164,19 @@
         
         if(totalAngleDegreesAbs > 20.0f)
         {
-            speedRatio = 0.40f;
+            speedRatio = 0.350f;
         }
         else if(totalAngleDegreesAbs > 18.0f)
         {
-            speedRatio = 0.87f;
+            speedRatio = 0.80f;
         }
         else if(totalAngleDegreesAbs > 15.0f)
         {
-            speedRatio = 0.91f;
+            speedRatio = 0.90f;
         }
         else if(totalAngleDegreesAbs > 12.0f)
         {
-            speedRatio = 0.95f;
+            speedRatio = 0.92f;
         }
         else
         {