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:
50:c387c88141fb
Parent:
48:f76b5e252444
Child:
51:a8e58fd3e131
diff -r f76b5e252444 -r c387c88141fb main.cpp
--- a/main.cpp	Wed Apr 05 21:35:47 2017 +0000
+++ b/main.cpp	Thu Apr 06 18:22:02 2017 +0000
@@ -46,13 +46,20 @@
     //timer.reset();
     timer.start();
     float timeWas = timer.read();
+    int count = 0;
     
     motor_set_speeds(0.12f, 0.12f);
+    float speed = 0.12;
     servo_set_angle(0.0f);
     while (1) 
     {
+        count = count + 1;
+        if(count % 10000 == 0)
+        {
+            speed = speed + 0.0001f;
+            motor_set_speeds(speed, speed);
+        }
         
-        //motor_set_speeds(0.12f, 0.12f);
         //servo_set_angle(0.0f);
         
         float deltaTime = timeWas;
@@ -88,11 +95,11 @@
         volatile 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 + 2.0f) / 3.0f;
+        float srcX = static_cast<float>(centerLine[0] + centerLine[1] + centerLine[2]) / 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 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 disY = destY - srcY;
         float disX = destX - srcX;
@@ -108,6 +115,8 @@
         
         servo_set_angle((angle * -0.4f) + (offsetPercent * SERVO_MAX_ANGLE));
         
+        
+        
         //char buf[20];
         //sprintf(buf, "angle %f", angle);
         //g_core.GetUSBServer().PushUnreliableMsg('D', buf);