📄 a.c
字号:
{
g_WidthError = g_width[k] - g_width[k2] + 6;
g_MinWidth = g_width[k] - g_WidthError;
if(g_MinWidth < 2)
{
g_MinWidth = 2;
}
g_MaxWidth = g_width[k] + g_WidthError + 1;
if(g_MaxWidth > 25)
{
g_MaxWidth = 25;
}
}
//根据黑线的连续性
ValidLineError = 12 * (k - n);
if(ValidLineError > 36)
{
ValidLineError = 36;
}
else
{
; //ValidLineError keep
}
if(g_BlackPositionCenter[k2] > g_BlackPositionCenter[k])
{
g_SearchDirection = 1;
g_error = ((g_BlackPositionCenter[k2] - g_BlackPositionCenter[k]) + ValidLineError);
g_SearchStart = g_BlackPositionCenter[k] - g_error - g_MaxWidth;
if((g_BlackPositionCenter[k2] - g_BlackPositionCenter[k]) > 6)
{
g_SearchEnd = g_BlackPositionCenter[k] + g_MaxWidth / 2;
}
else
{
g_SearchEnd = g_BlackPositionCenter[k] + g_error + g_MaxWidth;
}
if(g_SearchStart < 0)
{
g_SearchStart = 0;
}
if(g_SearchEnd >= MAX_VIDEO_POINT)
{
g_SearchEnd = MAX_VIDEO_POINT - 1;
}
}
else if(g_BlackPositionCenter[k] >= g_BlackPositionCenter[k2])
{
g_SearchDirection = 0;
g_error = ((g_BlackPositionCenter[k] - g_BlackPositionCenter[k2]) + ValidLineError);
g_SearchStart = g_BlackPositionCenter[k] + g_error + g_MaxWidth;
if((g_BlackPositionCenter[k] - g_BlackPositionCenter[k2]) > 6)
{
g_SearchEnd = g_BlackPositionCenter[k] - g_MaxWidth / 2;
}
else
{
g_SearchEnd = g_BlackPositionCenter[k] - g_error - g_MaxWidth;
}
if(g_SearchEnd < 0)
{
g_SearchEnd = 0;
}
if(g_SearchStart >= MAX_VIDEO_POINT)
{
g_SearchStart = MAX_VIDEO_POINT - 1;
}
}
/* else if(g_BlackPositionCenter[k] == g_BlackPositionCenter[k2])
{
g_SearchDirection = 1; //left 扫描方向从左至右
g_SearchStart = g_BlackPositionCenter[k] - 10;
g_SearchEnd = g_BlackPositionCenter[k] + 10;
if(g_SearchStart < 0)
{
g_SearchStart = 0;
}
if(g_SearchEnd >= MAX_VIDEO_POINT)
{
g_SearchEnd = MAX_VIDEO_POINT - 1;
}
}
else
{
;
}
*/
cnt = 0;
if(0 == g_SearchDirection)
{
m = g_SearchStart;
while((m > g_SearchEnd) && (cnt < MAX_BLACK_NUM))
{
if(g_VideoImageDate[n][m] < MIDDLE)
{
l_BlackStartDot = m;
while((g_VideoImageDate[n][m - 1] < MIDDLE) && (m > g_SearchEnd))
{
m --;
}
if(((l_BlackStartDot - m + 1) < g_MaxWidth) && ((l_BlackStartDot - m + 1) >= g_MinWidth))
{
g_BlackPoint[0][cnt] = (l_BlackStartDot + m) / 2;
g_BlackPoint[1][cnt] = l_BlackStartDot - m + 1;
cnt ++;
}
else
{
;
}
if(m > 1)
{
m -= 2;
}
else
{
m = 0;
}
}
else
{
m --;
}
}
}
else if(1 == g_SearchDirection)
{
m = g_SearchStart;
while((m < g_SearchEnd) && (cnt < MAX_BLACK_NUM))
{
if(g_VideoImageDate[n][m] < MIDDLE)
{
l_BlackStartDot = m;
while((g_VideoImageDate[n][m + 1] < MIDDLE) && (m < g_SearchEnd))
{
m ++;
}
if(((m - l_BlackStartDot + 1) < g_MaxWidth) && ((m - l_BlackStartDot + 1) >= g_MinWidth))
{
g_BlackPoint[0][cnt] = (l_BlackStartDot + m) / 2;
g_BlackPoint[1][cnt] = m - l_BlackStartDot + 1;
cnt ++;
}
else
{
;
}
m += 2; //继续扫描
}
else
{
m ++ ;
}
}
}
for(k2 = 0; k2 < MAX_BLACK_NUM; k2 ++)
{
if((g_BlackPoint[0][k2] < (g_BlackPositionCenter[k] + g_error)) && (g_BlackPoint[0][k2] > (g_BlackPositionCenter[k] - g_error)))
{
g_BlackPositionCenter[n] = g_BlackPoint[0][k2];
g_width[n] = g_BlackPoint[1][k2];
k2 = MAX_BLACK_NUM;
}
else
{
;// do nothing
}
}
if(((g_BlackPositionCenter[k] < 10) || (g_BlackPositionCenter[k] >= 110)) && ((k - n) > 3))
{
g_BlackPositionCenter[n] = 180;
g_width[n] = 0;
n = -1;
}
}
}
else
{
;
}
//==================计算有效行中心===================
g_ValidLine = 0;
g_BlackLineTotal = 0;
g_DirectionControlLine = 0;
for(n = (MAX_VIDEO_LINE - 1); n >= 0; n --)
{
//SCI0SendByte(g_BlackPositionCenter[n]);
if(g_BlackPositionCenter[n] != 180)
{
if(0 == g_ValidLine)
{
g_DirectionControl = 0;
}
g_ValidLine ++;
g_BlackLineTotal += g_BlackPositionCenter[n];
if(g_ValidLine <= 26)
{
g_DirectionControl += g_BlackPositionCenter[n];
}
}
if(n == MAX_VIDEO_LINE - 10)
{
if(g_ValidLine != 0)
{
g_BottomMiddle = g_BlackLineTotal / g_ValidLine;
}
}
else
{
;
}
}
if(g_ValidLine == 0)
{
if(g_BlackMiddle[0] < 20) //判断上一有效场黑线中心靠左还是靠右
{
g_BlackMiddle[1] = 0;
g_BlackMiddle[0] = 1;
g_DirectionControl = 0;
}
else if(g_BlackMiddle[0] >= 100)
{
g_BlackMiddle[1] = 119;
g_BlackMiddle[0] = 118;
g_DirectionControl = 119;
}
else
{
g_BlackMiddle[1] = g_BlackMiddle[0];
}
}
else
{
g_BlackMiddle[1] = g_BlackLineTotal / g_ValidLine;
g_BlackMiddle[0] = g_BlackMiddle[1];
g_DirectionControl /= g_DirectionControlLine;
}
Speed_Adjust = 0;//(MAX_VIDEO_LINE - g_ValidLine)/3;
/****舵机转角PID计算*****/
if(g_BlackMiddle[1] < 0)
{
g_BlackMiddle[1] = 0;
}
else if(g_BlackMiddle[1] > 119)
{
g_BlackMiddle[1] = 119;
}
else
{
;
}
sPID.loca_FeedBack = g_DirectionControl;
PWMDTY67 = 1455 + loca_PIDCalc(&sPID);
/****驱动电机速度PID计算****/
//确定速度参考值
if(g_DirectionControl < 60)
{
sPID.vi_Ref = (g_HighestSpeed * 3481 - ((59 - g_DirectionControl) * (59 - g_DirectionControl) * (g_HighestSpeed - g_BasicSpeed))) / 3481 - Speed_Adjust;
}
else if(g_DirectionControl >= 60)
{
sPID.vi_Ref = (g_HighestSpeed * 3600 - ((g_DirectionControl - 60) * (g_DirectionControl - 60) * (g_HighestSpeed - g_BasicSpeed)) )/ 3600 - Speed_Adjust;
}
else
{
;//sPID.vi_Ref = g_HighestSpeed;
}
//根据中心位置确定驱动电机转速，直道加速，弯道减速
l_pulse = PACN10;
//SCI0SendByte(l_pulse&0xFF);
//SCI0SendByte((l_pulse>>8)&0xFF);
PACN10 = 0;
sPID.vi_FeedBack = l_pulse;
l_tempPWMDTY45 = (500 + V_PIDCalc(&sPID));
PWMDTY45 = l_tempPWMDTY45;
MovementAdjust = OFF;
}
INTCR_IRQEN = ON;
}
}
else
{
//None
}
}
#pragma CODE_SEG DEFAULT
/* Change code section to DEFAULT. */
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -