stm32f4工xun案例代码04 循迹整个地图以及小车控制

//避障
void Collision_Avoidance(void)
{
	Set_Go_Step(300,300);
	while(!Check_Go_Step(80));
	tracking_flag=0;
	car_speed_forward=0;
//	delay_ms(400);
	Set_Spin_Step(90,800);
	while(!Check_Spin_Step(5));
	set_spin_step_flag=1;
	Set_Go_Step(500,800);
	while(!Check_Go_Step(80));
//	delay_ms(400);
	Set_Spin_Step(-90,800);
	while(!Check_Spin_Step(5));
	set_spin_step_flag=1;
	Set_Go_Step(1400,800);
	while(!Check_Go_Step(80));
//	delay_ms(400);
	Set_Spin_Step(-90,800);
	while(!Check_Spin_Step(5));
	set_spin_step_flag=1;
	car_speed_forward=200;
	car_distance=0;
	while(!black_horizontal_line_flag && car_distance<600*pulse_per_millimeter);
	Set_Go_Step(200,300);
	while(!Check_Go_Step(80));
	delay_ms(200);
	Set_Spin_Step(90,800);
	while(!Check_Spin_Step(5));
	tracking_flag=1;
}
void Track_Element_Init(void)
{
	R_LED(1);
	car_move_flag=0;
	while(!key_sign[2] && !SW4);
	car_move_flag=1;
	key_sign[2]=0;
	R_LED(0);
	car_distance=0;
	yaw=0;
}
//循环赛道处理
void Track_Deal_Loop(void)
{
	car_move_flag=0;
	key_sign[2]=0;
	while(!key_sign[2]);
	key_sign[2]=0;
	Direction_Pid_Parameter_Init(0,9,2);
	Track_Element_Init();
	set_car_speed_forward=400;
	//变形
	while(1)
	{
		if(car_distance>300*pulse_per_millimeter)
		{
			Three_Dimensional_Move_Arm(x_grap_0,y_grap_0,arm_z,0,90,200);
			Three_Dimensional_Move_Arm(arm_x,arm_y,z_grap_0,0,90,200);
			break;
		}
	}
	//转第一个弯
	Track_Element_Init();
	while(1)
	{
		if(yaw>45 && black_horizontal_line_flag)
		{
			break;
		}
	}
	//上坡
	Direction_Pid_Parameter_Init(0,1,1);
	Track_Element_Init();
	SERIAL_MP3_PLAY_R_INDEX(1);
	while(1)
	{
		//角度检测
		if(roll<-11 && uphill_flag==0) 
		{
			uphill_flag=1;
		}
		else if(roll>-10 && uphill_flag==1) uphill_flag=2;
		if(car_distance>300*pulse_per_millimeter && uphill_flag==2)
		{
			break;
		}
		
		if(uphill_flag==1)
		{
			R_LED(1);
			G_LED(1);
			B_LED(1);
		}
	}
	//上料
	check_half_black_line_flag=1;
	set_car_speed_forward=100;
	Direction_Pid_Parameter_Init(0,9,2);
	
	Track_Element_Init();
	SERIAL_MP3_PLAY_R_INDEX(2);
	while(1)
	{
		if(Read_Right_Rled && car_distance>200*pulse_per_millimeter)
		{
			Loading_Material();
			break;
		}
		
		if(half_black_line_flag) 
		{
			R_LED(1);
			G_LED(0);
			B_LED(1);
		}
	}
	//下坡
	set_car_speed_forward=400;
	Direction_Pid_Parameter_Init(0,2,0);
	
	Track_Element_Init();
	while(1)
	{
		//角度检测
		if(roll>8 && downhill_flag==0) 
		{
			downhill_flag=1;
		}
		else if(roll<7 && downhill_flag==1) 
		{
			downhill_flag=2;
			car_distance=0;
		}
		if(car_distance>200*pulse_per_millimeter && downhill_flag==2)
		{
			break;
		}
		
		if(downhill_flag==1)
		{
			R_LED(1);
			G_LED(1);
			B_LED(1);
		}
		else if(half_black_line_flag) 
		{
			R_LED(1);
			G_LED(0);
			B_LED(1);
		}
	}
	//转弯
	Direction_Pid_Parameter_Init(0,9,2);
	
	Track_Element_Init();
	while(1)
	{
		if(car_distance>300*pulse_per_millimeter && yaw>45 && black_horizontal_line_flag)
		{
			break;
		}
	}
	//障碍
	Track_Element_Init();
	SERIAL_MP3_PLAY_R_INDEX(3);
	
	Collision_Avoidance();
	while(1)
	{
		
		if(yaw>70)
		{
			if(car_distance>100*pulse_per_millimeter) break;
		}
		else car_distance=0;
	}
	//下料
	set_car_speed_forward=200;
	check_half_black_line_flag=1;
	
	Track_Element_Init();
	SERIAL_MP3_PLAY_R_INDEX(4);
	while(1)
	{
		if(Read_Right_Rled)
		{
			unloading_distance=0;
			SERIAL_MP3_PLAY_R_INDEX(5);
			Unloading_Material();
			break;
		}
		
		if(half_black_line_flag) 
		{
			R_LED(1);
			G_LED(0);
			B_LED(1);
		}
	}
	//走完下料区
	set_car_speed_forward=400;
	
	Track_Element_Init();
	while(1)
	{
		if(car_distance>600*pulse_per_millimeter || yaw>15 || unloading_distance>900*pulse_per_millimeter)
		{
			break;
		}
		
		if(half_black_line_flag) 
		{
			R_LED(1);
			G_LED(0);
			B_LED(1);
		}
	}
	//终点线检测
	check_half_black_line_flag=0;
	
	Track_Element_Init();
	while(1)
	{
		if(lost_black_line_flag)
		{
			if(end_line_distance>400*pulse_per_millimeter) 
			{
				car_move_flag=0;
				SERIAL_MP3_PLAY_R_INDEX(6);
			}
		}
		else end_line_distance=0;
	}

	while(1);
}

void Car_Move_Control(void)
{
	float set_forward_speed=0;
	float set_spin_speed=0;
	float car_speed_max;
	if(car_move_flag)
	{
		if(set_go_step_flag)
		{
			Set_Go_Step_Pid_Calculate(set_go_step_distance);
			car_speed_forward=set_go_step_pid_uk;
		}
	  if(tracking_flag)
	  {
			if(!set_go_step_flag) 
			{
				car_speed_max=set_car_speed_forward-S2(error)*0.02f;
				if(car_speed_max<50) car_speed_max=50;
				if(car_speed_forward<car_speed_max) car_speed_forward+=8;
				else car_speed_forward=car_speed_max;
			}
			Direction_Pid_Calculate(error,-z_angular_speed);
			set_spin_speed=-direction_pid_uk;
		}
		if(set_spin_step_flag)
		{
			Set_Spin_Step_Pid_Calculate(yaw);
			set_spin_speed=-set_spin_step_pid_uk;
		}
		set_forward_speed=car_speed_forward;
		if((set_forward_speed>0 && tracking_flag) || !tracking_flag) Mecanum_Wheel_Motor_Calculate(set_forward_speed,0,set_spin_speed);//只有前进时循迹
		else Mecanum_Wheel_Motor_Calculate(set_forward_speed,0,0);
	}
	else 
	{
		Mecanum_Wheel_Motor_Calculate(0,0,0);
	}
}