Revision 100:ffbeefc9e218, committed 2017-04-20

Comitter:

hazheng

Date:

Thu Apr 20 21:04:10 2017 +0000

Parent:

99:c6665262fd3d

Commit message:

Better version of Intersection detection.

Changed in this revision

diff -r c6665262fd3d -r ffbeefc9e218 Hardwares/ArduCAM.cpp
--- a/Hardwares/ArduCAM.cpp	Thu Apr 20 16:23:19 2017 +0000
+++ b/Hardwares/ArduCAM.cpp	Thu Apr 20 21:04:10 2017 +0000
@@ -1,6 +1,6 @@
 #include "ArduCAM.h"
 
-//#define SW_DEBUG
+#define SW_DEBUG
 #include "GlobalVariable.h"
 #include "SWCommon.h"
 
@@ -25,16 +25,18 @@
 
 const static uint8_t CAM_BLK_CAL_LEFT  = ((RESOLUTION_WIDTH / 2) - 1);
 const static uint8_t CAM_BLK_CAL_RIGHT = ((RESOLUTION_WIDTH / 2) + 1);
-const static uint8_t TERMINATE_WIDTH = static_cast<uint8_t>(RESOLUTION_WIDTH * 0.28f);
+const static uint8_t TERMINATE_WIDTH = static_cast<uint8_t>(RESOLUTION_WIDTH * 0.29f);
 
 static DigitalOut cam_cs(PIN_ACC_CS, 1);
 
 static uint8_t temp_mid_pos = RESOLUTION_WIDTH / 2;
 static uint8_t black_calibrate = 70;
-static volatile uint8_t centerLine[CAM_ROI_UPPER_LIMIT * 2];
-static volatile uint8_t is_encounter_terminate = 0;
-//static volatile uint8_t is_border_find = BOTH_NOT_FOUND;
-static volatile uint8_t is_intersection_detected = 0;
+static uint8_t centerLine[CAM_ROI_UPPER_LIMIT * 2];
+static uint8_t leftLine[CAM_ROI_UPPER_LIMIT];
+static uint8_t rightLine[CAM_ROI_UPPER_LIMIT];
+static const uint8_t lineArrayEnd = CAM_ROI_UPPER_LIMIT - 1;
+static uint8_t is_encounter_terminate = 0;
+static uint8_t intersection_info = INTERSECTION_NULL;
 
 void cal_black_calibrate();
 
@@ -220,14 +222,21 @@
 
 }
 
-volatile const uint8_t* ardu_cam_get_center_array()
+ const uint8_t* ardu_cam_get_center_array()
 {
     return centerLine;
 }
 
 void image_processing()
 {
+    static uint8_t leftPos = 0;
+    static uint8_t rightPos = 0;
+    static uint8_t isBorderFound = BOTH_NOT_FOUND;
     
+    static uint8_t leftNearCenter1 = lineArrayEnd;
+    static uint8_t leftNearPos = 0;
+    static uint8_t rightNearCenter1 = leftNearCenter1;
+    static uint8_t rightNearPos = RESOLUTION_WIDTH;
     //while(true)
     {
         ardu_cam_start_capture();
@@ -240,7 +249,8 @@
         static float calTemp = 0.0f;
         calTemp = 0.0f;
         static uint16_t greenPixel = 0x00;
-        uint8_t isValid = 1;
+        static uint8_t isValid = 1;
+        isValid = 1;
         for (uint8_t j = 0; j < RESOLUTION_WIDTH; ++j)
         {
             if(CAM_BLK_CAL_LEFT < j && j <= CAM_BLK_CAL_RIGHT)
@@ -268,15 +278,16 @@
         }
 #endif
         //cal_black_calibrate();
-        uint8_t leftPos = 0;
-        uint8_t rightPos = 0;
-        uint8_t isBorderFound = BOTH_NOT_FOUND;
+        intersection_info = INTERSECTION_NULL;
         
-        is_intersection_detected = 0;
-        uint8_t leftNearCenter1 = 0;
-        uint8_t leftNearCenter2 = 0;
-        uint8_t rightNearCenter1 = 0;
-        uint8_t rightNearCenter2 = 0;
+        leftPos = 0;
+        rightPos = 0;
+        isBorderFound = BOTH_NOT_FOUND;
+        
+        leftNearCenter1 = lineArrayEnd;
+        leftNearPos = 0;
+        rightNearCenter1 = leftNearCenter1;
+        rightNearPos = RESOLUTION_WIDTH;
         
         is_encounter_terminate = 0;
         for (uint8_t i = 0; i < CAM_ROI_UPPER_LIMIT; ++i)
@@ -286,7 +297,8 @@
 #endif
             get_img_row_info(i, &leftPos, &rightPos, &isBorderFound);
             temp_mid_pos = (leftPos + rightPos) / 2;
-            uint8_t dis_btw_left_right = rightPos - leftPos - 1;
+    static uint8_t dis_btw_left_right;
+            dis_btw_left_right = rightPos - leftPos - 1;
             if(dis_btw_left_right < TERMINATE_WIDTH)
             {
                 is_encounter_terminate = 1;
@@ -296,27 +308,47 @@
             ardu_utft_set_camimg_rowcol(CAM_ROI_UPPER_LIMIT - i, temp_mid_pos);
             ardu_utft_write_DATA(0xF8, 0x00);
 #endif
-
-            centerLine[CAM_ROI_UPPER_LIMIT - i - 1] = temp_mid_pos;
-            centerLine[(2 * CAM_ROI_UPPER_LIMIT) - i - 1] = isBorderFound;
+    static uint8_t array_pos;
+            array_pos = lineArrayEnd - i;
+            centerLine[array_pos] = temp_mid_pos;
+            leftLine[array_pos] = leftPos;
+            rightLine[array_pos] = rightPos;
             
-            if(leftPos > leftNearCenter1)
+            centerLine[(2 * CAM_ROI_UPPER_LIMIT) - 1 - i] = isBorderFound;
+            
+            if(leftPos >= leftNearPos)
             {
-                leftNearCenter2 = leftNearCenter1; 
-                leftNearCenter1 = CAM_ROI_UPPER_LIMIT - i - 1;
+                leftNearCenter1 = lineArrayEnd - i;
+                leftNearPos = leftPos;
             }
-            if(rightPos < rightNearCenter1)
+            if(rightPos <= rightNearPos)
             {
-                rightNearCenter2 = rightNearCenter1;
-                rightNearCenter1 = CAM_ROI_UPPER_LIMIT - i - 1;
+                rightNearCenter1 = lineArrayEnd - i;
+                rightNearPos = rightPos;
             }
         }
+        if( (0 < rightNearCenter1 && rightNearCenter1 < lineArrayEnd) 
+            && ((rightLine[0] - rightLine[rightNearCenter1]) >= 4)
+            )
+        {
+            intersection_info = INTERSECTION_IN_R;
+        }
         
-        if( ((1 < rightNearCenter1 && rightNearCenter1 < (CAM_ROI_UPPER_LIMIT - 2)) && (abs(rightNearCenter1 - rightNearCenter2) == 1)) ||
-            ((1 < leftNearCenter1 && leftNearCenter1 < (CAM_ROI_UPPER_LIMIT - 2)) && (abs(leftNearCenter1 - leftNearCenter1) == 1)) )
+        if( (0 < leftNearCenter1 && leftNearCenter1 < lineArrayEnd) 
+            && ((leftLine[leftNearCenter1] - leftLine[0]) >= 4)
+            )
         {
-            is_intersection_detected = 1;
+            switch(intersection_info)
+            {
+            case INTERSECTION_IN_R:
+                intersection_info = INTERSECTION_IN;
+                break;
+            default:
+                intersection_info = INTERSECTION_IN_L;
+                break;
+            }
         }
+        //LOGI("#%d,%d,%d#", rightNearCenter1, abs(rightLine[0] - rightLine[rightNearCenter1]), abs(rightLine[lineArrayEnd] - rightLine[rightNearCenter1]));
     }
 }
 
@@ -432,9 +464,9 @@
     return ardu_cam_spi_read_8(ARDUCHIP_VER);
 }
 
-uint8_t ardu_cam_get_is_intersection_detected()
+uint8_t ardu_cam_get_intersection_info()
 {
-    return is_intersection_detected;
+    return intersection_info;
 }
 
 #ifdef __cplusplus

diff -r c6665262fd3d -r ffbeefc9e218 Hardwares/ArduCAM.h
--- a/Hardwares/ArduCAM.h	Thu Apr 20 16:23:19 2017 +0000
+++ b/Hardwares/ArduCAM.h	Thu Apr 20 21:04:10 2017 +0000
@@ -14,7 +14,7 @@
 #define RESOLUTION_WIDTH  80   /*!< @brief The resolution width we are using. */
 #define RESOLUTION_HEIGHT 60   /*!< @brief The resolution height we are using. */
 //#define MANUAL_REDUCE_RESULOTION_BY2
-#define CAM_ROI_UPPER_LIMIT 6  /*!< @brief The number of lines we actually using for the image processing. */
+#define CAM_ROI_UPPER_LIMIT 7  /*!< @brief The number of lines we actually using for the image processing. */
 
 
 #define BOTH_NOT_FOUND   0     /*!< @brief ENUM for whether the border is found. Both sides of the border is not found. */
@@ -22,6 +22,12 @@
 #define RIGHT_FOUND      2     /*!< @brief ENUM for whether the border is found. Right side of the border is found. */
 #define BOTH_FOUND       3     /*!< @brief ENUM for whether the border is found. Both sides of the border is found. */
 
+#define INTERSECTION_NULL 0
+#define INTERSECTION_IN_L   1
+#define INTERSECTION_IN_R   2
+#define INTERSECTION_IN     3
+#define INTERSECTION_OUT  5
+
 //=====Must pick one and only one here!=====
 //#define ARDUCAM_SHIELD_OV2640     /*!< @brief Use ArduCam Shield V2 with OV2640 camera module. */
 //#define ARDUCAM_SHIELD_OV7725     /*!< @brief Use ArduCam Shield V2 with OV7725 camera module. */
@@ -114,9 +120,9 @@
 
 /**
 * @brief Get the array that describe the center line that detected by the image processing.
-* @return A volatile pointer points to the array.
+* @return A  pointer points to the array.
 */
-volatile const uint8_t* ardu_cam_get_center_array();
+ const uint8_t* ardu_cam_get_center_array();
 
 /**
 * @brief Check if the capture is finished.
@@ -152,7 +158,7 @@
 */
 uint8_t ardu_cam_get_ver_num();
 
-uint8_t ardu_cam_get_is_intersection_detected();
+uint8_t ardu_cam_get_intersection_info();
 
 #ifdef __cplusplus
 }

diff -r c6665262fd3d -r ffbeefc9e218 Hardwares/Motor.h
--- a/Hardwares/Motor.h	Thu Apr 20 16:23:19 2017 +0000
+++ b/Hardwares/Motor.h	Thu Apr 20 21:04:10 2017 +0000
@@ -15,7 +15,7 @@
 #define MDIR_Backward 1  /*!< @brief Move backward is high voltage level. */
 
 
-#define MOTOR_MAX_SPEED_LIMIT 0.65f /*!< @brief The percentage of the max speed (power) level for the motor driver. (This will be multiply to the speed that finally set to the motor driver) */
+#define MOTOR_MAX_SPEED_LIMIT 0.55f /*!< @brief The percentage of the max speed (power) level for the motor driver. (This will be multiply to the speed that finally set to the motor driver) */
 
 #define MOTOR_DIFF_MIN_SPEED  0.55f /*!< @brief The percentage of the digital rear differential. (It should be used outside of these functions.) */
 

diff -r c6665262fd3d -r ffbeefc9e218 StateMachine/RunningState.cpp
--- a/StateMachine/RunningState.cpp	Thu Apr 20 16:23:19 2017 +0000
+++ b/StateMachine/RunningState.cpp	Thu Apr 20 21:04:10 2017 +0000
@@ -1,6 +1,6 @@
 #include "RunningState.h"
 
-#define SW_DEBUG
+//#define SW_DEBUG
 #include "SWCommon.h"
 
 #include "StateManager.h"
@@ -13,6 +13,8 @@
 #define PI 3.14159265f
 #include <math.h>
 
+//static DigitalOut led_g(LED_GREEN, 1);
+
 RunningState::RunningState() : 
     m_speedRatio(1.0f),
     m_lastAngle(0.0f),
@@ -73,21 +75,38 @@
     //const uint8_t lineFoundRefTop = ((2 * CAM_ROI_UPPER_LIMIT) - 1);
     //const uint8_t lineFoundRefBut = (CAM_ROI_UPPER_LIMIT);
     //const uint8_t isBorderFound = ardu_cam_get_is_border_found();
-    const uint8_t isIntersectionDetected = ardu_cam_get_is_intersection_detected();
-    
-    LOGI("%d", isIntersectionDetected);
+    const uint8_t intersectionInfo = ardu_cam_get_intersection_info();
 
-    float totalAngleDegrees = (angleDegrees * 0.52f) + (offsetDegrees * (centerLine[lineFoundRefRow] != BOTH_FOUND ? 0.90f : 0.35f));
+    float totalAngleDegrees = (angleDegrees * 0.55f) + (offsetDegrees * (centerLine[lineFoundRefRow] != BOTH_FOUND ? 0.90f : 0.35f));
     
     if(totalAngleDegrees > SERVO_MAX_ANGLE)
         totalAngleDegrees = SERVO_MAX_ANGLE;
     else if(totalAngleDegrees < -SERVO_MAX_ANGLE)
         totalAngleDegrees = -SERVO_MAX_ANGLE;
     
+    switch(intersectionInfo)
+    {
+    case INTERSECTION_IN_R:
+        totalAngleDegrees = -9.0f;
+        //led_g = 0;
+        break;
+    case INTERSECTION_IN_L:
+        totalAngleDegrees = 9.0f;
+        //led_g = 0;
+        break;
+    case INTERSECTION_IN:
+        //led_g = 0;
+        break;
+    default:
+        //led_g = 1;
+        break;
+    }
+    /*
     if(isIntersectionDetected)
     {
         totalAngleDegrees = ((-0.2f) * totalAngleDegrees);// + ((0.2f) * m_lastAngle);
     }
+    */
     //LOGI("%d %5.3f ", centerLine[lineFoundRefRow], totalAngleDegrees);
     servo_set_angle(totalAngleDegrees);