Diff: main.cpp

Revision:

54:f1f5648dfacf

Parent:

53:36d9335fec96

Child:

55:d4db9eef4a9d

--- a/main.cpp	Thu Apr 06 19:42:40 2017 +0000
+++ b/main.cpp	Thu Apr 06 20:36:51 2017 +0000
@@ -47,30 +47,33 @@
     float timeWas = timer.read();
     
     servo_set_angle(0.0f);
+    
+    float speedRatio = 1.0f;
+    
     while (1) 
     {
         float deltaTime = timeWas;
         timeWas = timer.read();
         deltaTime = timeWas - deltaTime;
         
-        g_core.Update(deltaTime);
+        //g_core.Update(deltaTime);
         
-        if(!isRegRead && g_core.GetUSBServer().GetStatus() == SER_STAT_RUNNING && timer.read() > 2.5f && ardu_cam_is_capture_finished())
-        {
+        //if(!isRegRead && g_core.GetUSBServer().GetStatus() == SER_STAT_RUNNING && timer.read() > 2.5f && ardu_cam_is_capture_finished())
+        //{
             //ardu_cam_print_debug();
-            isRegRead = true;
-        }
+        //    isRegRead = true;
+        //}
         
         //ardu_cam_display_img_utft();
         
-        volatile const uint8_t * centerLine = ardu_cam_get_center_array();
+        const uint8_t * centerLine = ardu_cam_get_center_array();
         
         /////////////Calculate the curvature:
-        float srcY = (0.0f + 1.0f) / 2.0f;
-        float srcX = static_cast<float>(centerLine[0] + centerLine[1]) / 2.0f;
+        float srcY = (0.0f) / 1.0f;
+        float srcX = static_cast<float>(centerLine[0]) / 1.0f;
         
-        float destY = static_cast<float>(CAM_ROI_UPPER_LIMIT - 1 + (CAM_ROI_UPPER_LIMIT - 2)) / 2.0f;
-        float destX = static_cast<float>(centerLine[CAM_ROI_UPPER_LIMIT - 1] + centerLine[CAM_ROI_UPPER_LIMIT - 2]) / 2.0f;
+        float destY = static_cast<float>(CAM_ROI_UPPER_LIMIT - 1) / 1.0f;
+        float destX = static_cast<float>(centerLine[CAM_ROI_UPPER_LIMIT - 1]) / 1.0f;
         
         float disY = destY - srcY;
         float disX = destX - srcX;
@@ -85,23 +88,42 @@
         float offsetDegrees = ((srcX - centerPos) / centerPos) * SERVO_MAX_ANGLE;
         //////////////////////////////////////
         
-        float totalAngleDegrees = (angleDegrees * 0.65f) + (offsetDegrees * 0.9f);
+        float totalAngleDegrees = (angleDegrees * 0.70f) + (offsetDegrees * 0.50f);
+        float totalAngleDegreesAbs = abs(totalAngleDegrees);
         servo_set_angle(totalAngleDegrees);
         
         if(totalAngleDegrees > SERVO_MAX_ANGLE)
             totalAngleDegrees = SERVO_MAX_ANGLE;
         else if(totalAngleDegrees < -SERVO_MAX_ANGLE)
             totalAngleDegrees = -SERVO_MAX_ANGLE;
-            
-        if(totalAngleDegrees < 0.0f)
+        
+        
+        if(totalAngleDegreesAbs > 17.0f)
         {
-            motor_set_left_speed((MOTOR_DIFF_MIN_SPEED) + (1.0f - MOTOR_DIFF_MIN_SPEED) * ((SERVO_MAX_ANGLE - abs(totalAngleDegrees)) / SERVO_MAX_ANGLE));
-            motor_set_right_speed(1.0f);    
+            speedRatio = 0.4f;
+        }
+        else if(totalAngleDegreesAbs > 12.0f)
+        {
+            speedRatio = 0.6f;
+        }
+        else if(totalAngleDegreesAbs > 7.0f)
+        {
+            speedRatio = 0.8f;
         }
         else
         {
-            motor_set_right_speed((MOTOR_DIFF_MIN_SPEED) + (1.0f - MOTOR_DIFF_MIN_SPEED) * ((SERVO_MAX_ANGLE - abs(totalAngleDegrees)) / SERVO_MAX_ANGLE));
-            motor_set_left_speed(1.0f);
+            speedRatio = 1.0f;
+        }
+        
+        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);    
+        }
+        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);
         }
         /*
         //////// Steer Vehicle / Adjust Speed for Differential //////////