stm32f4工 xun案例代码0巡线判断等

#include "track_deal.h"

u8 car_move_flag=0;

u8 uphill_flag=0;
u8 downhill_flag=0;

float set_car_speed_forward=0;

float car_speed_forward=0;
float spin_speed=0;

s32 car_distance=0;
s32 end_line_distance=0;
s32 unloading_distance=0;
s32 set_go_step_distance=0;

float track_data_min[8];
float track_data_max[8];
float track_data[8];

u8 black_line_status[8];

u8 black_horizontal_line_flag=0;
u8 lost_black_line_flag=0;
u8 black_line_valid=0;
u8 check_half_black_line_flag=0;
u8 half_black_line_flag=0;

float error;

//命令标志
u8 tracking_flag;//主循迹标志
u8 set_go_step_flag;//设定前进步数标志
u8 set_spin_step_flag;//设定旋转步数标志

u8 label;
u8 size;
u8 lighting=0;
u8 angle[3];

u32 pixels[2][6]={0}; 

materials_blob materials;

box box_mes[3];
//赛道参数初始化
void Track_Data_Init(void)
{
	u8 i;
	if(Read_AD_Max_Min()!=FR_OK)
	{
		for(i=0;i<8;i++)
		{
			track_data_min[i]=800;
			track_data_max[i]=2200;
		}
		LCD_ShowString(0,0,(u8 *)"Read AD Max Min Falid");
		R_LED(1);
		delay_ms(500);
		delay_ms(500);
		LCD_Clear(BLACK); 
	}
	if(Read_Pixels()!=FR_OK)
	{
		LCD_ShowString(0,0,(u8 *)"Read Pixels Falid");
		R_LED(1);
		delay_ms(500);
		delay_ms(500);
		LCD_Clear(BLACK); 
	}
	set_car_speed_forward=400;
	tracking_flag=1;
	set_go_step_flag=0;
	car_move_flag=1;
	car_speed_forward=set_car_speed_forward;
	box_mes[0].x1=loading_material_x1;
	box_mes[0].y1=loading_material_y1;
	box_mes[0].z1=loading_material_z1;
	box_mes[1].x1=loading_material_x2;
	box_mes[1].y1=loading_material_y2;
	box_mes[1].z1=loading_material_z2;
	box_mes[2].x1=loading_material_x3;
	box_mes[2].y1=loading_material_y3;
	box_mes[2].z1=loading_material_z3;
	
	box_mes[0].x2=unloading_material_x1;
	box_mes[0].y2=unloading_material_y1;
	box_mes[0].z2=unloading_material_z1;
	box_mes[1].x2=unloading_material_x2;
	box_mes[1].y2=unloading_material_y2;
	box_mes[1].z2=unloading_material_z2;
	box_mes[2].x2=unloading_material_x3;
	box_mes[2].y2=unloading_material_y3;
	box_mes[2].z2=unloading_material_z3;
}

//判断黑线状态
void Judge_Black_Line_Status(void)
{
	u8 i;
	u8 numb=0;
	u8 position[8];
	//判断8个循迹线
	for(i=0;i<8;i++)
	{
		if(track_data[i]>track_data_range*0.3) black_line_status[i]=0; 
		else black_line_status[i]=1;
	}
	//
	for(i=0;i<8;i++)
	{
		if(black_line_status[i]) 
		{
			position[numb]=i;
			numb++;
		}
	}
	//黑线数超过三个舍弃
	if(numb>3) 
	{
		black_line_valid=0;
		black_horizontal_line_flag=1;
	}
	else black_horizontal_line_flag=0;
	
	//丢线
	if(numb==0)
	{
		lost_black_line_flag=1;
	}
	else lost_black_line_flag=0;
	//判断黑线位置是否相邻
	if(numb==1 || \
		(numb==2 && position[0]+1==position[1]) || \
			(numb==3 && position[0]+1==position[1] && position[1]+1==position[2]))
	{
		black_line_valid=1;
	}
	else black_line_valid=0;
}
//归一化
void Track_Data_Normalization(void)
{
	u8 i;
	for(i=0;i<8;i++)
	{
		if(adc_data_fliter[i]>track_data_max[i]) track_data[i]=100;
		else if(adc_data_fliter[i]<track_data_min[i]) track_data[i]=0;
		else track_data[i]=(adc_data_fliter[i]-track_data_min[i])*track_data_range/(track_data_max[i]-track_data_min[i]);
	}
}
//差比和
float Difference_Divide_Sum(void)
{
	float difference,sum;
	difference=4*track_data[0]+3*track_data[1]+2*track_data[2]+track_data[3] \
	-track_data[4]-2*track_data[5]-3*track_data[6]-4*track_data[7];
	sum=track_data[0]+track_data[1]+track_data[2]+track_data[3] \
	+track_data[4]+track_data[5]+track_data[6]+track_data[7];
	if(sum) difference=100*difference/sum;
	else difference=0;
	if(difference>100) difference=100;
	else if(difference<-100) difference=-100;
  return (difference);
}
//单边线求偏差
float Half_Black_Line_Error(void)
{
	u8 i;
	for(i=0;i<4;i++)
	{
		if(black_line_status[i]) break;
	}
	return ((4-i)-1)*9;
}
//确认误差
void Confirm_Error(void)
{
	if(uphill_flag==1 || downhill_flag==1) error=Difference_Divide_Sum();//上下坡
	else if(check_half_black_line_flag) error=-Half_Black_Line_Error();
	else if(black_line_valid) error=Difference_Divide_Sum();
	else if(lost_black_line_flag) error=0;
	else error=0;
}
//中断赛道处理
void Track_Deal_Irq_20ms(void)
{
	Track_Data_Normalization();
	Judge_Black_Line_Status();
	Confirm_Error();
}