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:
71:5601d2faa61d
Parent:
70:311d32a596db
Child:
72:b8f2eebc8912
Child:
73:1dcf56e9f1d4
--- a/main.cpp	Fri Apr 14 18:44:41 2017 +0000
+++ b/main.cpp	Fri Apr 14 18:58:28 2017 +0000
@@ -66,6 +66,7 @@
     
     float speedRatio = 1.0f;
     float lastAngle = 0.0f;
+    float lastAngleAbs = 0.0f;
     float cornerRatio = 1.0f;
     
     //DebugCounter counter(10, PTE5);
@@ -122,12 +123,12 @@
         //////////////////////////////////////
         
 
-        float totalAngleDegrees = (angleDegrees * 0.55f) + (offsetDegrees * (centerLine[2 * CAM_ROI_UPPER_LIMIT - 1] != BOTH_FOUND ? 0.90f : 0.35f));
+        float totalAngleDegrees = (angleDegrees * 0.52f) + (offsetDegrees * (centerLine[2 * CAM_ROI_UPPER_LIMIT - 1] != BOTH_FOUND ? 0.90f : 0.35f));
         float totalAngleDegreesAbs = abs(totalAngleDegrees);
         if((totalAngleDegrees < 0.0f && centerLine[2 * CAM_ROI_UPPER_LIMIT - 1] == LEFT_FOUND) ||
             (totalAngleDegrees > 0.0f && centerLine[2 * CAM_ROI_UPPER_LIMIT - 1] == RIGHT_FOUND))
         {
-            totalAngleDegrees = ((-1.0f) * totalAngleDegrees);
+            totalAngleDegrees = ((-0.1f) * totalAngleDegrees);// + ((0.2f) * lastAngle);
         }
         LOGI("%d %5.3f ", centerLine[2 * CAM_ROI_UPPER_LIMIT - 1], totalAngleDegrees);
         servo_set_angle(totalAngleDegrees);
@@ -138,11 +139,11 @@
         else if(totalAngleDegrees < -SERVO_MAX_ANGLE)
             totalAngleDegrees = -SERVO_MAX_ANGLE;
         
-        if(totalAngleDegrees > lastAngle)
+        if(totalAngleDegreesAbs > lastAngleAbs)
         {
-            cornerRatio =  0.6;//(lastAngle / totalAngleDegrees);
+            cornerRatio =  0.5;//(lastAngle / totalAngleDegrees);
         }
-        else if(totalAngleDegrees < lastAngle)
+        else if(totalAngleDegreesAbs < lastAngleAbs)
         {
             cornerRatio =  1.0;//(lastAngle / totalAngleDegrees);    
         }
@@ -181,6 +182,7 @@
         }
 
         lastAngle = totalAngleDegrees;
+        lastAngleAbs = totalAngleDegreesAbs;
     }
 }