【智能车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();
}