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:
76:5de2cbe0c271
Parent:
74:2d45b954deb1
Child:
77:1ee12f434a23
diff -r de107fac3bc9 -r 5de2cbe0c271 main.cpp
--- a/main.cpp	Sun Apr 16 21:36:23 2017 +0000
+++ b/main.cpp	Mon Apr 17 14:14:02 2017 +0000
@@ -65,7 +65,7 @@
     servo_set_angle(0.0f);
     
     float speedRatio = 1.0f;
-    //float lastAngle = 0.0f;
+    float lastAngle = 0.0f;
     float lastAngleAbs = 0.0f;
     float cornerRatio = 1.0f;
     
@@ -126,8 +126,8 @@
 
         float totalAngleDegrees = (angleDegrees * 0.52f) + (offsetDegrees * (centerLine[lineFoundRefRow] != BOTH_FOUND ? 0.90f : 0.35f));
         
-        if((totalAngleDegrees < 0.0f && centerLine[lineFoundRefRow] == LEFT_FOUND) ||
-            (totalAngleDegrees > 0.0f && centerLine[lineFoundRefRow] == RIGHT_FOUND))
+        if((totalAngleDegrees < 0.0f && lastAngle > 0.0f && centerLine[lineFoundRefRow] == LEFT_FOUND) ||
+            (totalAngleDegrees > 0.0f && lastAngle < 0.0f  && centerLine[lineFoundRefRow] == RIGHT_FOUND))
         {
             totalAngleDegrees = ((-0.1f) * totalAngleDegrees);// + ((0.2f) * lastAngle);
         }
@@ -188,7 +188,7 @@
             motor_set_left_speed(speedRatio * cornerRatio * 1.0f);
         }
 
-        //lastAngle = totalAngleDegrees;
+        lastAngle = totalAngleDegrees;
         lastAngleAbs = totalAngleDegreesAbs;
     }
 }