stm32循迹避障小车代码 - - Powered by Discuz!

#include "PID.h"
#include "stm32f10x.h"
#include "motor.h"
#include "delay.h"
u16 ReadData = 0xffff;
int BasicSpeed = 0;
int LeftSpeed = 0;
int RightSpeed = 0;
int basespeed = 530;
unsigned char bizhangflag1 = 0;
unsigned char bizhangflag2 = 0;
unsigned char bizhangflag3 = 0;
unsigned char catchflag = 0;
//红外对管对应于PF1~PF10
void TIM4_IRQHandler(void)
{
static int LastError = 0;
static int CurrentError = 0;
static unsigned int PIDCounter = 0;
unsigned char PIDflag = 0;
static int DiffError = 0;
// static int delay_time = 0;
// static int IntError = 0;
int Output = 0;
unsigned char cornerflag = 0;
if(TIM_GetITStatus(TIM4,TIM_IT_Update)==SET)
{
// TIM_Cmd(TIM4, DISABLE);
ReadData = GPIO_ReadInputData(GPIOF);
/***************************************特殊情况******************************************************/
/***************************************直角情况*******************************************/
if(ReadData == 0xff81||ReadData==0xff03||ReadData==0xff01||ReadData==0xffc1||ReadData==0xff83||ReadData==0xff07||
ReadData == 0xffe1||ReadData==0xffc3||ReadData==0xff87)
{
delay_ms(15);
if(GPIO_ReadInputData(GPIOF)==0xffff)
{
motor(0,0);
delay_ms(10);
motor(-500,-500);
delay_ms(250);
motor(-500,500);
while((GPIO_ReadInputData(GPIOF)!= 0xff9f)&&(GPIO_ReadInputData(GPIOF)!=0xffbf)&&(GPIO_ReadInputData(GPIOF)!=0xff1f)
&&(GPIO_ReadInputData(GPIOF)!=0xff3f)&&(GPIO_ReadInputData(GPIOF)!=0xff8f));
cornerflag = 1;
}
}
if(ReadData ==0xf80f||ReadData ==0xf81f||ReadData==0xfc0f||ReadData==0xfc1f||ReadData==0xf83f||ReadData==0xfe0f||
ReadData ==0xf87f||ReadData ==0xfc3f||ReadData==0xff0f)
{
delay_ms(10);
if(GPIO_ReadInputData(GPIOF)==0xffff)
{
motor(0,0);
delay_ms(10);
motor(-500,-500);
delay_ms(250);
motor(500,-500);
while((GPIO_ReadInputData(GPIOF)!= 0xff9f)&&(GPIO_ReadInputData(GPIOF)!=0xffbf)&&(GPIO_ReadInputData(GPIOF)!=0xff1f)&&
(GPIO_ReadInputData(GPIOF)!=0xff3f)&&(GPIO_ReadInputData(GPIOF)!=0xff8f));
cornerflag = 1;
}
}
/***********************************虚线情况******************************/
if(ReadData == 0xffff)
{
motor(540,550);
delay_us(30);
cornerflag = 1;
}
/***********************************************************************************************/
if(PIDCounter<10)
{
PIDCounter++;
}
if(PIDCounter == 10)
{
PIDflag = 1;
PIDCounter = 0;
}
if((PIDflag == 1)||(cornerflag == 1))
{
PIDflag=0;
cornerflag = 0;
LastError = CurrentError;
switch(ReadData) //使用Pf1~Pf10
{
case 0xff9f:CurrentError = 0;break;//1111 111110011110
case 0xffbf:CurrentError = 1;break;//1111 111110111110
case 0xff3f:CurrentError = 3;break;//1111 111100111110
case 0xff7f:CurrentError = 4;break;//1111 111101111110
case 0xfe7f:CurrentError = 6;break;//1111 111001111110
case 0xfeff:CurrentError = 7;break;//1111 111011111110
case 0xfcff:CurrentError = 9;break;//1111 110011111110
case 0xfdff:CurrentError = 12;break;//1111 110111111110
case 0xf9ff:CurrentError = 14;break;//1111 100111111110
case 0xfbff:CurrentError = 16;break;//1111 101111111110
case 0xffdf:CurrentError = -0;break;//1111 111111011110
case 0xffcf:CurrentError = -1;break;//1111 111111001110
case 0xffef:CurrentError = -3;break;//1111 111111101110
case 0xffe7:CurrentError = -6;break;//1111 111111100110
case 0xff77:CurrentError = -7;break;//1111 111101110110
case 0xfff3:CurrentError = -9;break;//1111 111111110010
case 0xfffb:CurrentError = -12;break;//1111 11111111010
case 0xfff9:CurrentError = -14;break;//1111 11111111000
case 0xfffc:CurrentError = -16;break;//1111 11111111100
case 0xff1f:CurrentError = 3;break;//1111 111100011110
case 0xfe3f:CurrentError = 6;break;//1111 111000111110
case 0xfc7f:CurrentError = 12;break;//1111 110001111110
case 0xf8ff:CurrentError = 17;break;//1111 100011111110
case 0xff8f:CurrentError = -3;break;//1111 111110001110
case 0xffc7:CurrentError = -6;break;//1111 111111000110
case 0xffe3:CurrentError = -12;break;//1111 111111100010
case 0xfff1:CurrentError = -17;break;//1111 111111110000
default:break;
}
if((CurrentError-LastError)>=6||((CurrentError-LastError)<=-6))
{
BasicSpeed = 80 ;
if((CurrentError>=7)||(CurrentError<=-7))
{
BasicSpeed = 40;
}
}
else
{
BasicSpeed = basespeed;
}
DiffError = CurrentError - LastError;
Output = CurrentError * 45 + DiffError * 560 ;
LeftSpeed = BasicSpeed - 6 + Output;
RightSpeed = BasicSpeed - Output;
if(LeftSpeed>550)
{
LeftSpeed = 550;
}
else if(LeftSpeed < -500)
{
LeftSpeed = -500;
}
if(RightSpeed>550)
{
RightSpeed = 550;
}
else if(RightSpeed < -500)
{
RightSpeed = -500;
}
motor(LeftSpeed,RightSpeed);
//TIM_ClearITPendingBit(TIM4,TIM_FLAG_Update);
}
}
// TIM_Cmd(TIM4, ENABLE);
TIM_ClearITPendingBit(TIM4,TIM_FLAG_Update);
}
void EXTI4_IRQHandler(void)
{
TIM_Cmd(TIM4, DISABLE);
// delay_ms(15);
if( GPIO_ReadInputDataBit(GPIOE,GPIO_Pin_4)==0)
{
if(bizhangflag1 == 0) //第一次壁障抄近道
{
bizhangflag1 = 1;
motor(0,0);
delay_ms(145);
motor(0,500);
delay_ms(200);
motor(545,550);
delay_ms(1160);
while((GPIO_ReadInputData(GPIOF)!= 0xff9f)&&(GPIO_ReadInputData(GPIOF)!=0xffbf)&&(GPIO_ReadInputData(GPIOF)!=0xff1f)&&
(GPIO_ReadInputData(GPIOF)!=0xff3f)&&(GPIO_ReadInputData(GPIOF)!=0xff8f)&&(GPIO_ReadInputData(GPIOF)!=0xffc7)
&&(GPIO_ReadInputData(GPIOF)!=0xfe3f)&&(GPIO_ReadInputData(GPIOF)!=0xffbf)&&(GPIO_ReadInputData(GPIOF)!=0xffcf)
&&(GPIO_ReadInputData(GPIOF)!=0xfff9)&&(GPIO_ReadInputData(GPIOF)!=0xf9ff)&&(GPIO_ReadInputData(GPIOF)!=0xfffb)&&
(GPIO_ReadInputData(GPIOF)!=0xfdff));
}
else
{
if(bizhangflag3 == 0) //第二次壁障减速准备抓东西
{
catchflag = 1;
bizhangflag3 = 1;
motor(0,0);
delay_ms(400);
basespeed = 120;
}
else if(bizhangflag3 == 1) //第三次壁障减速准备放东西
{
catchflag = 1;
basespeed = 200;
}
}
}
TIM_Cmd(TIM4, ENABLE);
EXTI_ClearITPendingBit(EXTI_Line4);
}
void EXTI2_IRQHandler(void)
{
// delay_ms(5);
if( (GPIO_ReadInputDataBit(GPIOE,GPIO_Pin_2)==0)&&(catchflag == 1))
{
if(bizhangflag2 == 0) //第一次触碰开关抓东西绕开障碍物
{
TIM_Cmd(TIM4, DISABLE);
catchflag = 0;
bizhangflag2= 1;
motor(50,50);
delay_ms(10);
basespeed = 490;
starthand();
delay_ms(500);
motor(-400,-400);
delay_ms(400);
motor(0,0);
delay_ms(100);
// motor(400,0);
//delay_ms(8);
// motor(0,0);
// delay_ms(100);
motor(280,540);
while((GPIO_ReadInputData(GPIOF)!= 0xff9f)&&(GPIO_ReadInputData(GPIOF)!=0xffbf)&&(GPIO_ReadInputData(GPIOF)!=0xff1f)&&
(GPIO_ReadInputData(GPIOF)!=0xff3f)&&(GPIO_ReadInputData(GPIOF)!=0xff8f)&&(GPIO_ReadInputData(GPIOF)!=0xffc7)
&&(GPIO_ReadInputData(GPIOF)!=0xfe3f)&&(GPIO_ReadInputData(GPIOF)!=0xffbf)&&(GPIO_ReadInputData(GPIOF)!=0xffcf));
// motor(0,0);
//
delay_ms(30);
TIM_Cmd(TIM4, ENABLE);
}
else //第二次触碰开关放东西全场结束
{
motor(0,0);
delay_ms(100);
endhand();
while(1); //全场结束
}
}
EXTI_ClearITPendingBit(EXTI_Line2);
}

复制代码