【智能车Code review】——环岛的判定与补线操作

【1】效果展示图

状态1、状态2

状态3

状态4、状态5

状态6

状态7

状态8

【2】状态机设定需要的参数

以右环岛为例：

k_left：左线的斜率
rou_of_left：左线的方差
last_rou_of_left：上一帧的左线方差
l_start：左线第一行为黑的所在行
right_turn_down;
right_turn_middle;
right_turn_up;
左线的下、中、上拐点
fiv_width[i]:第i行的赛道宽度
leftline_duan_dian：左线跳变严重的所在行
if (j >= 2 && (lefetline[j] - lefetline[j - 1]) <= -20) leftline_duan_dian = j;
flag_shizi：是否为十字的标志
huandao_size：当前环岛的大小，一般来说是人工设定的。
huandao_memory：环岛的状态
huandao_flag_R_L：左右环岛区分标志

函数

        void R_mend_rightline_right_island(byte type, byte flag_R_L)
        {
            if (flag_R_L == 2)
            {
                if (type == 1 || type == 2)//从下拐点开始往上拉，拉到上拐点下五行
                {
                    byte j = 0;
                    if (right_turn_down[0] >= 5)
                    {
                        advanced_regression(2, right_turn_down[0] - 2, right_turn_down[0], right_turn_middle[0] - 1, right_turn_middle[0]);
                        //regression(2, right_turn_down[0], right_turn_middle[0]);
                        for (j = (byte)right_turn_down[0]; j <= (byte)right_turn_middle[0]; j++)
                        {
                            int jicun = (int)(parameterB * j + parameterA);
                            if (jicun >= 185) jicun = 185;
                            else if (jicun <= 0) jicun = 0;
                            rightline[j] = (byte)jicun;
                            //centerline[j] = (byte)((rightline[j] + lefetline[j]) / 2);
                        }
                        before_zuohuandao_k = parameterB;
                        before_zuohuandao_b = parameterA;
                    }
                    else
                    {
                        int delta = right_turn_middle[0] - 0;
                        if (delta == 0) delta = 1;
                        float k = (right_turn_middle[1] - rightline[0]) * 1.0f / delta;
                        float b = rightline[0];
                        for (j = (byte)0; j <= (byte)right_turn_middle[0]; j++)
                        {
                            int jicun = ((int)(k * j + b));
                            if (jicun >= 185) jicun = 185;
                            else if (jicun <= 0) jicun = 0;
                            rightline[j] = (byte)jicun;
                        }
                    }
                }
                else if (type == 3)
                {
                    byte j = 0;
                    int delta = right_turn_middle[0] - 0;
                    if (delta == 0) delta = 1;
                    float k = (right_turn_middle[1] - rightline[0]) * 1.0f / delta;
                    float b = rightline[0];
                    for (j = (byte)0; j <= (byte)right_turn_middle[0]; j++)
                    {
                        int jicun = ((int)(k * j + b));
                        if (jicun >= 185) jicun = 185;
                        else if (jicun <= 0) jicun = 0;
                        rightline[j] = (byte)jicun;
                    }
                    //byte j = 0;
                    ////advanced_regression(2, right_turn_down[0] - 2, right_turn_down[0], right_turn_middle[0] - 1, right_turn_middle[0]);
                    ////regression(2, right_turn_middle[0]-7, right_turn_middle[0]-1);
                    //for (j = 0; j <= (byte)right_turn_middle[0]; j++)
                    //{
                    //    int jicun = (int)(before_zuohuandao_k * j + before_zuohuandao_b);
                    //    if (jicun >= 185) jicun = 185;
                    //    else if (jicun <= 0) jicun = 0;
                    //    rightline[j] = (byte)jicun;
                    //    //centerline[j] = (byte)((rightline[j] + lefetline[j]) / 2);
                    //}
                }
                else if (type == 8)
                {
                    byte j = 0;
                    //advanced_regression(2, right_turn_down[0] - 2, right_turn_down[0], right_turn_middle[0] - 1, right_turn_middle[0]);
                    if (right_turn_up[0] <= 5 && right_turn_up[0] != 0)
                    {
                        regression(2, r_sec_start, r_sec_start + 3);
                        for (j = 0; j <= (byte)r_sec_start; j++)
                        {
                            int jicun = (int)(parameterB * j + parameterA);
                            if (jicun >= 185) jicun = 185;
                            else if (jicun <= 0) jicun = 0;
                            rightline[j] = (byte)jicun;
                            //centerline[j] = (byte)((rightline[j] + lefetline[j]) / 2);
                        }
                    }
                    else if (right_turn_up[0] >= 5 && right_turn_up[1] <= 4 && break_hangshu >= right_turn_up[0])
                    {
                        regression(2, right_turn_up[0], break_hangshu);
                        for (j = 0; j <= (byte)right_turn_up[0]; j++)
                        {
                            int jicun = (int)(parameterB * j + parameterA);
                            if (jicun >= 185) jicun = 185;
                            else if (jicun <= 0) jicun = 0;
                            rightline[j] = (byte)jicun;
                            //centerline[j] = (byte)((rightline[j] + lefetline[j]) / 2);
                        }
                    }
                    else
                    {
                        int delta = right_turn_up[0] - 0;
                        if (delta == 0) delta = 1;
                        float k = (right_turn_up[1] - rightline[0]) * 1.0f / delta;
                        float b = rightline[0];
                        for (j = (byte)0; j <= (byte)right_turn_up[0]; j++)
                        {
                            int jicun = ((int)(k * j + b));
                            if (jicun >= 185) jicun = 185;
                            else if (jicun <= 0) jicun = 0;
                            rightline[j] = (byte)jicun;
                        }
                        //regression(2, right_turn_up[0], right_turn_up[0] + 2);
                        //for (j = 0; j <= (byte)right_turn_up[0]; j++)
                        //{
                        //    int jicun = (int)(parameterB * j + parameterA);
                        //    if (jicun >= 185) jicun = 185;
                        //    else if (jicun <= 0) jicun = 0;
                        //    rightline[j] = (byte)jicun;
                        //    //centerline[j] = (byte)((rightline[j] + lefetline[j]) / 2);
                        //}
                    }

                }
            }
        }
        void R_mend_leftline_right_island(byte type, byte flag_R_L)
        {
            if (flag_R_L == 2)
            {
                // SetText("TYPE:" + type);
                if (type == 4)
                {
                    byte j = 0;
                    if (right_turn_up[0] >= 4)
                    {
                        int delta = right_turn_up[0] - 3;
                        if (delta == 0) delta = 1;
                        float k = (right_turn_up[1] - lefetline[3]) * 1.0f / (delta * 1.0f);
                        // setText用户自定义("斜率" + k);
                        float b = lefetline[3] - k * 3;
                        if (right_turn_up[0] <= 24)
                        {
                            right_turn_up[0] = 24;
                        } 
                        for (j = (byte)3; j <= (byte)right_turn_up[0]; j++)
                        {
                            int jicun = ((int)(k * j + b));
                            if (jicun >= 185) jicun = 185;
                            else if (jicun <= 0) jicun = 0;
                            lefetline[j] = (byte)jicun;
                            //centerline[j] = (byte)((rightline[j] + lefetline[j]) / 2);
                        }
                        //advanced_LeastSquareCalc_Curve(1, 4, 10, (byte)(right_turn_up[0] - 3), (byte)(right_turn_up[0] - 1));
                        //for (j = (byte)3; j <= break_hangshu - 1; j++)
                        //{
                        //    int jicun = ((int)(curve_a * j * j + curve_b));
                        //    if (jicun >= 185) jicun = 185;
                        //    else if (jicun <= 0) jicun = 0;
                        //    lefetline[j] = (byte)jicun;
                        //    //centerline[j] = (byte)((rightline[j] + lefetline[j]) / 2);
                        //}
                    }
                    else
                    {

                    }
                }
                else if (type == 5) //重新扫线，从左边往右边扫
                {
                    byte j = 0;
                    if (right_turn_up[0] >= 4 && (last_memory == 4 || leftline_duan_dian >= 40))
                    {
                         setText用户自定义("拟合直线");
                        int delta = right_turn_up[0] - 3;
                        if (delta == 0) delta = 1;
                        float k = (right_turn_up[1] - lefetline[3]) * 1.0f / (delta * 1.0f);
                        float b = lefetline[3] - k * 3;
                        if (right_turn_up[0] <= 24)
                        {
                            right_turn_up[0] = 24;
                            for (int ss = 0; ss <= 24; ss++)
                            {
                                rightline[ss] = 0;
                            }
                        }
                        for (j = (byte)3; j <= (byte)right_turn_up[0]; j++)
                        {
                            int jicun = ((int)(k * j + b));
                            if (jicun >= 185) jicun = 185;
                            else if (jicun <= 0) jicun = 0;
                            lefetline[j] = (byte)jicun;
                            //centerline[j] = (byte)((rightline[j] + lefetline[j]) / 2);
                        }
                        //advanced_LeastSquareCalc_Curve(1, 4, 10, (byte)(right_turn_up[0] - 3), (byte)(right_turn_up[0] - 1));
                        //setText用户自定义("拟合曲线");
                        //for (j = (byte)3; j <= right_turn_up[0]; j++)
                        //{
                        //    int jicun = ((int)(curve_a * j * j + curve_b));
                        //    if (jicun >= 185) jicun = 185;
                        //    else if (jicun <= 0) jicun = 0;
                        //    lefetline[j] = (byte)jicun;
                        //    //centerline[j] = (byte)((rightline[j] + lefetline[j]) / 2);
                        //}
                    }
                    else if (leftline_duan_dian < 40 || right_turn_up[0]<4)
                    {
                        //advanced_regression(2, right_turn_up[0] - 2, right_turn_down[0], right_turn_middle[0] - 1, right_turn_middle[0]);
                        if (leftline_duan_dian + 5 <= break_hangshu)
                        {
                            regression(1, leftline_duan_dian, break_hangshu);
                            parameterA = lefetline[0];
                            if (parameterB <= -5) parameterB = -5;
                            setText用户自定义("斜率拟合" + parameterB);
                        }
                        else
                        {
                            int delta = break_hangshu - 0;
                            if (delta == 0) delta = 1;
                            parameterB = (rightline[break_hangshu] - lefetline[0])*1.0f / delta;
                            parameterA = lefetline[0];
                            setText用户自定义("斜率计算" + parameterB);
                        }
                        if (leftline_duan_dian <= 24)
                        {
                            leftline_duan_dian = 24;
                            for (int ss = 0; ss <= 24; ss++)
                            {
                                rightline[ss] = 0;
                            }
                        }
                        for (j = 0; j <= (byte)break_hangshu; j++)
                        {
                            int jicun = (int)(parameterB * j + parameterA);
                            if (jicun >= 185) jicun = 185;
                            else if (jicun <= 0) jicun = 0;
                            lefetline[j] = (byte)jicun;
                            //centerline[j] = (byte)((rightline[j] + lefetline[j]) / 2);
                        }
                    }
                }
                else if (type == 6)
                {
                    byte j = 0;
                    byte youguai_y = 0;
                    byte youguai_x = 0;
                    byte leftguai_y = 0;
                    byte leftguai_x = 0;
                    /***找左下拐点***********/
                    for (j = 2; j <= 60; j++)
                    {
                        if (lefetline[j - 1] - lefetline[j - 2] <= 0 && lefetline[j] - lefetline[j - 1] >= 0 && (lefetline[j + 1] - lefetline[j] > 0)
                            && leftfindflag[j - 2] == 1 && leftfindflag[j - 1] == 1 && leftfindflag[j] == 1)
                        {
                            leftguai_y = (byte)(j - 1);
                            leftguai_x = lefetline[j - 1];
                            break;
                        }
                    }
                    for (j = 8; j <= 60; j++)
                    {
                        //右连接处
                        if (rightline[j] != 0)
                        {
                            youguai_y = j;
                            youguai_x = rightline[j];
                            break;
                        }
                    }

                    if (leftguai_y != 0 && leftguai_x != 0 && youguai_y != 4 && youguai_x != 0)
                    {
                        SetText("找到右上拐点");
                        SetText("右上拐点坐标：" + youguai_y + "  " + youguai_x);
                        SetText("找到左下拐点");
                        SetText("左下拐点坐标：" + leftguai_y + "  " + leftguai_x);
                        if (youguai_y >= leftguai_y + 4)
                        {
                            SetText("要减速了");
                            jiansu_flag = 1;
                        }
                        else jiansu_flag = 0;
                        //开始拉线
                        int delta = youguai_y - leftguai_y;
                        if (delta == 0) delta = 1;
                        float k = (youguai_x - leftguai_x) * 1.0f / delta;
                        float b = leftguai_x - k * leftguai_y;
                        if (youguai_y >= leftguai_y)
                        {
                            for (j = (byte)leftguai_y; j <= (byte)youguai_y; j++)
                            {
                                int jicun = ((int)(k * j + b));
                                if (jicun >= 185) jicun = 185;
                                else if (jicun <= 0) jicun = 0;
                                lefetline[j] = (byte)jicun;
                            }
                        }
                    }
                }   
                else if (type == 7 && (long_turn_flag_left <= 55 || rou_of_left >= 5000) && ((flag_blank_out_huandao == 1 && l_start >= 16) || flag_blank_out_huandao == 0))
                {
                    jiansu_flag = 0;
                    byte j = 0;
                    byte youguai_y = r_sec_start;
                    byte youguai_x = 0;
                    byte leftguai_y = 0;
                    byte leftguai_x = lefetline[0];
                    setText用户自定义("左线最后一行所在列数" + lefetline[break_hangshu]);
                    if (left_turn_down[0] != 0 && left_turn_down[0] <= 40 && left_turn_down[1]>=140)
                    {
                        leftguai_y = (byte)left_turn_down[0];
                        leftguai_x = (byte)left_turn_down[1];
                    }
                    if (r_sec_start >= 10)
                    {
                        youguai_y = r_sec_start;
                    }
                    else
                    {
                        youguai_y = break_hangshu;
                    }
                    if (l_start >= 5) flag_blank_out_huandao = 1;
                    setText用户自定义("l_start" + l_start);
                    SetText("找到右上拐点");
                    SetText("右上拐点坐标：" + youguai_y + "  " + youguai_x);
                    SetText("找到左下拐点");
                    SetText("左下拐点坐标：" + leftguai_y + "  " + leftguai_x);
                    //开始拉线
                    //if (youguai_x <= 1 && youguai_y <= 25)
                    //{
                        youguai_y = break_hangshu;
                        if (lefetline[break_hangshu] <= 55)
                        {
                            youguai_x = lefetline[break_hangshu];
                        } 
                   // }
                    int delta = youguai_y - leftguai_y;
                    if (delta == 0) delta = 1;
                    float k = (youguai_x - leftguai_x) * 1.0f / delta;
                    float b = leftguai_x - k * leftguai_y;
                    for (j = (byte)leftguai_y; j <= (byte)youguai_y; j++)
                    {
                        int jicun = ((int)(k * j + b));
                        if (jicun >= 185) jicun = 185;
                        else if (jicun <= 0) jicun = 0;
                        lefetline[j] = (byte)jicun;
                        //纠正右线
                        if (rightline[j] > lefetline[j]) rightline[j] = lefetline[j];
                    }
                    if (youguai_y <= 27)
                    {
                        for (j = youguai_y; j <= break_hangshu; j++)
                        {
                            rightline[j] = 0;
                            lefetline[j] = 0;
                        }
                    }
                }
                else if (type == 8)
                {
                    flag_blank_out_huandao = 0;
                }
            }
        }

        void juge_left_succesive_and_if_k_limit()
        {
            //可能是环岛的地带                                                                -0.17
            if (podao_flag == 0 && huandao_flag_R_L != 1 && huandao_memory <= 5 && k_left < -0.03 && k_left > -1.5 && flag_find_huan_rightmiddle_point == 1 && rou_of_left <= 380 && last_rou_of_left <= 380 && l_start <= 20)
            {
                /*******行驶到环岛出口或者在行驶到环岛出口前********/
                if (huandao_memory <= 3 && flag_rukou2 == 0)
                {                                                       //    30        
                    if (right_jiao == 1 &&
                        ((right_turn_middle[0] >= 28 && r_start >= 14 && rightline[5] <= 10 && fiv_width[10] >= 130 && fiv_width[15] <= 183 && fiv_width[20] <= 181 && fiv_width[22] <= 180 && rou_of_right >= 1800 && last_rou_of_right >= 1800 && !(right_turn_up[0] <= 30 && right_turn_up[0] >= 3 && left_turn_up[0] <= 30 && left_turn_up[0] >= 3) && fiv_width[40] <= 100)
                        || (rightline[5] <= 10 && r_start >= 14 && break_hangshu - long_turn_flag_left <= 5 && break_hangshu - long_turn_flag_right > 10 && fiv_width[10] >= 161 && fiv_width[15] >= 157 && fiv_width[20] >= 153 && fiv_width[25] >= 127 && fiv_width[40] <= 100 && flag_shizi == 0)
                        || (rightline[5] <= 10 && r_start >= 14 && break_hangshu - long_turn_flag_left <= 5 && break_hangshu - long_turn_flag_right > 10 && fiv_width[10] >= 141 && fiv_width[10] <= 158 && fiv_width[15] >= 137 && fiv_width[15] <= 154 && fiv_width[20] >= 132 && fiv_width[20] <= 149 && fiv_width[25] >= 127 && fiv_width[25] <= 145 && fiv_width[40] <= 100 && flag_shizi == 0)
                        || (rightline[5] <= 10 && r_start >= 14 && break_hangshu - long_turn_flag_left <= 3 && break_hangshu - long_turn_flag_right > 7 && fiv_width[10] >= 133 && fiv_width[10] <= 158 && fiv_width[15] >= 128 && fiv_width[15] <= 154 && fiv_width[20] >= 120 && fiv_width[20] <= 149 && fiv_width[25] >= 113 && fiv_width[25] <= 145 && fiv_width[40] <= 100 && flag_shizi == 0)
                        || (huandao_size == 100 && r_start >= 30 && break_hangshu - long_turn_flag_left <= 2 && break_hangshu - long_turn_flag_right > 8 && fiv_width[10] >= 110 && centerline[0] <= 80 && fiv_width[10] <= 130 && fiv_width[15] >= 105 && fiv_width[15] <= 110 && fiv_width[20] >= 99 && fiv_width[20] <= 115 && fiv_width[25] >= 90 && fiv_width[25] <= 100 && fiv_width[40] <= 100 && flag_shizi == 0)
                        )
                        )
                    {
                        //setText用户自定义("1");
                        huandao_flag = 1;
                        huandao_flag_R_L = 2;
                        // SetText("行驶到环岛出口（没进环岛呢）");
                        huandao_memory = 3;
                        huandao_procedure_variable = 1;
                    }
                    else if (huandao_procedure_variable == 0 && right_turn_down[0] <= 25 && right_turn_down[0] > 0 && ((rou_of_right >= 7000 && last_rou_of_right >= 8000) || (long_turn_flag_left >= 50 && long_turn_flag_right < 20 && last_rou_of_right >= 1000)))
                    {
                        huandao_flag = 1;
                        huandao_flag_R_L = 2;
                        huandao_memory = 2;
                        // SetText("行驶到环岛出口前（没进环岛呢）(近)");       30                    条件放松一点
                        if (/*right_turn_down[0] <= 10 &&*/ (rightline[5] <= 10 && right_turn_middle[0] >= 28 && r_start >= 14 && fiv_width[15] <= 183 && fiv_width[20] <= 183 && fiv_width[22] <= 183 && rou_of_right >= 1800 && last_rou_of_right >= 1800 && !(right_turn_up[0] <= 30 && right_turn_up[0] >= 3 && left_turn_up[0] <= 30 && left_turn_up[0] >= 3) && fiv_width[40] <= 100)
                            || (rightline[5] <= 10 && r_start >= 14 && break_hangshu - long_turn_flag_left <= 5 && break_hangshu - long_turn_flag_right > 10 && fiv_width[10] >= 163 && fiv_width[15] >= 160 && fiv_width[20] >= 157 && fiv_width[25] >= 127 && fiv_width[40] <= 100 && flag_shizi == 0))
                        {
                            // SetText("行驶到环岛出口（没进环岛呢）");
                            huandao_memory = 3;
                            //setText用户自定义("2");
                            huandao_procedure_variable = 1;
                        }
                    }
                    else if (huandao_memory == 2 || huandao_memory == 1)
                    {
                        if (/*right_turn_down[0] <= 10 &&*/ (rightline[5] <= 10 && right_turn_middle[0] >= 28 && r_start >= 14 && fiv_width[15] <= 183 && fiv_width[20] <= 183 && fiv_width[22] <= 183 && rou_of_right >= 1800 && last_rou_of_right >= 1800 && !(right_turn_up[0] <= 30 && right_turn_up[0] >= 3 && left_turn_up[0] <= 30 && left_turn_up[0] >= 3))
                             || (rightline[5] <= 10 && r_start >= 14 && break_hangshu - long_turn_flag_left <= 5 && break_hangshu - long_turn_flag_right > 10 && fiv_width[10] >= 163 && fiv_width[15] >= 160 && fiv_width[20] >= 157 && fiv_width[25] >= 127 && flag_shizi == 0))
                        {
                            // SetText("行驶到环岛出口（没进环岛呢）");
                            huandao_memory = 3;
                            huandao_procedure_variable = 1;
                        }
                    }
                    else if (huandao_procedure_variable == 0 && right_turn_down[0] > 25 && right_turn_down[0] <= 40 && rou_of_right >= 7000 && last_rou_of_right >= 8000 && long_turn_flag_left >= 50 && break_hangshu <= 66 && long_turn_flag <= 50 && fiv_width[10] > 110 && fiv_width[15] > 105)
                    {
                        huandao_flag = 1;
                        huandao_flag_R_L = 2;
                        huandao_memory = 1;
                        // SetText("行驶到环岛出口前（没进环岛呢）(远)");
                    }
                    else if (huandao_memory <= 4 && right_turn_middle[0] >= 20 /*&& right_turn_middle[0] <= 55*/ && huandao_procedure_variable == 1 && r_start <= 5)
                    {
                        huandao_flag = 1;
                        huandao_flag_R_L = 2;
                        flag_rukou = 1;
                        huandao_memory = 4;
                        // SetText("行驶到环岛入口前（没进环岛呢）");
                    }
                }
                else if (huandao_memory <= 4 && right_turn_middle[0] >= 20 && /*right_turn_middle[0] <= 55 &&*/ huandao_procedure_variable == 1 && r_start <= 5)
                {
                    huandao_flag = 1;
                    flag_rukou = 1;
                    huandao_memory = 4;
                    // SetText("行驶到环岛入口前（没进环岛呢）");
                }
                else if (huandao_flag_R_L != 1 && huandao_memory <= 5 && huandao_memory >= 4 && fiv_width[10] >= 140 && fiv_width[15] >= 130 && ((right_turn_up[0] <= 33 && right_turn_up[0] > 0) || (leftline_duan_dian <= 30 && leftline_duan_dian != 0)) && huandao_procedure_variable == 1)
                {
                    huandao_flag = 1;
                    flag_rukou2 = 1;
                    huandao_memory = 5;
                    // SetText("行驶到环岛入口（没进环岛呢）");                 
                }
            }
            else if (huandao_flag_R_L != 1 && huandao_memory <= 4 && right_turn_middle[0] >= 20 && right_turn_middle[0] <= 55 && huandao_procedure_variable == 1 && r_start <= 5)
            {
                huandao_flag = 1;
                flag_rukou = 1;
                huandao_memory = 4;
            }
            else if (huandao_flag_R_L != 1 && huandao_memory <= 5 && huandao_memory >= 4 && fiv_width[10] >= 140 && fiv_width[15] >= 130 && ((right_turn_up[0] <= 33 && right_turn_up[0] > 0) || (leftline_duan_dian <= 30 && leftline_duan_dian != 0)) && huandao_procedure_variable == 1)
            {
                huandao_flag = 1;
                flag_rukou2 = 1;
                huandao_memory = 5;
                //SetText("rightfindflag[1]" + rightfindflag[1]);
                //SetText("rightfindflag[2]" + rightfindflag[2]);
                if (huandao_flag_R_L != 1 && huandao_memory == 5 && (( fiv_width[10]<=133 &&fiv_width[15]<=123 && r_start>=20) ||(r_start >= 20 && leftfindflag[5] != 0 && leftfindflag[6] != 0 && leftfindflag[7] != 0 && leftfindflag[8] != 0 && lefetline[leftline_duan_dian] <= 65 && leftline_duan_dian <= 38) || (leftfindflag[5] != 0 && leftfindflag[6] != 0 && leftfindflag[7] != 0 && leftfindflag[8] != 0 && leftline_duan_dian <= 25)))
                {
                    huandao_flag = 1;
                    huandao_memory = 6;
                    //flag_rukou = 3;
                    // SetText("已经进去了");
                }
                //SetText("行驶到环岛入口（没进环岛呢）");
            }
            else if (huandao_flag_R_L != 1 && huandao_memory == 5 && ((fiv_width[10] <= 133 && fiv_width[15] <= 123 && r_start >= 20)||(r_start >= 20 && leftfindflag[5] != 0 && leftfindflag[6] != 0 && leftfindflag[7] != 0 && leftfindflag[8] != 0 && lostleft_times <= 10 && leftline_duan_dian <= 42) || (r_start >= 20 && leftfindflag[5] != 0 && leftfindflag[6] != 0 && leftfindflag[7] != 0 && leftfindflag[8] != 0 && leftline_duan_dian <= 38) || (leftfindflag[5] != 0 && leftfindflag[6] != 0 && leftfindflag[7] != 0 && leftfindflag[8] != 0 && leftline_duan_dian <= 25)))
            {
                huandao_flag = 1;
                huandao_memory = 6;
                //flag_rukou = 3;
                // SetText("已经进去了");
            }
            else if (huandao_flag_R_L != 1 && huandao_memory == 6 && left_turn_down[0] >= 1 && left_turn_down[0] <= 27 && left_turn_down[1] > 25 && flag_memory_6 >= 4)
            {
                huandao_flag = 1;
                huandao_memory = 7;
                //SetText("出环了");
            }
            else if (huandao_flag_R_L != 1 && huandao_memory == 7 && (My_Abs(right_turn_up[0] - (l_start - 4)) >= 5) && (fiv_width[10] < 182 && fiv_width[15] < 182)
                &&(((left_turn_down[0] == 0 || left_turn_down[0]>=43) && right_turn_up[0] >= 15 && right_turn_up[0] <= 51 && right_turn_up[1] <= 100 && r_start >= 26 && rou_of_left <= 5000 /*&& rightline[break_hangshu] < 85*/)
                || ((left_turn_down[0] == 0 || left_turn_down[0] >= 43) && right_turn_up[0] >= 15 && right_turn_up[0] <= 51 && right_turn_up[1] >= 20 && right_turn_up[1] <= 100 && rou_of_left <= 5000 /*&& rightline[break_hangshu] < 85*/)
                || ((left_turn_down[0] == 0 || left_turn_down[0] >= 43) && long_turn_flag_left >= 40 && k_left >= -1.5 && k_left < 0 && lefetline[0] < 155 && l_first_notget >= 40/*&& rou_of_left <= 5000 && rightline[break_hangshu] < 85*/)
                || ((left_turn_down[0] == 0 || left_turn_down[0] >= 43) && right_turn_up[0] >= 35 && right_turn_up[0] <= 51 && right_turn_up[1] >= 20 && right_turn_up[1] <= 100 && rou_of_left <= 5000)
                || ((left_turn_down[0] == 0 || left_turn_down[0] >= 43) && huandao_size ==50 && right_turn_up[0] >= 15 && right_turn_up[0] <= 51 && right_turn_up[1] >= 10 && right_turn_up[1] <= 50 && rou_of_left <= 5000)))
            {
                huandao_flag = 1;
                huandao_memory = 8;
                ////SetText("第二次到入口了");
            }
            else if (huandao_flag_R_L != 1 && ((fps_out_huan_island >= 10 && huandao_size >= 80) || (huandao_size <= 70)) && huandao_memory >= 8 && (fps_out_huan_island >= fps_set_out_HUAN_Island || (
                ((right_turn_up[0] < 25 && right_turn_up[0] != 0) || ((right_turn_up[0] >= 50 || right_turn_up[0] == 0) && fiv_width[10] >= 120 && fiv_width[10] <= 135 && fiv_width[15] >= 110 && fiv_width[15] <= 128))
                || (fiv_width[30] >= 60 && fiv_width[30] <= 103 && fiv_width[25] >= 68 && fiv_width[25] <= 115 && (My_Abs(long_turn_flag_left - long_turn_flag_right) <= 5 || (lostleft_times == 0 && lostright_times == 0))))
                && ((rightfindflag[15] == 1 && rightfindflag[16] == 1 && rightfindflag[17] == 1 && rightfindflag[18] == 1) || (r_sec_start == 0 && l_sec_start == 0))))
            {
                huandao_memory = 9;
                huan_order++;
                cross_road_times++;
                //SetText("完全脱离环岛，可以清除标志了");
                clear_huandao_data();

            }
            R_mend_leftline_right_island(huandao_memory, huandao_flag_R_L);
            R_mend_rightline_right_island(huandao_memory, huandao_flag_R_L);
            last_memory = huandao_memory;
            if (huandao_memory == 8 && huandao_flag_R_L != 1)
            {
                fps_out_huan_island++;
                setText用户自定义("fps_out_huan_island" + fps_out_huan_island);
            }
            if (huandao_memory >= 1 && huandao_flag_R_L != 1)
            {
                setText用户自定义("huandaosize" + huandao_size);
            }
            byte j = 0;
            if (huandao_memory <= 5 && huandao_memory != 0)
            {
                for (j = 0; j < 70; j++)
                {
                    //LCenter[j] = forecast_centerline[j];
                    centerline[j] = (byte)((lefetline[j] + rightline[j]) / 2);
                    LCenter[j] = centerline[j];
                    L_black[j] = lefetline[j];
                    R_black[j] = rightline[j];
                }
            }
            else
            {
                for (j = 0; j < break_hangshu; j++)
                {
                    //LCenter[j] = forecast_centerline[j];
                    centerline[j] = (byte)((lefetline[j] + rightline[j]) / 2);
                    LCenter[j] = centerline[j];
                    L_black[j] = lefetline[j];
                    R_black[j] = rightline[j];
                }
            }
            show_condition_of_huan();
        }