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:
77:1ee12f434a23
Parent:
76:5de2cbe0c271
Child:
78:9f20bf037db6
diff -r 5de2cbe0c271 -r 1ee12f434a23 main.cpp
--- a/main.cpp	Mon Apr 17 14:14:02 2017 +0000
+++ b/main.cpp	Mon Apr 17 15:43:05 2017 +0000
@@ -68,6 +68,7 @@
     float lastAngle = 0.0f;
     float lastAngleAbs = 0.0f;
     float cornerRatio = 1.0f;
+    uint8_t lastLineFound = BOTH_FOUND;
     
     //DebugCounter counter(10, PTE5);
     
@@ -126,8 +127,13 @@
 
         float totalAngleDegrees = (angleDegrees * 0.52f) + (offsetDegrees * (centerLine[lineFoundRefRow] != BOTH_FOUND ? 0.90f : 0.35f));
         
-        if((totalAngleDegrees < 0.0f && lastAngle > 0.0f && centerLine[lineFoundRefRow] == LEFT_FOUND) ||
-            (totalAngleDegrees > 0.0f && lastAngle < 0.0f  && centerLine[lineFoundRefRow] == RIGHT_FOUND))
+        if(totalAngleDegrees > SERVO_MAX_ANGLE)
+            totalAngleDegrees = SERVO_MAX_ANGLE;
+        else if(totalAngleDegrees < -SERVO_MAX_ANGLE)
+            totalAngleDegrees = -SERVO_MAX_ANGLE;
+        
+        if((totalAngleDegrees < 0.0f && lastAngle > 17.0f && lastLineFound != BOTH_FOUND && centerLine[lineFoundRefRow] == LEFT_FOUND) ||
+            (totalAngleDegrees > 0.0f && lastAngle < -17.0f && lastLineFound != BOTH_FOUND  && centerLine[lineFoundRefRow] == RIGHT_FOUND))
         {
             totalAngleDegrees = ((-0.1f) * totalAngleDegrees);// + ((0.2f) * lastAngle);
         }
@@ -135,11 +141,6 @@
         servo_set_angle(totalAngleDegrees);
         
         
-        if(totalAngleDegrees > SERVO_MAX_ANGLE)
-            totalAngleDegrees = SERVO_MAX_ANGLE;
-        else if(totalAngleDegrees < -SERVO_MAX_ANGLE)
-            totalAngleDegrees = -SERVO_MAX_ANGLE;
-            
         float totalAngleDegreesAbs = abs(totalAngleDegrees);
         
         if(totalAngleDegreesAbs > lastAngleAbs)
@@ -157,7 +158,7 @@
         
         if(totalAngleDegreesAbs > 20.0f)
         {
-            speedRatio = 0.50f;
+            speedRatio = 0.40f;
         }
         else if(totalAngleDegreesAbs > 18.0f)
         {
@@ -190,6 +191,7 @@
 
         lastAngle = totalAngleDegrees;
         lastAngleAbs = totalAngleDegreesAbs;
+        lastLineFound = centerLine[lineFoundRefRow];
     }
 }