Diff: main.cpp

Revision:

52:078b521c9edf

Parent:

51:a8e58fd3e131

Child:

53:36d9335fec96

--- a/main.cpp	Thu Apr 06 18:37:16 2017 +0000
+++ b/main.cpp	Thu Apr 06 18:57:54 2017 +0000
@@ -42,7 +42,6 @@
     
     ardu_cam_init();
     
-    float tempCount = 0;
     //timer.reset();
     timer.start();
     float timeWas = timer.read();
@@ -50,33 +49,14 @@
     servo_set_angle(0.0f);
     while (1) 
     {
-        
-        //servo_set_angle(0.0f);
-        
         float deltaTime = timeWas;
         timeWas = timer.read();
         deltaTime = timeWas - deltaTime;
         
-        //led = 0;
         g_core.Update(deltaTime);
-        //motor.Update(deltaTime);
-        //servo.Update(deltaTime);
-        //wheelEncoder.Update(deltaTime);
-        //cam.Update(deltaTime);
-        
-        tempCount += deltaTime;
-        if(tempCount > 1.0f)
-        {
-            //led = !led;
-            tempCount = 0.0f;
-        }
-        
         
         if(!isRegRead && g_core.GetUSBServer().GetStatus() == SER_STAT_RUNNING && timer.read() > 2.5f && ardu_cam_is_capture_finished())
         {
-            //OV7725RegBuf * regBuf = new OV7725RegBuf(g_core);
-            //regBuf->ReadRegisters();
-            //delete regBuf;
             //ardu_cam_print_debug();
             isRegRead = true;
         }
@@ -95,17 +75,35 @@
         float disY = destY - srcY;
         float disX = destX - srcX;
         
-        float angleRadians = static_cast<float>(atan(static_cast<double>(disX / disY)));
+        float angleRadians = -1.0f * static_cast<float>(atan(static_cast<double>(disX / disY)));
         
         float angleDegrees = (angleRadians * (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;
+        float offsetDegrees = ((srcX - centerPos) / centerPos) * SERVO_MAX_ANGLE;
         //////////////////////////////////////
         
+        float totalAngleDegrees = (angleDegrees * 0.3f) + (offsetDegrees * 1.0f);
+        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)
+        {
+            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);    
+        }
+        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);
+        }
+        /*
         //////// Steer Vehicle / Adjust Speed for Differential //////////
         servo_set_angle((angleDegrees * -0.3f) + (offsetPercent * SERVO_MAX_ANGLE * 0.3f));
         
@@ -124,7 +122,7 @@
             motor_set_right_speed((0.5) + (0.5) * ((0.366 - abs(angleRadians))/0.366));
             motor_set_left_speed(1.0);
         }
-        
+        */
         
         
         //char buf[20];