Diff: main.cpp

Revision:

67:1b5c8add3d01

Parent:

61:bdc31e4fa3c4

Parent:

66:c9b64f5337cc

Child:

69:34e97694f0ef

diff -r bdc31e4fa3c4 -r 1b5c8add3d01 main.cpp
--- a/main.cpp	Thu Apr 13 17:52:26 2017 +0000
+++ b/main.cpp	Thu Apr 13 17:57:31 2017 +0000
@@ -15,6 +15,7 @@
 
 #include "ArduCAM.h"
 #include "ArduUTFT.h"
+#include "IMUManager.h"
 
 #include "PinAssignment.h"
 
@@ -50,6 +51,13 @@
     
     ardu_cam_init();
     
+    
+    //uint8_t IMUInitResult = imu_manager_init();
+    //LOGI("IMU Init: %#x", IMUInitResult);
+    //imu_manager_calibrate();
+    //imu_manager_begin_tick();
+    //wait(0.5);
+    
     //timer.reset();
     timer.start();
     float timeWas = timer.read();
@@ -78,7 +86,25 @@
         
         //ardu_cam_display_img_utft();
         image_processing();
-        const volatile uint8_t * centerLine = ardu_cam_get_center_array();
+        uint8_t shouldTerminate = ardu_cam_get_is_encounter_terminate();
+        
+        if(shouldTerminate)
+        {
+            ardu_utft_print("F", 230, 100);
+            servo_set_angle(0.0f);
+            motor_set_right_speed(-1.0f);
+            motor_set_left_speed(-1.0f);
+            wait(0.5f);
+            motor_set_right_speed(0.0f);
+            motor_set_left_speed(0.0f);
+            return 0;
+        }
+        //else
+        //{
+        //    ardu_utft_print(" ", 230, 100);
+        //}
+        
+        const volatile uint8_t* centerLine = ardu_cam_get_center_array();
         
         /////////////Calculate the curvature:
         float srcY = (0.0f + 1.0f) / 2.0f;
@@ -100,10 +126,23 @@
         float offsetDegrees = ((srcX - centerPos) / centerPos) * SERVO_MAX_ANGLE;
         //////////////////////////////////////
         
-        float totalAngleDegrees = ((angleDegrees * 0.50f) + (offsetDegrees * 0.35f));
+
+        float totalAngleDegrees = (angleDegrees * 0.50f) + (offsetDegrees * (centerLine[2 * CAM_ROI_UPPER_LIMIT - 1] != BOTH_FOUND ? 0.90f : 0.35f));
         float totalAngleDegreesAbs = abs(totalAngleDegrees);
+        if((totalAngleDegrees < 0 && centerLine[CAM_ROI_UPPER_LIMIT] == LEFT_FOUND) ||
+            (totalAngleDegrees > 0 && centerLine[CAM_ROI_UPPER_LIMIT] == RIGHT_FOUND))
+        {
+            totalAngleDegrees = totalAngleDegrees * -1.0f;
+        }
+        /*
+        if(totalAngleDegreesAbs >= 1.0f && totalAngleDegreesAbs <= 4.0)
+        {   
+            continue;
+        }
+        */
         servo_set_angle(totalAngleDegrees);
         
+        
         if(totalAngleDegrees > SERVO_MAX_ANGLE)
             totalAngleDegrees = SERVO_MAX_ANGLE;
         else if(totalAngleDegrees < -SERVO_MAX_ANGLE)
@@ -111,7 +150,7 @@
         
         if(totalAngleDegrees > lastAngle)
         {
-            cornerRatio =  cornerRatio * (LasstAngle / totalAngleDegrees);
+            cornerRatio =  cornerRatio * (lastAngle / totalAngleDegrees);
         }
         else if(totalAngleDegrees < lastAngle)
         {
@@ -139,6 +178,7 @@
             speedRatio = 1.0f;
         }
         
+   
         if(totalAngleDegrees < 0.0f)
         {
             motor_set_left_speed(speedRatio * cornerRatio * ((MOTOR_DIFF_MIN_SPEED) + ((1.0f - MOTOR_DIFF_MIN_SPEED) * ((SERVO_MAX_ANGLE - abs(totalAngleDegrees)) / SERVO_MAX_ANGLE))));
@@ -149,10 +189,28 @@
             motor_set_right_speed(speedRatio * cornerRatio * ((MOTOR_DIFF_MIN_SPEED) + ((1.0f - MOTOR_DIFF_MIN_SPEED) * ((SERVO_MAX_ANGLE - abs(totalAngleDegrees)) / SERVO_MAX_ANGLE))));
             motor_set_left_speed(speedRatio * cornerRatio * 1.0f);
         }
+
         
         lastAngle = totalAngleDegrees;
         
         //LOGI("FPS: %f", 1 / deltaTime);
+        
+        //imu_manager_init();
+        //imu_manager_update();
+        //float imuTemp = imu_manager_get_temp();
+        
+        //const volatile struct imu_vec3* AccelV = imu_manager_get_accl();
+        
+        //LOGI("A: %5.3f, %5.3f, %5.3f.T%5.2f ", AccelV->x, AccelV->y, AccelV->z, imuTemp);
+        
+        //const volatile struct imu_vec3* VelocityV = imu_manager_get_velocity();
+        
+        //LOGI("V: %5.3f, %5.3f, %5.3f.T%5.2f ", VelocityV->x, VelocityV->y, VelocityV->z, imuTemp);
+        
+        //const volatile struct imu_vec3* PositionV = imu_manager_get_position();
+        
+        //LOGI("P: %5.3f, %5.3f, %5.3f ", PositionV->x, PositionV->y, PositionV->z);
+        
         counter.Update();
         
         /*