智能车整理日记【六】————常用的辅助函数
文章目录
- 前言
-
-
-
- 速度环小模糊
- 求斜率截距拟合直线
- 求方差
- 求线连续数
- 求赛道宽度
- 拟线
- 动态找最高中线
-
-
- 总结
前言
放一些我的常用的辅助函数，有些是自己写的，有些是看到其他博主上有好用的就拿来进行一点修改，也有来自队友和学长的。欢迎大家提出意见和修改！ 一些数组和参数定义在前几篇文章有说明，如果觉得这篇文章有帮助就点个赞支持一下这个小白博主吧！
速度环小模糊
float Update_kp_Speed(int error, int absmin, int absmax)
{
if(abs(error) < absmin) return kpzengliang;
else if(abs(error) > absmax) return 0;
else return kpzengliang*(absmax-abs(error))/(absmax-absmin);
}
————————————————————————————————
求斜率截距拟合直线
void advanced_regression(int type, int startline1, int endline1, int startline2, int endline2)
{
int i = 0;
int sumlines1 = endline1 - startline1;
int sumlines2 = endline2 - startline2;
int sumX = 0;
int sumY = 0;
float averageX = 0;
float averageY = 0;
float sumUp = 0;
float sumDown = 0;
if (type == 0) //拟合中线
{
/**计算sumX sumY**/
for (i = startline1; i < endline1; i++)
{
sumX += i;
sumY += LCenter[i];
}
for (i = startline2; i < endline2; i++)
{
sumX += i;
sumY += LCenter[i];
}
if (sumlines1 + sumlines2 != 0)
{
averageX = sumX / (sumlines1 + sumlines2); //x的平均值
averageY = sumY / (sumlines1 + sumlines2); //y的平均值
}
for (i = startline1; i < endline1; i++)
{
sumUp += (LCenter[i] - averageY) * (i - averageX);
sumDown += (i - averageX) * (i - averageX);
}
for (i = startline2; i < endline2; i++)
{
sumUp += (LCenter[i] - averageY) * (i - averageX);
sumDown += (i - averageX) * (i - averageX);
}
if (sumDown == 0) parameterB = 0;
else parameterB = sumUp / sumDown;
parameterA = averageY - parameterB * averageX;
}
else if (type == 1) //拟合左线
{
/**计算sumX sumY**/
for (i = startline1; i < endline1; i++)
{
sumX += i;
sumY += L_black[i];
}
for (i = startline2; i < endline2; i++)
{
sumX += i;
sumY += L_black[i];
}
if (sumlines1 + sumlines2 != 0)
{
averageX = sumX / (sumlines1 + sumlines2); //x的平均值
averageY = sumY / (sumlines1 + sumlines2); //y的平均值
}
for (i = startline1; i < endline1; i++)
{
sumUp += (L_black[i] - averageY) * (i - averageX);
sumDown += (i - averageX) * (i - averageX);
}
for (i = startline2; i < endline2; i++)
{
sumUp += (L_black[i] - averageY) * (i - averageX);
sumDown += (i - averageX) * (i - averageX);
}
if (sumDown == 0) parameterB = 0;
else parameterB = sumUp / sumDown;
parameterA = averageY - parameterB * averageX;
}
else if (type == 2) //拟合右线
{
/**计算sumX sumY**/
for (i = startline1; i < endline1; i++)
{
sumX += i;
sumY += R_black[i];
}
for (i = startline2; i < endline2; i++)
{
sumX += i;
sumY += R_black[i];
}
if (sumlines1 + sumlines2 != 0)
{
averageX = sumX / (sumlines1 + sumlines2); //x的平均值
averageY = sumY / (sumlines1 + sumlines2); //y的平均值
}
for (i = startline1; i < endline1; i++)
{
sumUp += (R_black[i] - averageY) * (i - averageX);
sumDown += (i - averageX) * (i - averageX);
}
for (i = startline2; i < endline2; i++)
{
sumUp += (R_black[i] - averageY) * (i - averageX);
sumDown += (i - averageX) * (i - averageX);
}
if (sumDown == 0) parameterB = 0;
else parameterB = sumUp / sumDown;
parameterA = averageY - parameterB * averageX;
}
}
—————————————————————————————————
求方差
byte R_Start[70];
byte L_Start[70];
void shit(int type, int startline, int endline)
{
if (type == 2)
{
int delta = startline - endline;
if (delta == 0) delta = 1;
float k = (R_black[startline] - R_black[endline]) * 1.0f / delta;
float b = R_black[startline] - k * startline;
for (int j = (byte)startline; j <= (byte)endline; j++)
{
int jicun = ((int)(k * j + b));
if (jicun >= 185) jicun = 185;
else if (jicun <= 0) jicun = 0;
R_Start[j] = (byte)jicun;
//纠正右线
}
}
if (type == 1)
{
int delta = startline - endline;
if (delta == 0) delta = 1;
float k = (L_black[startline] - L_black[endline]) * 1.0f / delta;
float b = L_black[startline] - k * startline;
for (int j = (byte)startline; j <= (byte)endline; j++)
{
int jicun = ((int)(k * j + b));
if (jicun >= 185) jicun = 185;
else if (jicun <= 0) jicun = 0;
L_Start[j] = (byte)jicun;
//纠正右线
}
}
}
float sumE = 0;
float fangcha(int type, int startline, int endline)
{
int i = 0;
int sumlines = endline - startline;
int sum = 0;
float average = 0;
if (type == 0) //拟合中线
{
for (i = startline; i <= endline; i++)
{
sum += LCenter[i];
}
if (sumlines != 0)
{
average = (float)sum / sumlines; //x的平均值
}
else
{
average = 0; //x的平均值 //y的平均值
}
for (i = startline; i <= endline; i++)
{
sumE += (LCenter[i] - average) * (LCenter[i] - average);
}
if (sumlines != 0)
{
sumE = (sumE) / sumlines;
return sumE;
}
else sumE = 0;
}
else if (type == 1)//拟合左线
{
shit(1, startline, endline);
for (i = startline; i <= endline; i++)
{
sum += (L_black[i] - L_Start[i]) * (L_black[i] - L_Start[i]);
}
if (sumlines != 0)
{
sumE = (sum) / sumlines;
return sumE;
}
else sumE = 0;
}
else if (type == 2)//拟合右线
{
shit(2, startline, endline);
for (i = startline; i <= endline; i++)
{
sum += (R_black[i] - R_Start[i]) * (R_black[i] - R_Start[i]);
}
if (sumlines != 0)
{
sumE = (sum) / sumlines;
return sumE;
}
else sumE = 0;
}
return 0;
}
—————————————————————————————————
求线连续数
int long_turn_flag_left = 0; // 左线连续数
int long_turn_flag_right = 0; //右线连续数
int long_turn_flag = 0; //中线连续
void juge_lineContinuity(byte start_point, byte end_point, int positive_T, int negatie_T, byte ArryName)
{
byte j;
int center_delta;
int left_delta;
int right_delta;
switch (ArryName)
{
case 0: //CENTER
{
for (j = start_point; j < end_point; j++)//从第10行开始，防止下面提早断掉，影响判断
{
center_delta = LCenter[j + 1] - LCenter[j]; //left线的偏差
if (center_delta >= positive_T || center_delta <= negatie_T)
{
long_turn_flag = j;
break;
}
else
{
if (j >= end_point - 1)
{
long_turn_flag = end_point;
}
}
}
}
break;
case 1: //LEFT
{
for (j = start_point; j < end_point; j++)//从第10行开始，防止下面提早断掉，影响判断
{
left_delta = L_black[j + 1] - L_black[j]; //left线的偏差
if (left_delta >= positive_T || left_delta <= negatie_T)
{
long_turn_flag_left = j;
break;
}
else
{
if (j >= end_point - 1)
{
long_turn_flag_left = end_point;
}
}
}
}
break;
case 2: //RIGHT
{
for (j = start_point; j < end_point; j++)//从第10行开始，防止下面提早断掉，影响判断
{
right_delta = R_black[j + 1] - R_black[j]; //left线的偏差
if (right_delta >= positive_T || right_delta <= negatie_T)
{
long_turn_flag_right = j;
break;
}
else
{
if (j >= end_point - 1)
{
long_turn_flag_right = end_point;
}
}
}
}
break;
default:
{
for (j = start_point; j < end_point; j++)//从第10行开始，防止下面提早断掉，影响判断
{
center_delta = LCenter[j + 1] - LCenter[j]; //left线的偏差
//大于偏差，break，记录break点
if (center_delta >= positive_T || center_delta <= negatie_T)
{
long_turn_flag = j;
break;
}
//小于偏差，若是大于end点，则记录end点，否则不做操作
else
{
if (j >= end_point - 1)
{
long_turn_flag = end_point;
}
}
}
}
break;
}
}
—————————————————————————————————
求赛道宽度
int fiv_width[70];
void line_width()
{
byte i;
for (i = 0; i < b; i++)
{
fiv_width[i] = L_black[i] - R_black[i];
}
}
—————————————————————————————————
拟线
void run(int type, int startline, int endline, float parameterB, float parameterA)
{
int z=0;
if (type == 1) //左
{
for (int i = startline; i < endline; i++)
{
z = (int)(parameterB * i + parameterA);
if (z > 185)
{
z = 185;
}
if (z < 0)
{
z = 185;
}
L_black[i] = (byte)(z);
}
}
else if (type == 2) //右
{
for (int i = startline; i < endline; i++)
{
z = (int)(parameterB * i + parameterA);
if (z > 185)
{
z = 0;
}
if (z < 0)
{
z = 0;
}
R_black[i] = (byte)(z);
}
}
else if (type == 0) //中
{
for (int i = startline; i < endline; i++)
{
LCenter[i] = (byte)((L_black[i] / 2 + R_black[i] / 2));
if (LCenter[i] < 0)
{
LCenter[i] = 0;
}
if (LCenter[i] > 185)
{
LCenter[i] = 185;
}
}
}
else if (type == 3) //左辅助线
{
for (int i = startline; i < endline; i++)
{
z = (int)(parameterB * i + parameterA);
if (z > 185)
{
z = 185;
}
if (z < 0)
{
z = 185;
}
L_Start[i] = (byte)(z);
}
}
else if (type == 4) //右辅助线
{
for (int i = startline; i < endline; i++)
{
z = (int)(parameterB * i + parameterA);
if (z > 185)
{
z = 0;
}
if (z < 0)
{
z = 0;
}
R_Start[i] = (byte)(z);
}
}
else if (type == 5) //左辅助线和右线拉中线
{
for (int i = startline; i < endline; i++)
{
LCenter[i] = (byte)((L_Start[i] / 2 + R_black[i] / 2));
if (LCenter[i] < 0)
{
LCenter[i] = 0;
}
if (LCenter[i] > 185)
{
LCenter[i] = 185;
}
}
}
else if (type == 6) //左线和右辅助线拉中线
{
for (int i = startline; i < endline; i++)
{
LCenter[i] = (byte)((L_black[i] / 2 + R_Start[i] / 2));
if (LCenter[i] < 0)
{
LCenter[i] = 0;
}
if (LCenter[i] > 185)
{
LCenter[i] = 185;
}
}
}
}
————————————————————————————————
动态找最高中线
void hd2_findmiddle()
{
byte max_X = 0;
byte max_Y = 0;
for (x = R_black[0]; x < L_black[0]; x++) //找一条中线进行左右扫线
{
for (y = 1; y < 70; y++)
{
if (y == 69 || (Pixels[y - 1][x] != 0 && Pixels[y][x] == 0))
{
if (y > max_Y)
{
max_Y = y;
max_X = x;
break;
}
}
if (Pixels[y - 1][x] == 0 && Pixels[y][x] == 0)
{
break;
}
}
}
if (max_X == 0)
max_X = 1;
if (max_X == 185)
max_X = 184;
// SetText("max_Y=" + max_Y + " max_X=" + max_X + " 根据中线x=" + max_X + "拉线");
for (y = 1; y < 70; y++)
{
if (Pixels[y][max_X] == 0 || y == 69)
{
b = y;
break;
// SetText("新丢线行b=" + b);
}
for (x = max_X; 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;
}
}
for (x = max_X; x >= 0; x--)
{
if (x == 0 || (Pixels[y][x] != 0 && Pixels[y][x - 1] == 0 && Pixels[y][x + 1] != 0)) //右边界
{
R_black[y] = x;
break;
}
}
LCenter[y] = (byte)(R_black[y] / 2 + L_black[y] / 2);
}
}
总结
这些是我使用频率比较高的辅助函数，希望对后面的智能车er们有所帮助。如果你觉得文章有所帮助还希望点赞鼓励一下！欢迎大家在评论区提出改进意见！
最后的最后如果想看看我这一年智能车的经历和小小建议可以到第一篇文章最下面浏览一下 智能车学习日记【一】