基于飞思卡尔单片机自动循迹小车控制的设计

基于飞思卡尔单片机自动循迹小车控制的设计

return MostBlackPoint; }

/***************寻找最白的部分***********************/ uint16 SeekCenterSingle3(Image image,uint8 lens) {

uint16 i, white,BestWhite=0, BestWhitePoint; BestWhite = image[lens][65]; for(i=30; i

white=image[lens][i];

white+=image[lens][i-9]; white+=image[lens][i-7]; white+=image[lens][i-5]; white+=image[lens][i-3]; white+=image[lens][i+9]; white+=image[lens][i+7]; white+=image[lens][i+5]; white+=image[lens][i+3]; if(BestWhite

BestWhite=white; BestWhitePoint=i; } }

return(BestWhitePoint); }

uint16 SeekCenterSingle2(Image image,uint8 lens) {

uint16 i, white,BestWhite=0, BestWhitePoint; BestWhite = image[lens][65]; for(i=30; i

white = image[lens][i-28];

white += image[lens][i-21]; white += image[lens][i-14];

`

32

基于飞思卡尔单片机自动循迹小车控制的设计

white += image[lens][i-7]; white += image[lens][i]; white += image[lens][i+7]; white += image[lens][i+14]; white += image[lens][i+21]; white += image[lens][i+28]; if(BestWhite

BestWhite = white; BestWhitePoint = i; } }

return(BestWhitePoint); }

/********************************计算车的偏差**********************/ void CenterFarRow() {

short FarRow;

FarRow=Row-ValidRow; NearRow=FarRow; if(SquareFlag==0) {

while(FarRow>1) //切弯 {

if(JudgeValid(NearRow)) {

STEERING.setpulse=(short)(Gen*atanf((float)S1/(float)Row2CarDISTANCE[NearRow]));//舵机的偏差

if(abs(STEERING.setpulse)>20)

{ Left_Motor.setpulse=(short)(280+ValidRow+(short)((float)(STEERING.setpulse)*SpeedC)); //差速控制

Right_Motor.setpulse=(short)(280+ValidRow-(short)((float)(STEERING.setpulse)*SpeedC));

`

33

基于飞思卡尔单片机自动循迹小车控制的设计

} else {

Left_Motor.setpulse=280+2*ValidRow; Right_Motor.setpulse=280+2*ValidRow; } return; }

NearRow++; if(NearRow>58) { break; } }

Calculate();

if(abs(STEERING.setpulse)>20) {

Left_Motor.setpulse=420+3*ValidRow-5*abs(STEERING.setpulse)+(short)((float)(STEERING.setpulse)*SpeedC);

Right_Motor.setpulse=420+3*ValidRow-5*abs(STEERING.setpulse)-(short)((float)(STEERING.setpulse)*SpeedC); } else {

Left_Motor.setpulse=480+3*ValidRow-5*abs(STEERING.setpulse); Right_Motor.setpulse=480+3*ValidRow-5*abs(STEERING.setpulse); } } else {

SquareSeek(); } }

`

34