基于飞思卡尔单片机自动循迹小车控制的设计 - 图文(7)

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

FTM_PWM_Duty(FTM0,FTM_CH1,-left_count); FTM_PWM_Duty(FTM0,FTM_CH2,0); }

if(right_count>=0) {

FTM_PWM_Duty(FTM0,FTM_CH3,right_count); FTM_PWM_Duty(FTM0,FTM_CH4,0); } else {

FTM_PWM_Duty(FTM0,FTM_CH3,0);

FTM_PWM_Duty(FTM0,FTM_CH4,-right_count); }

gpio_turn(test1); push(0,2);

push(0,Left_Motor.setpulse); push(1,Left_Motor.OUT); push(2,Right_Motor.setpulse); push(3,Right_Motor.OUT); push(4,Left_Motor.backpulse); push(5,Right_Motor.backpulse); SendData2Scope();

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

uint16 MostWhite,white,i,MostWhitePoint; MostWhite=image[lens][C]*9; MostWhitePoint=C; for(i=10;i

white=image[lens][i] white+=image[lens][i-4]; white+=image[lens][i-3]; white+=image[lens][i-2]; white+=image[lens][i-1];

`

30

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

white+=image[lens][i+4]; white+=image[lens][i+3]; white+=image[lens][i+2]; white+=image[lens][i+1]; if(white>MostWhite) {

MostWhitePoint=i; MostWhite=white; } }

return MostWhitePoint; }

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

uint16 MostBlack,black,i,MostBlackPoint; MostBlack=image[lens][C]*9; MostBlackPoint=C; for(i=10;i

black=image[lens][i]; black+=image[lens][i-4]; black+=image[lens][i-3]; black+=image[lens][i-2]; black+=image[lens][i-1]; black+=image[lens][i+4]; black+=image[lens][i+3]; black+=image[lens][i+2]; black+=image[lens][i+1]; if(black

MostBlackPoint=i; MostBlack=black; } }

`

31

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

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