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:
58:996effac29b9
Parent:
57:0d8a155d511d
Child:
61:bdc31e4fa3c4
--- a/main.cpp	Sat Apr 08 15:58:52 2017 +0000
+++ b/main.cpp	Sat Apr 08 17:44:39 2017 +0000
@@ -57,6 +57,8 @@
     servo_set_angle(0.0f);
     
     float speedRatio = 1.0f;
+    float lastAngle = 0.0f;
+    float cornerRatio = 1.0f;
     
     DebugCounter counter(10, PTE5);
     
@@ -89,8 +91,8 @@
         float disX = destX - srcX;
         
         float angleRadians = -1.0f * static_cast<float>(atan(static_cast<double>(disX / disY)));
+        float angleDegrees = (angleRadians * (180.0f / PI));
         
-        float angleDegrees = (angleRadians * (180.0f / PI));
         ///////////////////////////////////////
         
         /////////////Calculate the offset:
@@ -98,7 +100,7 @@
         float offsetDegrees = ((srcX - centerPos) / centerPos) * SERVO_MAX_ANGLE;
         //////////////////////////////////////
         
-        float totalAngleDegrees = (angleDegrees * 0.50f) + (offsetDegrees * 0.35f);
+        float totalAngleDegrees = ((angleDegrees * 0.50f) + (offsetDegrees * 0.35f));
         float totalAngleDegreesAbs = abs(totalAngleDegrees);
         servo_set_angle(totalAngleDegrees);
         
@@ -107,18 +109,30 @@
         else if(totalAngleDegrees < -SERVO_MAX_ANGLE)
             totalAngleDegrees = -SERVO_MAX_ANGLE;
         
+        if(totalAngleDegrees > lastAngle)
+        {
+            cornerRatio = (lastAngle / totalAngleDegrees);
+        }
+        else if(totalAngleDegrees < lastAngle)
+        {
+            cornerRatio = (lastAngle / totalAngleDegrees);    
+        }
+        else
+        {
+            cornerRatio = 1.0f;    
+        }
         
-        if(totalAngleDegreesAbs > 19.0f)
+        if(totalAngleDegreesAbs > 18.0f)
         {
-            speedRatio = 0.82f;
+            speedRatio = 0.90f;
         }
         else if(totalAngleDegreesAbs > 15.0f)
         {
-            speedRatio = 0.87f;
+            speedRatio = 0.94f;
         }
-        else if(totalAngleDegreesAbs > 10.0f)
+        else if(totalAngleDegreesAbs > 12.0f)
         {
-            speedRatio = 0.95f;
+            speedRatio = 0.98f;
         }
         else
         {
@@ -127,15 +141,17 @@
         
         if(totalAngleDegrees < 0.0f)
         {
-            motor_set_left_speed(speedRatio * ((MOTOR_DIFF_MIN_SPEED) + ((1.0f - MOTOR_DIFF_MIN_SPEED) * ((SERVO_MAX_ANGLE - abs(totalAngleDegrees)) / SERVO_MAX_ANGLE))));
-            motor_set_right_speed(speedRatio * 1.0f);    
+            motor_set_left_speed(speedRatio * cornerRatio * ((MOTOR_DIFF_MIN_SPEED) + ((1.0f - MOTOR_DIFF_MIN_SPEED) * ((SERVO_MAX_ANGLE - abs(totalAngleDegrees)) / SERVO_MAX_ANGLE))));
+            motor_set_right_speed(speedRatio * cornerRatio * 1.0f);    
         }
         else
         {
-            motor_set_right_speed(speedRatio * ((MOTOR_DIFF_MIN_SPEED) + ((1.0f - MOTOR_DIFF_MIN_SPEED) * ((SERVO_MAX_ANGLE - abs(totalAngleDegrees)) / SERVO_MAX_ANGLE))));
-            motor_set_left_speed(speedRatio * 1.0f);
+            motor_set_right_speed(speedRatio * cornerRatio * ((MOTOR_DIFF_MIN_SPEED) + ((1.0f - MOTOR_DIFF_MIN_SPEED) * ((SERVO_MAX_ANGLE - abs(totalAngleDegrees)) / SERVO_MAX_ANGLE))));
+            motor_set_left_speed(speedRatio * cornerRatio * 1.0f);
         }
         
+        lastAngle = totalAngleDegrees;
+        
         //LOGI("FPS: %f", 1 / deltaTime);
         counter.Update();