STM32小车避障程序代码 - 智能车制作

/*
u16 LeftDistance, RightDistance;
ili9320_DisplayStringLine(Line12, " Measure Distance ",Blue,Green);
AODistance = Measure_Start();
if((AODistance > 20) && (car_state == 0 )) //距离障碍大于25cm
{
car_state = 1; //为1表示小车直行
Send_Motor_Data[0] = 0x08; //小车直行
Send_Motor_Data[1] = AO_Speed;
Transmit_Data(0xFB);
}
if(AODistance <= 20 ) //距离障碍小于等于25cm
{
car_state = 0; //为0表示小车即将进入非直线状态
Send_Motor_Data[0] = 0x10; //停车
Transmit_Data(0xFB);
Steer_Corner(90, 0); //舵机左转到90度
Delay_ms(1000);
LeftDistance = Measure_Start(); //测量左方距离
Steer_Corner(90, 1); //舵机右转到90度
Delay_ms(1000);
RightDistance = Measure_Start(); //测量右方距离
Delay_ms(1000);
if(LeftDistance > 40 )
{
Send_Motor_Data[0] = 0x41;
Send_Motor_Data[1] = 0x5A; //左转90度
Transmit_Data(0xFB);
Delay_ms(500);
Delay_ms(1000); //等待小车转好弯
Send_Motor_Data[0] = 0x08; //小车直行
Send_Motor_Data[1] = AO_Speed;
Transmit_Data(0xFB);
while(Measure_Start() < 40)
{
Steer_Corner(0, 1);
Delay_ms(700);
if(Measure_Start() < 20) break;
Steer_Corner(90, 1);
Delay_ms(700);
}
if(Measure_Start() < 20) //没有路可走
{
LeftNoWay = 1; //左方没有路可走
Send_Motor_Data[0] = 0x41; //小车左转180度
Send_Motor_Data[1] = 0xB4;
Transmit_Data(0xFB);
Steer_Corner(90, 0); //舵机左转90度
Delay_ms(1000);
Delay_ms(1000);
}
if(Measure_Start() >= 40) //找到了路可以过去
{
Steer_Corner(0, 1);
Delay_ms(500); //等待一段时间直至间距可容小车通过
Send_Motor_Data[0] = 0x61; //右转90度
Send_Motor_Data[1] = 0x5A;
Transmit_Data(0xFB);
Delay_ms(1000);
Delay_ms(500);
}
}
if(LeftDistance <= 40)
{
LeftNoWay = 1; //小车左方没有路
Send_Motor_Data[0] = 0x61; //小车右转90度
Send_Motor_Data[1] = 0x5A;
Transmit_Data(0xFB);
Steer_Corner(90, 0); //舵机左转90度
Delay_ms(500);
Delay_ms(1000);
}
if((RightDistance > 40) && (LeftNoWay == 1)) //小车向右运动
{
Send_Motor_Data[0] = 0x08; //小车直行
Send_Motor_Data[1] = AO_Speed;
Transmit_Data(0xFB);
while(Measure_Start() < 40)
{
Steer_Corner(0, 1);
Delay_ms(700);
if(Measure_Start() < 20) break;
Steer_Corner(90, 0);
Delay_ms(700);
}
if(Measure_Start() < 20 ) //没有路可走此时也说明这是一个没有间隙的障碍，不可过去，停车
{
Send_Motor_Data[0] = 0x10; //停车
Transmit_Data(0xFB);
TIM_Cmd(TIM2, DISABLE); //关闭定时器
return; //返回主函数
}
if(Measure_Start() >= 40) //找到了路可以过去
{
Steer_Corner(0, 1); //舵机归位
Delay_ms(500); //等待一段时间直至间距可容小车通过
Send_Motor_Data[0] = 0x41; //左转90度
Send_Motor_Data[1] = 0x5A;
Transmit_Data(0xFB);
Delay_ms(500);
Delay_ms(1000);
}
}
if((RightDistance <= 40) && (LeftNoWay == 1) ) //无路可走
{
Send_Motor_Data[0] = 0x10; //停车
Transmit_Data(0xFB);
TIM_Cmd(TIM2, DISABLE); //关闭定时器
return; //返回主函数
}
} */

复制代码