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:
72:b8f2eebc8912
Parent:
71:5601d2faa61d
--- a/main.cpp	Fri Apr 14 18:58:28 2017 +0000
+++ b/main.cpp	Sun Apr 16 19:50:50 2017 +0000
@@ -65,7 +65,6 @@
     servo_set_angle(0.0f);
     
     float speedRatio = 1.0f;
-    float lastAngle = 0.0f;
     float lastAngleAbs = 0.0f;
     float cornerRatio = 1.0f;
     
@@ -131,6 +130,7 @@
             totalAngleDegrees = ((-0.1f) * totalAngleDegrees);// + ((0.2f) * lastAngle);
         }
         LOGI("%d %5.3f ", centerLine[2 * CAM_ROI_UPPER_LIMIT - 1], totalAngleDegrees);
+        
         servo_set_angle(totalAngleDegrees);
         
         
@@ -141,7 +141,7 @@
         
         if(totalAngleDegreesAbs > lastAngleAbs)
         {
-            cornerRatio =  0.5;//(lastAngle / totalAngleDegrees);
+            cornerRatio =  0.3;//(lastAngleAbs / totalAngleDegreesAbs);
         }
         else if(totalAngleDegreesAbs < lastAngleAbs)
         {
@@ -152,36 +152,44 @@
             cornerRatio = 1.0f;    
         }
         
-        if(totalAngleDegreesAbs > 18.0f)
+        if(totalAngleDegreesAbs > 20.0f)
         {
-            speedRatio = 0.90f;
+            speedRatio = 0.70f;
+        }
+        else if(totalAngleDegreesAbs > 18.0f)
+        {
+            speedRatio = 0.80f;
         }
         else if(totalAngleDegreesAbs > 15.0f)
         {
-            speedRatio = 0.94f;
+            speedRatio = 0.85f;
         }
         else if(totalAngleDegreesAbs > 12.0f)
         {
-            speedRatio = 0.98f;
+            speedRatio = 0.90f;
         }
         else
         {
             speedRatio = 1.0f;
         }
         
+        float diffRatio = ((MOTOR_DIFF_MIN_SPEED) + ((1.0f - MOTOR_DIFF_MIN_SPEED) * ((SERVO_MAX_ANGLE - abs(totalAngleDegrees)) / SERVO_MAX_ANGLE)));
+        if(totalAngleDegreesAbs > 20.0f)
+        {
+            diffRatio = -0.9f * diffRatio;
+        }
    
         if(totalAngleDegrees < 0.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_left_speed(speedRatio * cornerRatio * diffRatio);
             motor_set_right_speed(speedRatio * cornerRatio * 1.0f);    
         }
         else
         {
-            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_right_speed(speedRatio * cornerRatio * diffRatio);
             motor_set_left_speed(speedRatio * cornerRatio * 1.0f);
         }
-
-        lastAngle = totalAngleDegrees;
+        
         lastAngleAbs = totalAngleDegreesAbs;
     }
 }