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:
53:36d9335fec96
Parent:
52:078b521c9edf
Child:
54:f1f5648dfacf
diff -r 078b521c9edf -r 36d9335fec96 main.cpp
--- a/main.cpp	Thu Apr 06 18:57:54 2017 +0000
+++ b/main.cpp	Thu Apr 06 19:42:40 2017 +0000
@@ -66,11 +66,11 @@
         volatile const uint8_t * centerLine = ardu_cam_get_center_array();
         
         /////////////Calculate the curvature:
-        float srcY = (0.0f + 1.0f + 2.0f) / 3.0f;
-        float srcX = static_cast<float>(centerLine[0] + centerLine[1] + centerLine[2]) / 3.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 + (CAM_ROI_UPPER_LIMIT - 2) + (CAM_ROI_UPPER_LIMIT - 3)) / 3.0f;
-        float destX = static_cast<float>(centerLine[CAM_ROI_UPPER_LIMIT - 1] + centerLine[CAM_ROI_UPPER_LIMIT - 2] + centerLine[CAM_ROI_UPPER_LIMIT - 3]) / 3.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;
@@ -85,7 +85,7 @@
         float offsetDegrees = ((srcX - centerPos) / centerPos) * SERVO_MAX_ANGLE;
         //////////////////////////////////////
         
-        float totalAngleDegrees = (angleDegrees * 0.3f) + (offsetDegrees * 1.0f);
+        float totalAngleDegrees = (angleDegrees * 0.65f) + (offsetDegrees * 0.9f);
         servo_set_angle(totalAngleDegrees);
         
         if(totalAngleDegrees > SERVO_MAX_ANGLE)