Diff: main.cpp

Revision:

46:a5eb9bd3bb55

Parent:

45:501b7909139a

Child:

47:a682be9908b9

Child:

48:f76b5e252444

--- a/main.cpp	Thu Mar 30 03:50:23 2017 +0000
+++ b/main.cpp	Thu Mar 30 22:34:20 2017 +0000
@@ -1,6 +1,7 @@
 #include "mbed.h"
 
 #include <math.h>
+#define PI 3.14159265f
 
 #include "Motor.h"
 #include "Servo.h"
@@ -26,10 +27,11 @@
     
     //SW::Core core;
     
-    Motor motor(g_core);
+    //Motor motor(g_core);
     //Servo servo(g_core);
     //WheelEncoder wheelEncoder(core);
     //Camera cam(core);
+    motor_init();
     servo_init();
     
     bool isRegRead = false;
@@ -45,18 +47,21 @@
     timer.start();
     float timeWas = timer.read();
     
-    motor.setLeftSpeed(0.12f);
-    motor.setRightSpeed(0.12f);
+    motor_set_speeds(0.12f, 0.12f);
+    servo_set_angle(0.0f);
     while (1) 
     {
         
+        //motor_set_speeds(0.12f, 0.12f);
+        //servo_set_angle(0.0f);
+        
         float deltaTime = timeWas;
         timeWas = timer.read();
         deltaTime = timeWas - deltaTime;
         
         //led = 0;
         g_core.Update(deltaTime);
-        motor.Update(deltaTime);
+        //motor.Update(deltaTime);
         //servo.Update(deltaTime);
         //wheelEncoder.Update(deltaTime);
         //cam.Update(deltaTime);
@@ -79,22 +84,53 @@
         }
         
         //ardu_cam_display_img_utft();
+        
         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 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;
+        
+        float angle = static_cast<float>(atan(static_cast<double>(disX / disY)) * (180.0f / PI));
+        ///////////////////////////////////////
+        
+        /////////////Calculate the offset:
         float centerPos = static_cast<float>(RESOLUTION_WIDTH / 2);
         float offsetPercent = (static_cast<float>(centerLine[3]) - centerPos);
         offsetPercent = offsetPercent / centerPos;
-        servo_set_angle(offsetPercent * 1.7f * SERVO_MAX_ANGLE);
+        //////////////////////////////////////
+        
+        servo_set_angle((angle * -0.4f) + (offsetPercent * SERVO_MAX_ANGLE));
+        
+        //char buf[20];
+        //sprintf(buf, "angle %f", angle);
+        //g_core.GetUSBServer().PushUnreliableMsg('D', buf);
+        /*
+        std::string tempStr = "XX";
+        for(uint8_t i = 0; i < CAM_ROI_UPPER_LIMIT; ++i)
+        {
+            tempStr[0] = i;
+            tempStr[1] = centerLine[i];
+            g_core.GetUSBServer().PushUnreliableMsg('L', tempStr);
+        }
+        */
+        
+        /*
         if(offsetPercent > 0.1)
         {
-            motor.setLeftSpeed(0.05f);
-            motor.setRightSpeed(0.05f);
+            motor_set_speeds(0.05f, 0.05f);
         }
         else
         {
-            motor.setLeftSpeed(0.13f);
-            motor.setRightSpeed(0.13f);
+            motor_set_speeds(0.13f, 0.13f);
         }
-        
+        */
         //wait(0.01);
     }
 }