智能车学习日记【四】————环岛
前言
环岛从第一次写完到国赛结束之间重写了很多遍，都是在上一次的思路上修修补补，只能说图像补出来的效果还行，但代码结构就和屎山一样，所以我环岛篇就着重说一下我的补线思路(我的方法其实还有很多漏洞需要改进，仅供大家参考理解一下，欢迎各位在评论区指点出可改进之处！)
一、环岛分状态（以左环岛为例）
我将环岛分为7个状态，如下图
环岛状态1：远处看见并判出环岛
环岛状态2：环岛直行部分
环岛状态3：拉线转入环岛
环岛状态4：环岛内转弯
环岛状态5：拉线出环
环岛状态6：直道拉线摆正
环岛状态7：拉线直行
环岛状态8：退出（可以不细分这个状态）
整体效果(视频转gif后丢了很多帧导致看着很卡顿)
二、每个状态思路
状态1和状态2：
状态1和状态2我是放在了判断的最前头，因为直道接环岛会先进入状态1，而弯道接环岛可能会跳过状态1而直接进入状态2。
是进入状态1还是状态2是取决于找到拐点的数量和其他小的限制条件。
下图中红色是下拐点，橙色是中拐点，绿色是上拐点
首先进行基础扫线将左右边线扫描出来。这时要进行一波分类，首先找下拐点。
1.若是能找到下拐点，则说明可能是直道接环岛，接下来若能找到中上拐点则直接进入环岛状态1。
2.若找不到下拐点，且左下边的丢线行比较多则说明可能是弯接环岛，接下来若能找到中上拐点则直接进入环岛状态2。
找下拐点思路：与普通十字找下拐点思路类似，一般都会有很明显的突变，根据图像突变找即可。
找中拐点思路：中拐点需要找中间的")" 或 "("形状的最突出点。这个只需要用边线的变化，对于左环岛，中间这段边线是先减小再增大，则突变点就为中拐点。
找上拐点思路：找上拐点与找十字上拐点的方法相似，都是通过上边连续下边突变的方法来找到，当然条件需要适当修改，一般可以认为中拐点与上拐点在一个相近的x区域中，防止上拐点找错找偏。对于状态一而言上拐点太远了，有时候可能就是几个像素，所以我在状态1时拉线会多考虑下拐点和中拐点进行拉线，上拐点只是作为一个进入状态的依据。
状态3：
状态3为拉线入环。这里就是找一个左上拐点进行拉线即可，这一状态的方法比较简单。值得一提的是在入环时由于车身逐渐偏移，可能会导致中线偏移，使得原本属于左线的部分被归类为右线，就找不出左上拐点。为了解决这种情况，我是添加了一种扫线方式，即找到的左上拐点较远时，依旧采用中线扫线的方式来找拐点拉线，而当左上拐点的高度下降到一定值时，切换扫线方式，改为由左向右扫线(即左边全丢线，然后扫出右边线去找右上拐点。若是右环岛应改为从右向左扫线)。当然使用这些方法的目的是准确找出拉线的上拐点，只要能准确找出来方法是什么倒不是很重要。
状态4：
这个状态没什么好说的，正常的弯道转弯，不需要任何的拉线操作。调好pid和差速即可
状态5：
状态5为拉线出环，之前状态4过程中是无法找到图中拉线的右下拐点的，当找到右下拐点时跳转到状态5，开始拉线出环。这里找拐点的方式比较简单，几乎不存在中线扫错的情况，左右边线基本都能正确找出来，唯一和找正常下拐点不同的是这个拐点的上下部分都比较连续，没有很大突变，博主是放宽了找拐点的条件，只要有凸出来的地方就算是拐点，而且这种地方一般只有这个拐点处凸的很明显，也很难找错（要是路肩贴的不好出现奇奇怪怪的拐角就另说了）。找到拐点就好拉线了，我这里并没有拉到最左边，因为拉到最左边的话会导致图像倾斜特别厉害，小环容易转得太厉害了导致撞内路肩和侧翻，所以我是在左边线上找了一个点进行拉线。
状态6：
当拉线的拐点消失或者太低了就从状态5转到状态6。状态六的拉线我是比较暴力的，状态5结束时，底下基本都是白色的，左右两边也是白色的，于是我从最左底下开始往上找，找到不丢线的点，然后与右下原点直接拉线。这里拉线方式无所谓，目的就是让他能一直往左转弯，甚至要是很懒的话可以把5和6合并为一个状态用一个拉线方式，只是6的下拐点固定为右下原点（0，0）。
状态7：
状态6的时候一直找左上拐点，由于状态6左边一直丢线，所以是一直找不到左上拐点，当找到时，跳转到7状态，持续找左上拐点，然后拉线直行。等到左上拐点太低了，或者是找不到了，跳转到最后的退出状态。
部分代码：
前言就说了我开始写这个环岛时没想过什么安排一个合理的结构，所以代码特别乱，有些部分判段嵌套在其他地方了，没办法全部移过来，不过代码主体思路就是我上面的说明，然后就对每一个地方进行细化和微调即可。下面的代码看看图一乐知道大概就好
void huandao_zuo()
{
// SetText(" ————————————————————————————————————————————— ");
// SetText(" ************************************* 左环岛状态机 *************************************** ");
int k = 1;
byte unwhite_zuo = 0;
byte i;
byte laxian = 0;
byte k1 = 0;
find_middleline();
findpot();
int j = 0;
num1 = 0;
num2 = 0;
num3 = 0;
num4 = 0;
fangcha(2, 0, 50);
fangcha_you = (int)sumE;
advanced_regression(2, 20, 25, 25, 30);
xielv_now = parameterB;
advanced_regression(1, 0, 2, 2, 5);
// SetText("parameterB11 " + parameterB);
for (i = 0; i < 50; i++)
{
if (L_black[i] == 185)
{
j++;
}
else
{
// SetText("左边丢线行 " + j);
break;
}
}
if (kt >= 21)
{
hd_state = 2;
kt = 0;
}
if ((float_abs(parameterB) >= 0.1 || j < 20) && hd_state <= 1) //第一个状态，底下没丢线
{
hd2_findmiddle();
for (i = 1; i < 20; i++)
{
if (L_black[i] != 185)
{
k = i;
break;
}
}
for (i = (byte)k; i < 69; i++)
{
if (firstto_hd == 0)
{
if (((L_black[i] - L_black[i - 1] > 3 && L_black[i + 1] - L_black[i - 1] > 3 && L_black[i + 1] - L_black[i] >= 0) || (tubian_xia_y > 0 && L_black[i] > tubian_xia_x)) && num2 <= 15 && i < 45)
{
num2++;
if (num2 == 1)
{
tubian_xia_y = (byte)(i - 1); //记录第一次突变点
tubian_xia_x = L_black[i - 1];
// SetText("tubian_xia_x " + tubian_xia_x + " tubian_xia_y " + tubian_xia_y);
}
}
if (num2 > 5)
{
if ((L_black[i] - L_black[i - 1] <= -1 && num3 <= 7) || (num3 >= 5 && L_black[i] - L_black[i - 1] <= 0))
{
num3++;
}
}
if (num3 >= 7)
{
if (L_black[i] - L_black[i - 1] > 0)
{
num4++;
if (num4 == 1 && i - 1 < 60)
{
tubian_zhong_y = (byte)(i - 1);
tubian_zhong_x = L_black[i - 1];
// SetText("tubian_zhong_x " + tubian_zhong_x + "tubian_zhong_y " + tubian_zhong_y);
}
}
}
if (num4 > 1)
{
if (abs(L_black[i + 1] - L_black[i]) <= 1 && L_black[i] - L_black[i - 1] < -6 && L_black[i] > tubian_zhong_x)
{
tubian_shang_x = L_black[i];
tubian_shang_y = i;
// SetText("tubian_shang_x " + tubian_shang_x + " tubian_shang_y " + tubian_shang_y);
}
}
}
if (tubian_xia_y > 0 && tubian_zhong_y > 0 && tubian_shang_y > 0) //3个拐点都找到
{
advanced_regression(1, tubian_xia_y - 1, tubian_xia_y, tubian_zhong_y - 1, tubian_zhong_y);
run(1, tubian_xia_y - 1, tubian_zhong_y, parameterB, parameterA);
advanced_regression(1, tubian_zhong_y - 1, tubian_zhong_y, tubian_shang_y, tubian_shang_y + 1);
run(1, tubian_zhong_y, tubian_shang_y, parameterB, parameterA);
run(0, 0, tubian_shang_y, parameterB, parameterA);
// SetText("parameterB " + parameterB + " parameterA " + parameterA);
// SetText(" 状态 ： 赛道中间 正常补线 ");
break;
}
else if (tubian_xia_y > 0 && tubian_zhong_y > 0 && tubian_shang_y == 0 && (i == 68 || firstto_hd == 1)) //上拐点没找到
{
parameterB = (float)((tubian_xia_x - tubian_zhong_x) * 1.0 / (tubian_xia_y - tubian_zhong_y)*1.0);
parameterA = tubian_zhong_x - parameterB * tubian_zhong_y;
run(1, tubian_xia_y - 1, tubian_zhong_y, parameterB, parameterA);
run(0, 0, tubian_zhong_y, parameterB, parameterA);
// SetText("parameterB " + parameterB + " parameterA " + parameterA);
// SetText(" 状态 ： 赛道中间 没找到上拐点 正常补线 ");
break;
}
}
hd_state = 1; //状态一 正常补线
firstto_hd = 0;
}
if ((parameterB > -0.1 && hd_state == 1 && tubian_xia_y == 0 && L_black[0] == 185) || hd_state == 2) // 底下丢线
{
hd2_findmiddle();
findpot();
hdtime2++;
byte unwhitenum_zuo = 0;
for (i = 1; i < 20; i++)
{
if (L_black[i] != 185)
{
k = i;
break;
}
}
for (i = (byte)k; i < 69; i++) //这里找下拐点是因为弯接环岛后实际应该为状态1，却判定为状态2，而本来下拐点的位置找成中拐点，所以可以根据中下拐点的位置来判断是否需要回退状态,这个下拐点找的条件较松，尽量按照中拐点的方式去找
{
if (L_black[i] - L_black[i - 1] >= 1 && L_black[i + 1] - L_black[i - 1] >= 1 && L_black[i + 1] - L_black[i] >= 0 && i > 1)
{
tubian_xia_y = (byte)(i - 1); //记录第一次突变点
tubian_xia_x = L_black[i - 1];
// SetText("tubian_xia_x " + tubian_xia_x + " tubian_xia_y " + tubian_xia_y);
break;
}
}
for (i = (byte)k; i < b-1; i++)
{
if ((L_black[i] - L_black[i - 1] > 0 && i > 1 && L_black[i+1] - L_black[i-1] > 0 && L_black[i - 1] >= 5 && L_black[i - 1] <= 180) || (num4 >= 1 && L_black[i] == 185))
{
num4++;
if (num4 >= 1 && i - 1 < 59 && tubian_zhong_y == 0)
{
tubian_zhong_y = (byte)(i - 1);
tubian_zhong_x = L_black[i - 1];
// SetText("tubian_zhong_x " + tubian_zhong_x + "tubian_zhong_y " + tubian_zhong_y);
}
}
if (num4 > 1)
{
if (abs(L_black[i + 1] - L_black[i]) <= 6 && L_black[i] - L_black[i - 1] >= -3 && L_black[i] < tubian_zhong_x && tubian_zhong_y != 0)
{
tubian_shang_x = L_black[i];
tubian_shang_y = i;
// SetText("tubian_shang_x " + tubian_shang_x + " tubian_shang_y " + tubian_shang_y);
}
}
if (tubian_zhong_y > 0)
{
j = 0;
for (y = 0; y < tubian_zhong_y; y++)
{
if (L_black[y] == 185)
{
j++;
}
}
}
if (tubian_zhong_y > 0 && tubian_shang_y > 0) //上中拐点都找到
{
hd2_ChangeStateTime++;
// SetText("hd2_ChangeStateTime = " + hd2_ChangeStateTime);
if (hd2_ChangeStateTime <= 4) //如果二状态中拐点下面不是全白，则回退一状态
{
for (y = tubian_zhong_y; y >= 0; y--)
{
if (L_black[y] == 185)
break;
if (y == 0)
break;
}
if (y <= 3)
{
if (abs(tubian_zhong_y - tubian_xia_y) < 5 && tubian_zhong_y <= 25 && L_black[tubian_zhong_y + 10] == 185)
{
hd_state = 1;
// SetText("弯接环岛第" + hd2_ChangeStateTime + "帧，底下出现非丢线行，环岛状态2变为1");
}
}
else
{
for (; y >= 0; y--)
{
if (L_black[y] != 185)
unwhitenum_zuo++;
if (y == 0)
break;
}
if (unwhitenum_zuo >= 5 && abs(tubian_zhong_y - tubian_xia_y) < 5)
{
hd_state = 1;
// SetText("弯接环岛第" + hd2_ChangeStateTime + "帧，底下出现非丢线行，环岛状态2变为1");
}
}
}
advanced_regression(1, tubian_zhong_y - 1, tubian_zhong_y, tubian_shang_y, tubian_shang_y + 1);
float kb = parameterB;
float jieju = parameterA;
if (parameterB <= -1.15 || parameterB >= 0)
{
advanced_regression(1, tubian_shang_y, tubian_shang_y + 1, tubian_shang_y + 2, tubian_shang_y + 3);
if (parameterB <= -1.15 || parameterB >= 0)
{
byte y1 = 0;
for (y = tubian_zhong_y; y > 1; y--)
{
if (L_black[y] - L_black[y - 1] < 0 && L_black[y - 1] - L_black[y - 2] < 0)
{
y1 = (byte)(y - 2);
break;
}
}
if (y1 > 3)
{
advanced_regression(1, y1, y1 - 1, y1 - 2, y1 - 3);
}
if (parameterB <= -1.15 || parameterB >= 0)
{
parameterB = kb;
parameterA = jieju;
}
}
}
run(1, 0, 68, parameterB, parameterA);
run(0, 0, tubian_shang_y, parameterB, parameterA);
// SetText("中上拐点之间拉线斜率为" + parameterB);
// SetText(" 状态 ： 即将进入环岛 正常补线 ");
break;
}
else if (tubian_zhong_y > 0 && tubian_shang_y == 0 && i == 68)
{
advanced_regression(1, tubian_zhong_y - 2, tubian_zhong_y - 1, tubian_zhong_y - 1, tubian_zhong_y);
run(1, 0, 68, parameterB, parameterA);
run(0, 0, 68, parameterB, parameterA);
// SetText(" 状态 ： 即将进入环岛 没找到上拐点 正常补线 ");
break;
}
}
if ((unwhitenum_zuo >= 5 || ((abs(tubian_zhong_y - tubian_xia_y) < 5) && hd_state == 1 && hd2_ChangeStateTime <= 5)))
{
hd2_ChangeStateTime = 10;
}
else
{
// SetText("j = " + j);
hd_state = 2;
if ((tubian_zhong_y == 0 && tubian_shang_y == 0) || (tubian_shang_y >= 58 && tubian_zhong_y >= 50 && Pixels[0][185] != 0 && Pixels[5][185] != 0 && last_tubian_zhong_y < 45 && hdtime2 >= 5)) //都没找到，可能左边丢线
{
k1 = 0;
fangcha(1, 0, 50);
fangcha_zuo = (int)sumE;
fangcha(2, 0, 50);
fangcha_you = (int)sumE;
// SetText("fangcha_zuo " + fangcha_zuo + " fangcha_you " + fangcha_you);
for (y = 0; y < 50; y++)
{
if (Pixels[y][185] == 0)
{
k1++;
}
for (x = LCenter[0]; x < 186; x++)
{
if (x == 185)
{
k++;
break;
}
if (Pixels[y][x] != 0 && Pixels[y][x + 1] == 0 && Pixels[y][x - 1] != 0)
{
break;
}
}
}
if (k >= 37 && fangcha_zuo > 300 && lefty[1] != 0 && tubian_zhong_y >= 40)
{
hd_state = 3;
}
else if (k >= 31 && fangcha_zuo > 300 && lefty[1] != 0 && tubian_zhong_y < 40)
{
hd_state = 3;
}
else if (k >= 25 && fangcha_zuo < 900 && lefty[1] != 0 && tubian_zhong_y == 0)
{
hd_state = 3;
}
}
}
}
if ((Pixels[0][185] == 0 && Pixels[2][185] == 0 && Pixels[3][185] == 0 && hd_state == 2 && k1 > 20) || (hd_state == 3) || (j >= 40 && hd_state == 2 && (tubian_zhong_y < 44 || (j >= 40 && tubian_zhong_y >= 50 && tubian_shang_y == 0)) && lefty[1] != 0) || (tubian_zhong_y >= 35 && j >= 21 && lefty[1] != 0 && last_tubian_zhong_y < 30 && last_tubian_zhong_y != 0) || (last_tubian_zhong_y < 25 && tubian_zhong_x > 180 && last_tubian_zhong_y != 0 && (j > 15 || (j == 0 && hdtime2 >= 4))))
{
k = 0;
laxian = 0;
find_middleline();
findpot();
if (lefty[1] == 0)
{
if (righty[1] == 0)
{
for (i = 1; i < b; i++)
{
if (R_black[i] - R_black[i - 1] > 15 && i + 5 < b && R_black[i + 5] != 0)
{
lefty[1] = i;
leftx[1] = R_black[i];
break;
}
}
}
else
{
for (i = 1; i < 60; i++)
{
if (R_black[i] - R_black[i - 1] > 15 && R_black[i] >= rightx[1] && i <= last_guaiy)
{
lefty[1] = i;
leftx[1] = R_black[i];
break;
}
}
if (lefty[1] == 0)
{
for (i = 1; i < 60; i++)
{
if (L_black[i] - L_black[i - 1] <= -10 && i <= last_guaiy)
{
lefty[1] = i;
leftx[1] = L_black[i];
break;
}
}
}
}
}
if ((lefty[1] == 0 && b < 50) || abs(lefty[1] - b) < 10)
{
for (y = 1; y < b + 5; y++)
{
for (x = (byte)(LCenter[0] - 10); x < 186; x++)
{
if (x == 185 || (Pixels[y][x] != 0 && Pixels[y][x - 1] != 0 && Pixels[y][x + 1] == 0))
{
L_black[y] = x;
break;
}
}
if (L_black[y] - L_black[y - 1] < -20 && L_black[y + 1] != 185)
{
lefty[1] = y;
leftx[1] = L_black[y];
break;
}
}
}
if ((lefty[1] > 0 && lefty[1] <= 25 && L_black[0] == 185) || hd3_potlinechange == 1) //入环时右上拐点太低了就换个扫线方式，防止基本扫线的中线太靠中间导致舵机打角太小
{
hd3_potlinechange = 1;
hd_findline(4);
laxian = 1;
// SetText("改为从左向右扫线");
}
if (lefty[1] > 0)
{
last_guaix = leftx[1];
last_guaiy = lefty[1];
parameterB = (float)((leftx[1] - 0) * 1.2) / (lefty[1]);
parameterA = (float)(leftx[1] * 1.0 - parameterB * lefty[1] * 1.0);
// SetText("parameterB " + parameterB + " parameterA " + parameterA);
run(2, 0, lefty[1], parameterB, parameterA);
run(0, 0, lefty[1], parameterB, parameterA);
// SetText(" 状态 ： 进入环岛中 正常拉线 左上拐点 y，x" + lefty[1] + " " + leftx[1]);
if (lefty[1] > 25)
{
for (i = (byte)(lefty[1]); i < 69; i++)
{
LCenter[i] = LCenter[lefty[1] - 1];
}
}
else
{
for (i = b; i < 70; i++)
{
LCenter[i] = 184;
}
}
}
hd_state = 3;
if (lefty[1] == 0 && last_guaiy != 0)
{
if (lefty[1] == 0)
{
if (L_black[0] >= 185)
{
for (i = 184; i > 0; i--)
{
if (k == 0 && i > 5 && Pixels[0][i + 1] != 0 && Pixels[0][i] != 0 && Pixels[0][i - 1] == 0)
{
k = i;
}
if (i > 1 && k != 0 && Pixels[0][i + 1] == 0 && Pixels[0][i] == 0 && Pixels[0][i - 1] != 0)
{
// SetText("左下有断层，状态3变4");
flag4 = 1;
hd_state = 4;
}
}
if (hd_state != 4)
{
for (i = 184; i > 0; i--)
{
if (Pixels[0][i + 1] != 0 && Pixels[0][i] != 0 && Pixels[0][i - 1] == 0)
{
break;
}
}
if (i > 1)
{
i--;
for (; i >= 0; i--)
{
if (i == 0)
{
flag4 = 1;
hd_state = 4;
// SetText("上拐点找不到，已经进入弯道状态，状态3变4");
break;
}
if (Pixels[0][i] != 0)
{
break;
}
}
}
}
}
}
if (hd_state == 3)
{
parameterB = (float)((last_guaix - 0) * 1.2) / (last_guaiy);
parameterA = (float)(last_guaix * 1.0 - parameterB * last_guaiy * 1.0);
// SetText("parameterB " + parameterB + " parameterA " + parameterA);
run(2, 0, last_guaiy, parameterB, parameterA);
if (laxian == 1)
{
run(0, 0, b, parameterB, parameterA);
}
else
{
run(0, 0, last_guaiy, parameterB, parameterA);
for (i = (byte)(last_guaiy); i < 69; i++)
{
LCenter[i] = LCenter[last_guaiy - 1];
}
}
// SetText(" 状态 ： 进入环岛中 根据上次拐点拉线 ");
}
}
}
if ((hd_state == 3 && Pixels[5][0] == 0 && Pixels[7][0] == 0 && Pixels[10][0] == 0 && b < 45 && lefty[1] <= 20 && laxian == 0) || ((hd_state == 4) || flag4 == 1) || (lefty[1] == 0 && righty[1] > 0 && hd_state == 3 && last_guaiy != 0))
{
j = 0;
unwhite_zuo = 0;
find_middleline();
findpot();
for (i = 1; i < 50; i++)
{
if (LCenter[i] != LCenter[i - 1])
{
break;
}
if (i > 45)
{
//SetText("重新扫线 ");
hd_findline(1);
break;
}
// SetText(" 状态 ： 出环中 重新找线 ");
}
if (flag4 == 1)
{
hd_findline(1);
flag4 = 0;
}
// SetText(" 状态 ： 环岛内 正常拉线 ");
hd_state = 4;
hdtime4++;
for (i = (byte)(b-1); i > 0; i--)
{
if (L_black[i] != 185 && L_black[i - 1] != 185)
{
unwhite_zuo++;
}
}
for (i = 1; i < b; i++)
{
if (R_black[i] - R_black[i - 1] <= -1 && R_black[i] != 0 && R_black[i + 1] - R_black[i] <= 0 && R_black[i + 1] != 0 && R_black[i + 2] - R_black[i + 1] <= 0)
{
j = 1;
break;
}
}
}
if ((b >= 38 && hd_state == 4 && R_black[0] != 0 && hdtime4 > 3 && j == 1 && unwhite_zuo >= 3) || (hd_state == 5))
{
find_middleline();
//findpot();
for (i = 1; i < b; i++)
{
if (R_black[i] - R_black[i - 1] >= 0 && R_black[i + 1] - R_black[i] < 0)
{
righty[0] = i;
rightx[0] = R_black[i];
// SetText("找到右下拐点y x " + i + " " + R_black[i]);
break;
}
}
if (righty[0] > 0)
{
for (i = 1; i < b; i++)
{
if (Pixels[i - 1][185] != 0 && Pixels[i][185] != 0 && Pixels[i + 1][185] == 0)
{
x = i;
y = 185;
// SetText("x " + x + " y " + y);
break;
}
}
if (x == righty[0])
x++;
parameterB = (float)((y - rightx[0]) * 1.0 / (x - righty[0]));
parameterA = (float)(rightx[0] * 1.0 - parameterB * righty[0] * 1.0);
// SetText("parameterB " + parameterB + " parameterA " + parameterA);
run(2, righty[0] - 1, b, parameterB, parameterA);
for (i = 0; i < 60; i++)
{
L_black[i] = 185;
}
run(0,0, 50, parameterB, parameterA);
for (i = x; i < 70; i++)
{
LCenter[i] = LCenter[x];
}
//SetText(" 状态 ： 准备出环 正常拉线 ");
}
hd_state = 5;
if (last_guaiy - righty[0] < -5 && last_guaiy != 0 && last_guaiy <= 16)
{
hd_state = 6;
}
last_guaiy = righty[0];
}
if ((righty[0] <= 7 && hd_state == 5 && last_guaiy < 15) || (hd_state == 6))
{
x = 0;
y = 0;
k = 0;
byte cnt = 0;
float k_you = 0;
byte x_down = 0;
byte y_down = 73;
byte whitezuo = 0;
byte whiteyou = 0;
for (i = 0; i < 50; i++)
{
if (LCenter[i] != 92)
{
break;
}
if (i > 48)
{
hd_findline(1);
// SetText(" 状态 ： 出环中 重新找线 ");
}
}
for (i = 0; i < b; i++)
{
if (L_black[i] == 185)
whitezuo++;
else
break;
}
for (i = 0; i < b; i++)
{
if (R_black[i] == 0)
whiteyou++;
else
break;
}
// SetText("左边丢线行与右边丢线行的差值为：" + (whitezuo - whiteyou));
for (i = 1; i < 60; i++)
{
if (L_black[i] == 185 && R_black[i] - R_black[i - 1] > 2 && R_black[i] != 0 && R_black[0] != 0)
{
k++;
}
}
// SetText(" k " + k);
if (k < 17)
{
for (i = 1; i < 68; i++)
{
if (Pixels[i][185] == 0 && Pixels[i - 1][185] != 0 && Pixels[i + 1][185] == 0 && Pixels[i + 2][185] == 0)
{
x = i;
y = 185;
break;
}
}
}
cnt = 0;
for (i = 0; i < b; i++)
{
if (L_black[i] < 185 && L_black[i] >= 92)
{
cnt++;
}
}
// SetText(" 左上边出现不是丢线数 cnt" + cnt);
advanced_regression(2, 0, 15, 15, 30);
k_you = parameterB;
// SetText("右边下边30行的斜率是" + k_you); //下边斜率不能太斜，防止大弯转弯过大导致左边提前出现黑色退出六状态
if (b > 10)
{
fangcha(1, 0, b - 10);
fangcha_zuo = (int)sumE;
fangcha(2, 0, b - 10);
fangcha_you = (int)sumE;
// SetText("左方差 " + fangcha_zuo + " 右方差 " + fangcha_you);
}
for (i = 0; i < 185; i++)
{
if (Pixels[0][i] == 0 && Pixels[0][i+1] != 0)
{
y_down = i;
break;
}
}
if (x != 0 && y > 93)
{
// SetText("拉线的左上点 y=" + x + " x=" + y + "拉线下拐点y0=0 x0=" + y_down);
parameterB = (float)((y - y_down) * 1.0 / x);
parameterA = (float)(y * 1.0 - parameterB * x * 1.0);
run(2, 0, x, parameterB, parameterA);
run(0, 0, x, parameterB, parameterA);
// SetText("parameterB " + parameterB + " parameterA " + parameterA);
for (i = x; i < 60; i++)
{
L_black[i] = L_black[x - 1];
R_black[i] = R_black[x - 1];
LCenter[i] = LCenter[x - 1];
}
// SetText(" 状态 ： 出环中 正常拉线 ");
laxian = 1;
}
if (y < 93)
{
//SetText("拉线的左上点 y=" + y + " x=" + x);
for (i = 0; i < 70; i++)
{
if (Pixels[i][185] != 0 && Pixels[i + 1][185] == 0)
{
x = i;
y = 185;
break;
}
}
// SetText("拉线的左上点 y=" + x + " x=" + y + "拉线下拐点y0=0 x0=" + y_down);
parameterB = (float)((y - y_down) * 1.0 / x);
parameterA = (float)(y * 1.0 - parameterB * x * 1.0);
run(2, 0, x, parameterB, parameterA);
run(0, 0, x, parameterB, parameterA);
// SetText("parameterB " + parameterB + " parameterA " + parameterA);
if (x > 0)
{
for (i = x; i < 60; i++)
{
L_black[i] = L_black[x - 1];
R_black[i] = R_black[x - 1];
LCenter[i] = LCenter[x - 1];
}
}
laxian = 1;
}
if (b > 10)
{
if ((fangcha_zuo < 350 && cnt >= 15 && k_you <= 1.6 && k_you >= 0) || (fangcha_you < 40 && cnt >= 25 && k_you <= 2.3 && k_you >= 0) || (k_you <= 2 && (whitezuo - whiteyou) > 35 && k_you > 0.7)) //正常左线斜率不会这么小
{
hd_state = 7;
}
else
{
hd_state = 6;
}
}
}
if ((float_abs(xielv_now) > 2 && hd_state == 6 && float_abs(xielv_last) >= 2 && Pixels[5][185] == 0 && Pixels[10][185] == 0 && laxian == 0) || hd_state == 7)
{
find_middleline();
findpot();
fangcha(1, 0, 50);
fangcha_zuo = (int)sumE;
fangcha(2, 0, 50);
fangcha_you = (int)sumE;
// SetText("左方差 " + fangcha_zuo + " 右方差 " + fangcha_you);
juge_lineContinuity(20, 60, 3, -3, 0);
// SetText("中线连续行 " + long_turn_flag);
for (i = 0; i < 50; i++)
{
if (L_black[i] < 185)
{
i++;
}
}
advanced_regression(1, 0, 20, 20, 40);
// SetText("左下边40行的斜率为" + parameterB);
if ((i > 40 && L_black[15] != 185 && (lefty[1] <= 20 || lefty[1] >= 45) && long_turn_flag > 30) || (i > 40 && L_black[5] != 185 && L_black[3] != 185 && L_black[7] != 185))
{
if(parameterB <= 0)
hd_state = 8; //退出状态机
}
if (hd_state != 8)
{
if (lefty[1] > 0 && lefty[1] < 45)
{
advanced_regression(1, lefty[1], lefty[1] + 2, lefty[1] + 2, lefty[1] + 4);
run(1, 0, lefty[1], parameterB, parameterA);
run(0, 0, lefty[1], parameterB, parameterA);
// SetText(" 状态 ： 回到赛道中 正常拉线 ");
last_guaiy = lefty[1];
}
hd_state = 7;
}
}
if (hd_state == 8)
{
find_middleline();
hd_flag1 = 0;
hd_num++;
hd_state = 0;
flag4 = 0;
hd_crossback_flag = 1; //开始出环岛的十字回环保护
hd3_potlinechange = 0;
// SetText(" 状态 ： 退出左环岛状态机 ");
}
if (hd_state != 2 && hd_state != 3 && hd_state != 5)
{
last_guaix = 0;
last_guaiy = 0;
}
if (hd_state != 4)
hdtime4 = 0;
if (hd_state != 2)
hdtime2 = 0;
if (hd_state == 2)
{
if (last_tubian_zhong_y == 0)
last_tubian_zhong_y = tubian_zhong_y;
else if (last_tubian_zhong_y > tubian_zhong_y)
last_tubian_zhong_y = tubian_zhong_y;
}
else
{
last_tubian_zhong_y = 0;
}
//SetText("xielv_last " + xielv_last + " xielv_now " + xielv_now + " 环岛状态 " + hd_state + " 左边连续 " + j);
xielv_last = xielv_now;
// SetText(" ************************************* 左环岛状态机 *************************************** ");
//SetText(" ————————————————————————————————————————————— ");
}
总结
大二这一年从省赛到国赛现场我也看到很多环岛翻车的情况(暗暗庆幸我的小车正式比赛时没在环岛翻车过)。环岛出来了这么久，各种写法也越来越多，也有加陀螺仪之类的进行辅助，我这里只是提供一个纯图像过环岛的写法，能力有限说的可能不是很详细，讲的不一定完全正确，欢迎大佬在评论区进行指点和改进。有什么问题可以在评论区留言或私信我！
最后的最后如果想看看我这一年智能车的经历和建议可以到第一篇文章最下面浏览一下 智能车学习日记【一】
4586
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
