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:
73:1dcf56e9f1d4
Parent:
71:5601d2faa61d
Child:
74:2d45b954deb1
--- a/main.cpp	Fri Apr 14 18:58:28 2017 +0000
+++ b/main.cpp	Sun Apr 16 21:19:45 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;
     
@@ -122,15 +122,16 @@
         float offsetDegrees = ((srcX - centerPos) / centerPos) * SERVO_MAX_ANGLE;
         //////////////////////////////////////
         
+        const uint8_t lineFoundRefRow = ((2 * CAM_ROI_UPPER_LIMIT) - 1) - (CAM_ROI_UPPER_LIMIT / 2);
 
-        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))
+        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))
         {
             totalAngleDegrees = ((-0.1f) * totalAngleDegrees);// + ((0.2f) * lastAngle);
         }
-        LOGI("%d %5.3f ", centerLine[2 * CAM_ROI_UPPER_LIMIT - 1], totalAngleDegrees);
+        LOGI("%d %5.3f ", centerLine[lineFoundRefRow], totalAngleDegrees);
         servo_set_angle(totalAngleDegrees);
         
         
@@ -138,6 +139,8 @@
             totalAngleDegrees = SERVO_MAX_ANGLE;
         else if(totalAngleDegrees < -SERVO_MAX_ANGLE)
             totalAngleDegrees = -SERVO_MAX_ANGLE;
+            
+        float totalAngleDegreesAbs = abs(totalAngleDegrees);
         
         if(totalAngleDegreesAbs > lastAngleAbs)
         {
@@ -181,7 +184,7 @@
             motor_set_left_speed(speedRatio * cornerRatio * 1.0f);
         }
 
-        lastAngle = totalAngleDegrees;
+        //lastAngle = totalAngleDegrees;
         lastAngleAbs = totalAngleDegreesAbs;
     }
 }