Diff: main.cpp

Revision:

57:0d8a155d511d

Parent:

56:7d3395ae022d

Child:

58:996effac29b9

Child:

62:bc5caf59fe39

--- a/main.cpp	Thu Apr 06 22:19:59 2017 +0000
+++ b/main.cpp	Sat Apr 08 15:58:52 2017 +0000
@@ -3,7 +3,7 @@
 #include <math.h>
 #define PI 3.14159265f
 
-#define SW_DEBUG
+//#define SW_DEBUG
 #include "SWCommon.h"
 #include "GlobalVariable.h"
 
@@ -58,6 +58,8 @@
     
     float speedRatio = 1.0f;
     
+    DebugCounter counter(10, PTE5);
+    
     while (1) 
     {
         float deltaTime = timeWas;
@@ -73,15 +75,15 @@
         //}
         
         //ardu_cam_display_img_utft();
-        
+        image_processing();
         const volatile uint8_t * centerLine = ardu_cam_get_center_array();
         
         /////////////Calculate the curvature:
-        float srcY = (0.0f) / 1.0f;
-        float srcX = static_cast<float>(centerLine[0]) / 1.0f;
+        float srcY = (0.0f + 1.0f) / 2.0f;
+        float srcX = static_cast<float>(centerLine[0] + centerLine[1]) / 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 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 disY = destY - srcY;
         float disX = destX - srcX;
@@ -96,7 +98,7 @@
         float offsetDegrees = ((srcX - centerPos) / centerPos) * SERVO_MAX_ANGLE;
         //////////////////////////////////////
         
-        float totalAngleDegrees = (angleDegrees * 0.70f) + (offsetDegrees * 0.50f);
+        float totalAngleDegrees = (angleDegrees * 0.50f) + (offsetDegrees * 0.35f);
         float totalAngleDegreesAbs = abs(totalAngleDegrees);
         servo_set_angle(totalAngleDegrees);
         
@@ -106,17 +108,17 @@
             totalAngleDegrees = -SERVO_MAX_ANGLE;
         
         
-        if(totalAngleDegreesAbs > 17.0f)
+        if(totalAngleDegreesAbs > 19.0f)
         {
-            speedRatio = 0.4f;
+            speedRatio = 0.82f;
         }
-        else if(totalAngleDegreesAbs > 12.0f)
+        else if(totalAngleDegreesAbs > 15.0f)
         {
-            speedRatio = 0.6f;
+            speedRatio = 0.87f;
         }
-        else if(totalAngleDegreesAbs > 7.0f)
+        else if(totalAngleDegreesAbs > 10.0f)
         {
-            speedRatio = 0.8f;
+            speedRatio = 0.95f;
         }
         else
         {
@@ -134,7 +136,9 @@
             motor_set_left_speed(speedRatio * 1.0f);
         }
         
-        LOGI("FPS: %f", 1 / deltaTime);
+        //LOGI("FPS: %f", 1 / deltaTime);
+        counter.Update();
+        
         /*
         //////// Steer Vehicle / Adjust Speed for Differential //////////
         servo_set_angle((angleDegrees * -0.3f) + (offsetPercent * SERVO_MAX_ANGLE * 0.3f));