【正文】 r = position_buffer 。speed_adjust()。bai_tou_judgement() 。// scan_judgement() 。// start_scan_flag = TRUE 。// road_get_flag = TRUE 。}void slope_judgement(void){int i_slope,t_slope=0。static int T[21] 。for(i_slope=0。i_slope20。i_slope++){T[i_slope]=T[i_slope+1]。}T[20] = tri。for(i_slope=0。i_slopeslope_i。i_slope++) 11 {if(T[20i_slope]=slope_point)t_slope+=1。}if(t_slope==slope_i amp。amp。 (!slope_flag)){slope_flag=1。slope_delay_flag = 1。if((ir_delay_flagamp。amp。num_of_line0)||on_line_flag){num_of_line=1。}}t_slope=0。for(i_slope=0。i_slope7。i_slope++){if(T[20i_slope]=2amp。amp。T[20i_slope]0)t_slope+=1。}if(/*t_slope==7 amp。amp。*/ slope_delay_flag==0){if(slope_flag==1){ir_delay_flag = 5。}slope_flag=0。}}void bai_tou_judgement(void){int laser_temp。static int bai_t = 0。bai_t++。if(bai_t=3){bai_t = 0。if(slope_flag==FALSE){if(!no_bai_flag){// if(now_pt=60)laser_temp = (int)BAI_TOU +laser_engine_pwm_1[position_buffer]。// else// laser_temp += laser_engine_pwm_2[posit]。if(laser_temp bai_tou_pwm_min){laser_temp = bai_tou_pwm_min。}else if(laser_temp bai_tou_pwm_max){laser_temp = bai_tou_pwm_max。}BAI_TOU = (word)laser_temp。}}else{BAI_TOU = (word)bai_tou_pwm_center 。}}}void speed_adjust(void){int sp_dang。sp_dang = abs(position)/10。//abs(BAI_TOUbai_tou_pwm_center)/10。if(sp_dang=NUM_OF_SPEED_DANG){sp_dang=NUM_OF_SPEED_DANG1。}if(!slope_flag)speed_ept = speed_tab[sp_dang] 。elsespeed_ept = slope_speed 。}void speed_col(void){int pwm_temp 。static int before_speed = 0。now_speed = (int)PACNT 。PACNT = 0。// if(now_speed=90){pwm_temp = (int)SPEED_F (int)SPEED_B 。if(speed_eptnow_speed){if(now_speed before_speed){pwm_temp = pwm_temp + abs(speed_eptnow_speed)*2/3 +abs(now_speedbefore_speed)/150 。}else{pwm_temp = pwm_temp + abs(speed_eptnow_speed)*2/3 abs(now_speedbefore_speed)/150 。}}else{if(now_speedbefore_speed){pwm_temp = pwm_temp abs(speed_eptnow_speed)*2/3 +abs(now_speedbefore_speed)/150 。}else{pwm_temp = pwm_temp abs(speed_eptnow_speed)*2/3 abs(now_speedbefore_speed)/150 。}}if(pwm_temp255){SPEED_F = 255。SPEED_B = 0。}else if(pwm_temp255){SPEED_F = 0。SPEED_B = 255。}else{if(pwm_temp=0){SPEED_F = (byte)pwm_temp。SPEED_B = 0。}else{SPEED_F = 0。SPEED_B = (byte)(pwm_temp)。}}}}void get_road_in(void){byte i 。road_in = 0 。tri = 0 。for(i=0。 iNUM_OF_LASER。 i++){if(laser_ad_val[i] argue[i]){road_in |= ((word)0x1i) 。tri++ 。}}} 15 void lost_judgement(void){int i_lost,t_lost=0。static word R[10] ,lost_state=0。for(i_lost=0。i_lost9。i_lost++){R[i_lost]=R[i_lost+1]。}R[9] = road_in 。for(i_lost=0。i_lost3。i_lost++){if(R[9i_lost]==0)t_lost+=1。}if( t_lost==3 amp。amp。((position_buffer==0||position_buffer==1)amp。amp。BAI_TOU==bai_tou_pwm_max) ){lost_flag=1。lost_state = 0 。}if( t_lost==3 amp。amp。((position_buffer==NUM_OF_LASER2||position_buffer==NUM_OF_LASER1)amp。amp。BAI_TOU==bai_tou_pwm_min) ){lost_flag=1。lost_state = 1 。}t_lost=0。for(i_lost=0。i_lost5。i_lost++){if(R[9i_lost]!=0)t_lost+=1。}if( t_lost==5 amp。amp。((position_buffer==0||position_buffer==1)amp。amp。BAI_TOU==bai_tou_pwm_max) amp。amp。lost_state==0)lost_flag=0。if( t_lost==5 amp。amp。 ((position_buffer==NUM_OF_LASER2||position_buffer==NUM_OF_LASER1)amp。amp。BAI_TOU==bai_tou_pwm_min) amp。amp。 lost_state==1)lost_flag=0。}void straight_judgement(void){if(abs(position)=430){if(t_straight20)t_straight++ 。if(t_straight=20){straight_flag = 1 。}}else{t_straight=0。straight_flag = 0 。}}void long_straight_judgement(void){static int l_s_t = 0 。if(abs(position)=8 amp。amp。 straight_flag==1 amp。amp。 long_straight_flag==0){l_s_t++。if(l_s_t=90){l_s_t=0。long_straight_flag = 1。}}else if(straight_flag==0){long_straight_flag = 0。} 17 }void half_straight_judgement(void){if(!straight_flag){if(half_straight_t(half_straight_speed_limitsept)){half_straight_t++。}}else{half_straight_t = 0。}}void position_judgement(void){static int error_t = 0 。static int no_bai_t = 0。get_road_in() 。 //獲取轉(zhuǎn)換激光向量數(shù)據(jù)//~.21 對(duì)應(yīng)激光管左右switch(road_in){//一個(gè)點(diǎn)case ((word)0x10) : position_buffer = 0 。 break 。case ((word)0x11) : position_buffer = 1 。 break 。case ((word)0x12) : position_buffer = 2 。 break 。case ((word)0x13) : position_buffer = 3 。 break 。case ((word)0x14) : position_buffer = 4 。 break 。case ((word)0x15) : position_buffer = 5 。 break 。case ((word)0x16) : position_buffer = 6 。 break 。case ((word)0x17) : position_buffer = 7 。 break 。case ((word)0x18) : position_buffer = 8 。 break 。case ((word)0x19) : position_buffer = 9 。 break 。case ((word)0x110) : position_buffer = 10 。 break 。case ((word)0x111) : position_buffer = 11 。 break 。case ((word)0x112) : position_buffer = 12 。 break 。case ((word)0x113) : position_buffer = 13 。 break 。//兩個(gè)點(diǎn)case ((word)0x30) : position_buffer = 0 。 break 。case ((word)0x31) : position_buffer = 1 。 break 。case ((word)0x32) : position_buffer = 2 。 break 。case ((word)0x33) : position_buffer = 3 。 break 。case ((word)0x34) : position_buffer = 4 。 break 。case ((word)0x35) : position_buffer = 5 。 break 。case ((word)0x36) : position_buffer = 6 。 break 。case ((word)0x37) : position_buffer = 8 。 break 。case ((word)0x38) : position_buffer = 9 。 break 。case ((word)0x39) : position_buffer = 10 。 break 。case ((word)0x310) : position_buffer = 11 。 break 。case ((word)0x311) : position_buffer = 12 。 break 。case ((word)0x312) : position_buffer = 13 。 break 。//三個(gè)點(diǎn)case ((word)0x70) : position_buffer = 1 。 break 。case ((word)0x71) : position_buffer = 2 。 break 。case ((word)0x72) : position_buffer = 3 。 break 。case