基于飞思卡尔单片机自动循迹小车控制的设计
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