迷宮機器人的程序，基于stm32的開發板寫的，用了pid算法

ID:448250 · 發表于 2018-12-17 23:40

#include "stm32f10x.h"
#include "stdio.h"
#include "delay.h"
#include "tft.h"
#include "math.h"
#include "stm32_init.h"
#include "motor.h"
#include "pid.h"
#include "math.h"
#define pi 3.1415926535898
#define TwoPi 6.2831853071796
#define ZhiXing 1
#define HouZhuan 2
#define ZuoZhuan 3
#define YouZhuan 4
#define ZhengZhuan 0
#define FanZhuan 1
#define NoWall 0
#define YouGenQiang 1
#define ZuoGenQiang 2
//#define ShuangGenQiang 3
#define YouGenQiangXia 4
#define ZuoGenQiangXia 5
//#define ShuangGenQiangXia 6
#define X_ZhengFangXiang 1
#define Y_ZhengFangXiang 2
#define X_FuFangXiang 3
#define Y_FuFangXiang 4
#define YiGeTime1 96000 //95000
//#define YiGeTime2 94000
#define YiGeTime3 99000
#define BanGeTime1 47500
#define ZhongDianZuoBiao 7
#define Active 1
#define Dead 2
#define FenZhiID_MAX_3FangXiang 2
#define FenZhiID_MAX_2FangXiang 1
#define XunLu 0
#define FanHui 1
#define ChongCi 2
#define YES 1
#define NO 0
#define XiuZhengZhuanWanYiGeTime 85000 //90000
#define GenQiangYuZhi1 0x80
#define GenQiangYuZhi2 0xff
#define MAX_SuDu 400
#define MIN_SuDu 500
u16 tim2_count=0;//程序循環周期
u16 shuju[4];//定義第一組、2、4、6、8
u16 shuju1[4];//定義第二組、1、3、5、7
u16 qian,hou,zuo,you,zuoshang,zuoxia,youshang,youxia;//&0xf00
u16 youshang2,youxia2,zuoshang2,zuoxia2;//&0xfc0,0xf80
u16 youshang3,youxia3,zuoshang3,zuoxia3;//
u8 GenQiangFlag=0;//跟墻標志，做跟、右跟、雙跟
u8 YunXingFlag=0;//運行標志，直行、左轉、右轉、后轉
u8 QianJinFangXiang=0;//前進方向，Y正、Y負、X正、X負
u8 XunLuFanHuiChongCiFlag=XunLu;//尋路返回沖刺標志
u8 YiZouGuoGeShu=0;//已經走過的格數
u8 FenZhiDuiZhan=0;//分支堆棧
//u8 FenZhiID=0;//分支ID
u8 FenZhiID_MAX=0;//分支ID的最大值
u8 CunChuLuJingShuZuXiaBiao=0;//存儲路徑數組下標
u8 ChongCiLuJingXiaBiao=0;//沖刺路徑下標
u32 YiGeTime_PanDuan=0;//判斷一格的時間，默認為時間1，轉彎后為時間2，下次滿格變成時間3
u8 LianXuZhiXing_Count=0;//判斷是否連續直行，連續直行需要改變一格時間
u8 YouShangGenQiang_YuZhi=0;//右上跟墻閾值
u8 YouXiaGenQiang_YuZhi=0;//右下跟墻閾值
u8 ZuoShangGenQiang_YuZhi=0;//左上跟墻閾值
u8 ZuoXiaGenQiang_YuZhi=0;//左下跟墻閾值
u8 YouShangGenQiang_Count=0;//右上跟墻計數
u8 YouXiaGenQiang_Count=0;//右下跟墻計數
u8 ZuoShangGenQiang_Count=0;//左上跟墻計數
u8 ZuoXiaGenQiang_Count=0;//左下跟墻計數
const u16 juli[201];//查表，速度與距離的關系
const int sin_figure[451];
u8 LuJingZuoBiao[128][2];//路徑坐標
u8 FenZhiDianZuoBiao[64][2];//分支點坐標
u8 FenZhiID[64];//分支ID
u8 FenZhiQiWangFangXiang[64][4]; //分支期望方向
u8 LuJingQiWangFangXiang[1];//路徑期望方向
float zuolun=0;
float youlun=0;
u16 zuolunint=0;
u16 youlunint=0;
int x_point=0;
int y_point=0;
u32 zuolun_juli=0;
u32 youlun_juli=0;
u32 hecheng_juli=0;//合成距離
u8 FenZhi_ZhiZhen=0;
void CaiYang(void);
void XianShiZhi(void);
int ABS_2(int x);
void QianXiangPanDuan(void);//前向判斷
void YouZhuanChuLi(void);
void ZuoZhuanChuLi(void);
void HouZhuanChuLi(void);
void JiSuanZuoBiao(void);//計算坐標
void QiBu_Init(void);//起步初始化
void JiLuFenZhiDianJiXuanZeFangXiang(void);//記錄分支點及選擇方向
void GenJuQiWangFangXiangJiSuanQianJinFangXiang(void);//根據期望方向計算前進方向
void XianLuJing(void);//顯示路徑坐標
void GenJuLuJingQiWangFangXiangJiSuanQianJinFangXiang(void);//根據路徑期望方向計算前進方向
void AnLuJingFanHui(void);//按路徑返回
void GenQiangZiChengXu(void);//跟墻子程序
void AnLuJingChongCi(void);//按路徑沖刺
void ZhuanWanQianDaoWei(void);//轉彎前到位
void ChongCiQianXiangPanDuan(void);
void QiBu_Init2(void);
/******************************************************
*入口參數：無
*出口參數：無
*功能：主函數
******************************************************/
int main(void)
{
RCC_Configuration();//RCC配置
GPIO_Configuration();//GPIO配置
EXTI_Configuration();//初始化PA9外部中斷
NVIC_Configuration();//嵌套向量中斷控制器配置
TIM2_Int_Init(999,359);//tim2初始化
SPI_Configuration();
GPIO_SetBits(GPIOE,GPIO_Pin_15);//打開TFT電源
delay(0xffffff);//延時
Lcd_Init();//初始化TFT屏
LCD_Clear(0);//清屏顯示黑色
QiBu_Init();//起步初始化
CaiYang();
while(qian>0x200)
{
CaiYang();
delay(10000);
}
Init_Motor(400,ZhengZhuan,400,ZhengZhuan);//直行
delay(15000000);
while(1)
{
if(tim2_count>=20)//20*5ms=100ms
{
tim2_count=0;
if(YunXingFlag==ZhiXing)//2800//3300
{
zuolun_juli = juli[zuolunint-400];
youlun_juli = juli[youlunint-400];
hecheng_juli += (zuolun_juli + youlun_juli);
}
if(hecheng_juli>YiGeTime_PanDuan)//
{
hecheng_juli=0;
JiSuanZuoBiao();
}
CaiYang();
if(XunLuFanHuiChongCiFlag==XunLu)//尋路
{
QianXiangPanDuan();
}
else if(XunLuFanHuiChongCiFlag==ChongCi)
{
ChongCiQianXiangPanDuan();
}
}
}
}
/**********************沖刺前向判斷***********************/
void ChongCiQianXiangPanDuan(void)
{
char msg[20];
if(qian>0x500)//qian zhuang qiang
{
if((you<0x200)&&(zuo<0x200))
{
if(x_point==LuJingZuoBiao[ChongCiLuJingXiaBiao+1][0])
{
if(LuJingZuoBiao[ChongCiLuJingXiaBiao+1][1]<y_point)
{
LuJingQiWangFangXiang[0]=Y_FuFangXiang;
}
else if(LuJingZuoBiao[ChongCiLuJingXiaBiao+1][1]>y_point)
{
LuJingQiWangFangXiang[0]=Y_ZhengFangXiang;
}
}
else if(y_point==LuJingZuoBiao[ChongCiLuJingXiaBiao+1][1])
{
if(LuJingZuoBiao[ChongCiLuJingXiaBiao+1][0]<x_point)
{
LuJingQiWangFangXiang[0]=X_FuFangXiang;
}
else if(LuJingZuoBiao[ChongCiLuJingXiaBiao+1][0]>x_point)
{
LuJingQiWangFangXiang[0]=X_ZhengFangXiang;
}
}
else
{
Off_Motor(3);
LCD_ShowString(0,0,"cuo wu");
sprintf(msg,"x_zb=%1d",LuJingZuoBiao[ChongCiLuJingXiaBiao+1][0]);
LCD_ShowString(0,96,msg);
sprintf(msg,"y_zb=%1d",LuJingZuoBiao[ChongCiLuJingXiaBiao+1][1]);
LCD_ShowString(0,128,msg);
while(1);
}
GenJuLuJingQiWangFangXiangJiSuanQianJinFangXiang();
}
else if(you<0x200)//bu shi fen zhi dian
{
YouZhuanChuLi();
}
else if(zuo<0x200)//bu shi fen zhi dian
{
ZuoZhuanChuLi();
}
else//shi si hu tong
{
HouZhuanChuLi();
}
}
else//qian wu qiang bi
{
if(hecheng_juli==0)//zheng ge pan duan
{
if((qian<0x80)&&((zuo<0x200)||(you<0x200)))
{
if(x_point==LuJingZuoBiao[ChongCiLuJingXiaBiao+1][0])
{
if(LuJingZuoBiao[ChongCiLuJingXiaBiao+1][1]<y_point)
{
LuJingQiWangFangXiang[0]=Y_FuFangXiang;
}
else if(LuJingZuoBiao[ChongCiLuJingXiaBiao+1][1]>y_point)
{
LuJingQiWangFangXiang[0]=Y_ZhengFangXiang;
}
}
else if(y_point==LuJingZuoBiao[ChongCiLuJingXiaBiao+1][1])
{
if(LuJingZuoBiao[ChongCiLuJingXiaBiao+1][0]<x_point)
{
LuJingQiWangFangXiang[0]=X_FuFangXiang;
}
else if(LuJingZuoBiao[ChongCiLuJingXiaBiao+1][0]>x_point)
{
LuJingQiWangFangXiang[0]=X_ZhengFangXiang;
}
}
else
{
Off_Motor(3);
LCD_ShowString(0,0,"cuo wu");
sprintf(msg,"x_zb=%1d",LuJingZuoBiao[ChongCiLuJingXiaBiao+1][0]);
LCD_ShowString(0,96,msg);
sprintf(msg,"y_zb=%1d",LuJingZuoBiao[ChongCiLuJingXiaBiao+1][1]);
LCD_ShowString(0,128,msg);
while(1);
}
GenJuLuJingQiWangFangXiangJiSuanQianJinFangXiang();
}
else
{
GenQiangZiChengXu();
}
}
else
{
GenQiangZiChengXu();
}
}
}
/**********************前向判斷***********************/
void QianXiangPanDuan(void)
{
if(qian>0x500)//qian zhuang qiang
{
if((you<0x200)&&(zuo<0x200))
{
JiLuFenZhiDianJiXuanZeFangXiang();
GenJuQiWangFangXiangJiSuanQianJinFangXiang();
}
else if(you<0x200)//bu shi fen zhi dian
{
YouZhuanChuLi();
}
else if(zuo<0x200)//bu shi fen zhi dian
{
ZuoZhuanChuLi();
}
else//shi si hu tong
{
HouZhuanChuLi();
}
}
else//qian wu qiang bi
{
if(hecheng_juli==0)//zheng ge pan duan
{
if((qian<0x80)&&((zuo<0x200)||(you<0x200)))
{
JiLuFenZhiDianJiXuanZeFangXiang();
GenJuQiWangFangXiangJiSuanQianJinFangXiang();
}
else
{
GenQiangZiChengXu();
}
}
else
{
GenQiangZiChengXu();
}
}
}
/**********************跟墻子程序***********************/
void GenQiangZiChengXu(void)
{
// float you1,zuo1;
YunXingFlag=ZhiXing;
if(youshang>0x300)
{
zuolunint=MIN_SuDu;
youlunint=MAX_SuDu;
}
else if(zuoshang>0x300)
{
zuolunint=MAX_SuDu;
youlunint=MIN_SuDu;
}
else
{
if(you>0x100)
{
if((youshang>0x80)&&(youshang<0x300))
{
if(GenQiangFlag != YouGenQiang)
{
YouShangGenQiang_Count++;
YouXiaGenQiang_Count=0;
ZuoShangGenQiang_Count=0;
ZuoXiaGenQiang_Count=0;
if(YouShangGenQiang_Count>2)
{
YouShangGenQiang_Count=0;
GenQiangFlag=YouGenQiang;
PID_init2();
PID_realize(0x180,youshang3,YouGenQiang);//0x180
}
}
else
{
PID_realize(0x180,youshang3,YouGenQiang);//0x180
}
}
else if((youxia>0x80)&&(youxia<0x300))
{
if(GenQiangFlag == YouGenQiang)
{
YouShangGenQiang_Count=0;
YouXiaGenQiang_Count=0;
ZuoShangGenQiang_Count=0;
ZuoXiaGenQiang_Count=0;
GenQiangFlag=YouGenQiangXia;
PID_init2();
PID_realize2(0x180,youxia3,YouGenQiangXia);//0x180
}
else if((GenQiangFlag != YouGenQiangXia)&&(youxia>0x100))
{
YouShangGenQiang_Count=0;
YouXiaGenQiang_Count++;
ZuoShangGenQiang_Count=0;
ZuoXiaGenQiang_Count=0;
if(YouXiaGenQiang_Count>2)
{
YouXiaGenQiang_Count=0;
GenQiangFlag=YouGenQiangXia;
PID_init2();
PID_realize2(0x180,youxia3,YouGenQiangXia);//0x180
}
}
else if(GenQiangFlag==YouGenQiangXia)
{
PID_realize2(0x180,youxia3,YouGenQiangXia);//0x180
}
}
}
else if(zuo>0x100)
{
if((zuoshang>0x80)&&(zuoshang<0x300))
{
if(GenQiangFlag != ZuoGenQiang)
{
YouShangGenQiang_Count=0;
YouXiaGenQiang_Count=0;
ZuoShangGenQiang_Count++;
ZuoXiaGenQiang_Count=0;
if(ZuoShangGenQiang_Count>2)
{
ZuoShangGenQiang_Count=0;
GenQiangFlag=ZuoGenQiang;
PID_init3();
PID_realize(0x140,zuoshang3,ZuoGenQiang);//0x180
}
}
else
{
PID_realize(0x140,zuoshang3,ZuoGenQiang);//0x180
}
}
else if((zuoxia>0x80)&&(zuoxia<0x300))
{
if(GenQiangFlag == ZuoGenQiang)
{
YouShangGenQiang_Count=0;
YouXiaGenQiang_Count=0;
ZuoShangGenQiang_Count=0;
ZuoXiaGenQiang_Count=0;
GenQiangFlag=ZuoGenQiangXia;
PID_init3();
PID_realize2(0x140,zuoxia3,ZuoGenQiangXia);//0x180
}
else if((GenQiangFlag != ZuoGenQiangXia)&&(zuoxia>0x100))
{
YouShangGenQiang_Count=0;
YouXiaGenQiang_Count=0;
ZuoShangGenQiang_Count=0;
ZuoXiaGenQiang_Count++;
if(ZuoXiaGenQiang_Count>2)
{
ZuoXiaGenQiang_Count=0;
GenQiangFlag=ZuoGenQiangXia;
PID_init3();
PID_realize2(0x140,zuoxia3,ZuoGenQiangXia);//0x180
}
}
else if(GenQiangFlag==ZuoGenQiangXia)
{
PID_realize2(0x140,zuoxia3,ZuoGenQiangXia);//0x180
}
}
}
else if((zuo<0x80)&&(you<0x80)&&(hou<0x80))
{
GenQiangFlag=NoWall;
// if(qian>0x80)
// {
// PID_init4();
// you1=youshang2;
// zuo1=zuoshang2;
// PID_realize(0,ABS(you1-zuo1),NoWall);
// }
// else
// {
zuolunint=MAX_SuDu;
youlunint=MAX_SuDu;
// }
}
}
if(zuolunint==0)
{
zuolunint=400;
}
if(youlunint==0)
{
youlunint=400;
}
Init_Motor(zuolunint,ZhengZhuan,youlunint,ZhengZhuan);//直行
}
/**********************計算坐標***********************/
void JiSuanZuoBiao(void)
{
u8 i,j;
char msg[20];
if(QianJinFangXiang==Y_ZhengFangXiang)
{
y_point += 1;
}
else if(QianJinFangXiang==Y_FuFangXiang)
{
y_point -= 1;
}
else if(QianJinFangXiang==X_ZhengFangXiang)
{
x_point += 1;
}
else if(QianJinFangXiang==X_FuFangXiang)
{
x_point -= 1;
}
if(y_point<0)
{
y_point=0;
}
else if(y_point>ZhongDianZuoBiao)
{
y_point=ZhongDianZuoBiao;
}
if(x_point<0)
{
x_point=0;
}
else if(x_point>ZhongDianZuoBiao)
{
x_point=ZhongDianZuoBiao;
}
// sprintf(msg,"FenZhi_ZhiZhen=%1d",FenZhi_ZhiZhen);
// LCD_ShowString(0,64,msg);
// sprintf(msg,"FenZhiDuiZhan=%1d",FenZhiDuiZhan);
// LCD_ShowString(0,96,msg);
sprintf(msg,"x_point=%1d",x_point);
LCD_ShowString(0,160,msg);
sprintf(msg,"y_point=%1d",y_point);
LCD_ShowString(0,192,msg);
if(XunLuFanHuiChongCiFlag==XunLu)
{
if((LuJingZuoBiao[YiZouGuoGeShu-1][0]==x_point)&&(LuJingZuoBiao[YiZouGuoGeShu-1][1]==y_point))
{
return;
}
for(i=0;i<YiZouGuoGeShu;i++)
{
if((LuJingZuoBiao[i][0]==x_point)&&(LuJingZuoBiao[i][1]==y_point))
{
YiZouGuoGeShu=i;
for(j=0;j<FenZhiDuiZhan+1;j++)
{
if((FenZhiDianZuoBiao[j][0]==x_point)&&(FenZhiDianZuoBiao[j][1]==y_point))
{
FenZhiID[j]++;
FenZhi_ZhiZhen=j;
break;
}
}
break;
}
}
LuJingZuoBiao[YiZouGuoGeShu][0]=x_point;
LuJingZuoBiao[YiZouGuoGeShu][1]=y_point;
YiZouGuoGeShu++;
if((x_point==ZhongDianZuoBiao)&&(y_point==ZhongDianZuoBiao))
{
Init_Motor(400,ZhengZhuan,400,ZhengZhuan);//直行
delay(15000000);
Off_Motor(3);
LCD_Clear(0);//清屏顯示黑色
XianLuJing();
while(XunLuFanHuiChongCiFlag==XunLu);
}
}
else if(XunLuFanHuiChongCiFlag==ChongCi)
{
ChongCiLuJingXiaBiao++;
if((x_point==ZhongDianZuoBiao)&&(y_point==ZhongDianZuoBiao))
{
Init_Motor(400,ZhengZhuan,400,ZhengZhuan);//直行
delay(15000000);
Off_Motor(3);
LCD_Clear(0);//清屏顯示黑色
LCD_ShowString(0,0,"success");
while(1);
}
}
}
/**********************記錄分支點及選擇方向***********************/
void JiLuFenZhiDianJiXuanZeFangXiang(void)
{
u8 i;
if( (x_point==0) && (y_point==0) )
{
return;
}
if( (x_point==FenZhiDianZuoBiao[FenZhiDuiZhan][0]) && (y_point==FenZhiDianZuoBiao[FenZhiDuiZhan][1]) )
{
return;
}
for(i=0;i<FenZhiDuiZhan;i++)
{
if( (x_point==FenZhiDianZuoBiao[i][0]) && (y_point==FenZhiDianZuoBiao[i][1]) )
{
// FenZhiID[i]++;
// FenZhi_ZhiZhen=i;
return;
}
}
FenZhiDuiZhan++;
FenZhiDianZuoBiao[FenZhiDuiZhan][0]=x_point;
FenZhiDianZuoBiao[FenZhiDuiZhan][1]=y_point;
FenZhi_ZhiZhen=FenZhiDuiZhan;
if((qian<0x80)&&(zuo<0x200)&&(you<0x200))//qian zuo you
{
FenZhiID_MAX=FenZhiID_MAX_3FangXiang;
if(QianJinFangXiang==Y_ZhengFangXiang)
{
FenZhiQiWangFangXiang[FenZhiDuiZhan][0]=Y_ZhengFangXiang;
FenZhiQiWangFangXiang[FenZhiDuiZhan][1]=X_ZhengFangXiang;
FenZhiQiWangFangXiang[FenZhiDuiZhan][2]=X_FuFangXiang;
FenZhiQiWangFangXiang[FenZhiDuiZhan][3]=Y_FuFangXiang;
}
else if(QianJinFangXiang==Y_FuFangXiang)
{
FenZhiQiWangFangXiang[FenZhiDuiZhan][0]=X_ZhengFangXiang;
FenZhiQiWangFangXiang[FenZhiDuiZhan][1]=Y_FuFangXiang;
FenZhiQiWangFangXiang[FenZhiDuiZhan][2]=X_FuFangXiang;
FenZhiQiWangFangXiang[FenZhiDuiZhan][3]=Y_ZhengFangXiang;
}
else if(QianJinFangXiang==X_ZhengFangXiang)
{
FenZhiQiWangFangXiang[FenZhiDuiZhan][0]=X_ZhengFangXiang;
FenZhiQiWangFangXiang[FenZhiDuiZhan][1]=Y_ZhengFangXiang;
FenZhiQiWangFangXiang[FenZhiDuiZhan][2]=Y_FuFangXiang;
FenZhiQiWangFangXiang[FenZhiDuiZhan][3]=X_FuFangXiang;
}
else if(QianJinFangXiang==X_FuFangXiang)
{
FenZhiQiWangFangXiang[FenZhiDuiZhan][0]=Y_ZhengFangXiang;
FenZhiQiWangFangXiang[FenZhiDuiZhan][1]=X_FuFangXiang;
FenZhiQiWangFangXiang[FenZhiDuiZhan][2]=Y_FuFangXiang;
FenZhiQiWangFangXiang[FenZhiDuiZhan][3]=X_ZhengFangXiang;
}
}
else if((qian<0x80)&&(zuo<0x200))//qian zuo
{
FenZhiID_MAX=FenZhiID_MAX_2FangXiang;
if(QianJinFangXiang==Y_ZhengFangXiang)
{
FenZhiQiWangFangXiang[FenZhiDuiZhan][0]=Y_ZhengFangXiang;
FenZhiQiWangFangXiang[FenZhiDuiZhan][1]=X_FuFangXiang;
FenZhiQiWangFangXiang[FenZhiDuiZhan][2]=Y_FuFangXiang;
}
else if(QianJinFangXiang==Y_FuFangXiang)
{
FenZhiQiWangFangXiang[FenZhiDuiZhan][0]=X_ZhengFangXiang;
FenZhiQiWangFangXiang[FenZhiDuiZhan][1]=Y_FuFangXiang;
FenZhiQiWangFangXiang[FenZhiDuiZhan][2]=Y_ZhengFangXiang;
}
else if(QianJinFangXiang==X_ZhengFangXiang)
{
FenZhiQiWangFangXiang[FenZhiDuiZhan][0]=X_ZhengFangXiang;
FenZhiQiWangFangXiang[FenZhiDuiZhan][1]=Y_ZhengFangXiang;
FenZhiQiWangFangXiang[FenZhiDuiZhan][2]=X_FuFangXiang;
}
else if(QianJinFangXiang==X_FuFangXiang)
{
FenZhiQiWangFangXiang[FenZhiDuiZhan][0]=X_FuFangXiang;
FenZhiQiWangFangXiang[FenZhiDuiZhan][1]=Y_FuFangXiang;
FenZhiQiWangFangXiang[FenZhiDuiZhan][2]=X_ZhengFangXiang;
}
}
else if((qian<0x80)&&(you<0x200))//qian you
{
FenZhiID_MAX=FenZhiID_MAX_2FangXiang;
if(QianJinFangXiang==Y_ZhengFangXiang)
{
FenZhiQiWangFangXiang[FenZhiDuiZhan][0]=Y_ZhengFangXiang;
FenZhiQiWangFangXiang[FenZhiDuiZhan][1]=X_ZhengFangXiang;
FenZhiQiWangFangXiang[FenZhiDuiZhan][2]=Y_FuFangXiang;
}
else if(QianJinFangXiang==Y_FuFangXiang)
{
FenZhiQiWangFangXiang[FenZhiDuiZhan][0]=Y_FuFangXiang;
FenZhiQiWangFangXiang[FenZhiDuiZhan][1]=X_FuFangXiang;
FenZhiQiWangFangXiang[FenZhiDuiZhan][2]=Y_ZhengFangXiang;
}
else if(QianJinFangXiang==X_ZhengFangXiang)
{
FenZhiQiWangFangXiang[FenZhiDuiZhan][0]=X_ZhengFangXiang;
FenZhiQiWangFangXiang[FenZhiDuiZhan][1]=Y_FuFangXiang;
FenZhiQiWangFangXiang[FenZhiDuiZhan][2]=X_FuFangXiang;
}
else if(QianJinFangXiang==X_FuFangXiang)
{
FenZhiQiWangFangXiang[FenZhiDuiZhan][0]=Y_ZhengFangXiang;
FenZhiQiWangFangXiang[FenZhiDuiZhan][1]=X_FuFangXiang;
FenZhiQiWangFangXiang[FenZhiDuiZhan][2]=X_ZhengFangXiang;
}
}
else if((zuo<0x200) &&(you<0x200))
{
FenZhiID_MAX=FenZhiID_MAX_2FangXiang;
if(QianJinFangXiang==Y_ZhengFangXiang)
{
FenZhiQiWangFangXiang[FenZhiDuiZhan][0]=X_ZhengFangXiang;
FenZhiQiWangFangXiang[FenZhiDuiZhan][1]=X_FuFangXiang;
FenZhiQiWangFangXiang[FenZhiDuiZhan][2]=Y_FuFangXiang;
}
else if(QianJinFangXiang==Y_FuFangXiang)
{
FenZhiQiWangFangXiang[FenZhiDuiZhan][0]=X_ZhengFangXiang;
FenZhiQiWangFangXiang[FenZhiDuiZhan][1]=X_FuFangXiang;
FenZhiQiWangFangXiang[FenZhiDuiZhan][2]=Y_ZhengFangXiang;
}
else if(QianJinFangXiang==X_ZhengFangXiang)
{
FenZhiQiWangFangXiang[FenZhiDuiZhan][0]=Y_ZhengFangXiang;
FenZhiQiWangFangXiang[FenZhiDuiZhan][1]=Y_FuFangXiang;
FenZhiQiWangFangXiang[FenZhiDuiZhan][2]=X_FuFangXiang;
}
else if(QianJinFangXiang==X_FuFangXiang)
{
FenZhiQiWangFangXiang[FenZhiDuiZhan][0]=Y_ZhengFangXiang;
FenZhiQiWangFangXiang[FenZhiDuiZhan][1]=Y_FuFangXiang;
FenZhiQiWangFangXiang[FenZhiDuiZhan][2]=X_ZhengFangXiang;
}
}
}
/**********************根據期望方向計算前進方向***********************/
void GenJuQiWangFangXiangJiSuanQianJinFangXiang(void)
{
u8 FangXiangChaZhi;
if(FenZhiQiWangFangXiang[FenZhi_ZhiZhen][FenZhiID[FenZhi_ZhiZhen]]<QianJinFangXiang)
{
FangXiangChaZhi=QianJinFangXiang-FenZhiQiWangFangXiang[FenZhi_ZhiZhen][FenZhiID[FenZhi_ZhiZhen]];
if(FangXiangChaZhi==2)
{
FenZhiID[FenZhi_ZhiZhen]++;
}
}
else
{
FangXiangChaZhi=FenZhiQiWangFangXiang[FenZhi_ZhiZhen][FenZhiID[FenZhi_ZhiZhen]]-QianJinFangXiang;
if(FangXiangChaZhi==2)
{
FenZhiID[FenZhi_ZhiZhen]++;
}
}
if(FenZhiQiWangFangXiang[FenZhi_ZhiZhen][FenZhiID[FenZhi_ZhiZhen]]<QianJinFangXiang)
{
FangXiangChaZhi=QianJinFangXiang-FenZhiQiWangFangXiang[FenZhi_ZhiZhen][FenZhiID[FenZhi_ZhiZhen]];
switch(FangXiangChaZhi)
{
case 1:YouZhuanChuLi();break;//delay(1000000);
// case 2:HouZhuanChuLi();break;
case 3:ZuoZhuanChuLi();break;
default:break;
}
}
else
{
FangXiangChaZhi=FenZhiQiWangFangXiang[FenZhi_ZhiZhen][FenZhiID[FenZhi_ZhiZhen]]-QianJinFangXiang;
switch(FangXiangChaZhi)
{
case 0:break;
case 1:ZuoZhuanChuLi();break;
// case 2:HouZhuanChuLi();break;
case 3:YouZhuanChuLi();break;
default:break;
}
}
}
/**********************起步初始化***********************/
/**********************起步初始化***********************/
void QiBu_Init2(void)
{
u8 i;
tim2_count=0;//程序循環周期
GenQiangFlag=0;//跟墻標志，做跟、右跟、雙跟
YunXingFlag=0;//運行標志，直行、左轉、右轉、后轉
XunLuFanHuiChongCiFlag=ChongCi;//尋路返回沖刺標志
FenZhiDuiZhan=0;//分支堆棧
// FenZhiID=0;//分支ID
FenZhiID_MAX=0;//分支ID的最大值
CunChuLuJingShuZuXiaBiao=0;//存儲路徑數組下標
ChongCiLuJingXiaBiao=0;//沖刺路徑下標
LianXuZhiXing_Count=0;//判斷是否連續直行，連續直行需要改變一格時間
zuolun=0;
youlun=0;
zuolunint=0;
youlunint=0;
x_point=0;
y_point=0;
zuolun_juli=0;
youlun_juli=0;
hecheng_juli=0;//合成距離
QianJinFangXiang=Y_ZhengFangXiang;
YiZouGuoGeShu=1;
YiGeTime_PanDuan=YiGeTime1;
for(i=0;i<64;i++)
{
FenZhiDianZuoBiao[i][0]=0;
FenZhiDianZuoBiao[i][1]=0;
FenZhiID[i]=0;
}
for(i=0;i<64;i++)
{
FenZhiQiWangFangXiang[i][0]=0;
FenZhiQiWangFangXiang[i][1]=0;
FenZhiQiWangFangXiang[i][2]=0;
FenZhiQiWangFangXiang[i][3]=0;
}
LuJingQiWangFangXiang[0]=0;
YouShangGenQiang_YuZhi=GenQiangYuZhi2;
YouXiaGenQiang_YuZhi=GenQiangYuZhi2;
ZuoShangGenQiang_YuZhi=GenQiangYuZhi2;
ZuoXiaGenQiang_YuZhi=GenQiangYuZhi2;
}
void QiBu_Init(void)
{
u8 i;
tim2_count=0;//程序循環周期
GenQiangFlag=0;//跟墻標志，做跟、右跟、雙跟
YunXingFlag=0;//運行標志，直行、左轉、右轉、后轉
XunLuFanHuiChongCiFlag=XunLu;//尋路返回沖刺標志
FenZhiDuiZhan=0;//分支堆棧
// FenZhiID=0;//分支ID
FenZhiID_MAX=0;//分支ID的最大值
CunChuLuJingShuZuXiaBiao=0;//存儲路徑數組下標
ChongCiLuJingXiaBiao=0;//沖刺路徑下標
LianXuZhiXing_Count=0;//判斷是否連續直行，連續直行需要改變一格時間
zuolun=0;
youlun=0;
zuolunint=0;
youlunint=0;
x_point=0;
y_point=0;
zuolun_juli=0;
youlun_juli=0;
hecheng_juli=0;//合成距離
QianJinFangXiang=Y_ZhengFangXiang;
YiZouGuoGeShu=1;
YiGeTime_PanDuan=YiGeTime1;
LuJingZuoBiao[0][0]=0;
LuJingZuoBiao[0][1]=0;
for(i=0;i<128;i++)
{
LuJingZuoBiao[i][0]=0;
LuJingZuoBiao[i][1]=0;
}
for(i=0;i<64;i++)
{
FenZhiDianZuoBiao[i][0]=0;
FenZhiDianZuoBiao[i][1]=0;
FenZhiID[i]=0;
}
for(i=0;i<64;i++)
{
FenZhiQiWangFangXiang[i][0]=0;
FenZhiQiWangFangXiang[i][1]=0;
FenZhiQiWangFangXiang[i][2]=0;
FenZhiQiWangFangXiang[i][3]=0;
}
LuJingQiWangFangXiang[0]=0;
YouShangGenQiang_YuZhi=GenQiangYuZhi2;
YouXiaGenQiang_YuZhi=GenQiangYuZhi2;
ZuoShangGenQiang_YuZhi=GenQiangYuZhi2;
ZuoXiaGenQiang_YuZhi=GenQiangYuZhi2;
}
/**********************按路徑返回***********************/
void AnLuJingFanHui(void)
{
if(CunChuLuJingShuZuXiaBiao!=0)
{
if(LuJingZuoBiao[CunChuLuJingShuZuXiaBiao][0]==LuJingZuoBiao[CunChuLuJingShuZuXiaBiao-1][0])
{
if(LuJingZuoBiao[CunChuLuJingShuZuXiaBiao-1][1]<LuJingZuoBiao[CunChuLuJingShuZuXiaBiao][1])
{
LuJingQiWangFangXiang[0]=Y_FuFangXiang;
}
else
{
LuJingQiWangFangXiang[0]=Y_ZhengFangXiang;
}
}
else
{
if(LuJingZuoBiao[CunChuLuJingShuZuXiaBiao-1][0]<LuJingZuoBiao[CunChuLuJingShuZuXiaBiao][0])
{
LuJingQiWangFangXiang[0]=X_FuFangXiang;
}
else
{
LuJingQiWangFangXiang[0]=X_ZhengFangXiang;
}
}
}
}
/**********************按路徑沖刺***********************/
void AnLuJingChongCi(void)
{
if(ChongCiLuJingXiaBiao != CunChuLuJingShuZuXiaBiao)
{
if(LuJingZuoBiao[ChongCiLuJingXiaBiao][0]==LuJingZuoBiao[ChongCiLuJingXiaBiao+1][0])
{
if(LuJingZuoBiao[ChongCiLuJingXiaBiao+1][1]<LuJingZuoBiao[ChongCiLuJingXiaBiao][1])
{
LuJingQiWangFangXiang[0]=Y_FuFangXiang;
}
else
{
LuJingQiWangFangXiang[0]=Y_ZhengFangXiang;
}
}
else
{
if(LuJingZuoBiao[ChongCiLuJingXiaBiao+1][0]<LuJingZuoBiao[ChongCiLuJingXiaBiao][0])
{
LuJingQiWangFangXiang[0]=X_FuFangXiang;
}
else
{
LuJingQiWangFangXiang[0]=X_ZhengFangXiang;
}
}
}
}
/**********************根據路徑期望方向計算前進方向***********************/
/**********************根據路徑期望方向計算前進方向***********************/
void GenJuLuJingQiWangFangXiangJiSuanQianJinFangXiang(void)
{
// LianXuZhiXing_Count=0;
// ZhuanWanQianDaoWei();
u8 FangXiangChaZhi;
if(LuJingQiWangFangXiang[0]<QianJinFangXiang)
{
FangXiangChaZhi=QianJinFangXiang-LuJingQiWangFangXiang[0];
switch(FangXiangChaZhi)
{
case 1:
YouZhuanChuLi();break;
case 3:
ZuoZhuanChuLi();break;
default:break;
}
}
else
{
FangXiangChaZhi=LuJingQiWangFangXiang[0]-QianJinFangXiang;
switch(FangXiangChaZhi)
{
case 0:break;
case 1:
ZuoZhuanChuLi();break;
case 3:
YouZhuanChuLi();break;
default:break;
}
}
}
/******************************************************
*入口參數：無
*出口參數：無
*功能：定時器2中斷服務函數
******************************************************/
void TIM2_IRQHandler(void)
{
TIM_ClearITPendingBit(TIM2,TIM_IT_Update); //清除中斷標志
tim2_count++;
}
void YouZhuanChuLi(void)
{
if(hecheng_juli>XiuZhengZhuanWanYiGeTime)
{
JiSuanZuoBiao();
}
hecheng_juli=0;
Off_Motor(3);
Init_Motor(400,ZhengZhuan,400,FanZhuan);//右轉
delay(6210000);
Off_Motor(3);
GenQiangFlag=NoWall;
YiGeTime_PanDuan=YiGeTime1;//
if(QianJinFangXiang==Y_ZhengFangXiang)
{
QianJinFangXiang=X_ZhengFangXiang;
}
else if(QianJinFangXiang==Y_FuFangXiang)
{
QianJinFangXiang=X_FuFangXiang;
}
else if(QianJinFangXiang==X_ZhengFangXiang)
{
QianJinFangXiang=Y_FuFangXiang;
}
else if(QianJinFangXiang==X_FuFangXiang)
{
QianJinFangXiang=Y_ZhengFangXiang;
}
YunXingFlag=ZhiXing;
zuolunint=MAX_SuDu;
youlunint=MAX_SuDu;
Init_Motor(zuolunint,ZhengZhuan,youlunint,ZhengZhuan);//直行
}
void ZuoZhuanChuLi(void)
{
if(hecheng_juli>XiuZhengZhuanWanYiGeTime)
{
JiSuanZuoBiao();
}
hecheng_juli=0;
Off_Motor(3);
Init_Motor(400,FanZhuan,400,ZhengZhuan);//左轉
delay(6210000);
Off_Motor(3);
GenQiangFlag=NoWall;
YiGeTime_PanDuan=YiGeTime1;//
if(QianJinFangXiang==Y_ZhengFangXiang)
{
QianJinFangXiang=X_FuFangXiang;
}
else if(QianJinFangXiang==Y_FuFangXiang)
{
QianJinFangXiang=X_ZhengFangXiang;
}
else if(QianJinFangXiang==X_ZhengFangXiang)
{
QianJinFangXiang=Y_ZhengFangXiang;
}
else if(QianJinFangXiang==X_FuFangXiang)
{
QianJinFangXiang=Y_FuFangXiang;
}
YunXingFlag=ZhiXing;
zuolunint=MAX_SuDu;
youlunint=MAX_SuDu;
Init_Motor(zuolunint,ZhengZhuan,youlunint,ZhengZhuan);//直行
}
void HouZhuanChuLi(void)
{
if(hecheng_juli>XiuZhengZhuanWanYiGeTime)
{
JiSuanZuoBiao();
}
hecheng_juli=0;
Off_Motor(3);
Init_Motor(400,FanZhuan,400,ZhengZhuan);//后轉
delay(12410000);
Off_Motor(3);
GenQiangFlag=NoWall;
YiGeTime_PanDuan=YiGeTime1;//
if(QianJinFangXiang==Y_ZhengFangXiang)
{
QianJinFangXiang=Y_FuFangXiang;
}
else if(QianJinFangXiang==Y_FuFangXiang)
{
QianJinFangXiang=Y_ZhengFangXiang;
}
else if(QianJinFangXiang==X_ZhengFangXiang)
{
QianJinFangXiang=X_FuFangXiang;
}
else if(QianJinFangXiang==X_FuFangXiang)
{
QianJinFangXiang=X_ZhengFangXiang;
}
YunXingFlag=ZhiXing;
zuolunint=MAX_SuDu;
youlunint=MAX_SuDu;
Init_Motor(zuolunint,ZhengZhuan,youlunint,ZhengZhuan);//直行
}
void ZhuanWanQianDaoWei(void)
{
u8 i;
if(qian>0x0)
{
i=0;
while(qian<0x500)
{
CaiYang();
i++;
if(i>100)
{
i=0;
GenQiangZiChengXu();
}
delay(1000);
}
}
}
/**********************顯示路徑***********************/
void XianLuJing(void)
{
char msg[20];
u8 i;
for(i=0;i<7;i++)
{
sprintf(msg,"%1d",LuJingZuoBiao[0+2*i][0]);//LuJingZuoBiao
LCD_ShowString(0,0+32*i,msg);
sprintf(msg,"%1d",LuJingZuoBiao[0+2*i][1]);
LCD_ShowString(32,0+32*i,msg);
sprintf(msg,"%1d",LuJingZuoBiao[1+2*i][0]);
LCD_ShowString(200,0+32*i,msg);
sprintf(msg,"%1d",LuJingZuoBiao[1+2*i][1]);
LCD_ShowString(232,0+32*i,msg);
}
}
/**********************顯示***********************/
void XianShiZhi(void)
{
char msg[20];
sprintf(msg,"1=%3X",shuju1[0]);
LCD_ShowString(100,0,msg);
sprintf(msg,"2=%3X",shuju[0]);
LCD_ShowString(0,32,msg);
sprintf(msg,"3=%3X",shuju1[1]);
LCD_ShowString(0,64,msg);
sprintf(msg,"4=%3X",shuju[1]);
LCD_ShowString(0,96,msg);
sprintf(msg,"5=%3X",shuju1[2]);
LCD_ShowString(100,128,msg);
sprintf(msg,"6=%3X",shuju[2]);
LCD_ShowString(200,96,msg);
sprintf(msg,"7=%3X",shuju1[3]);
LCD_ShowString(200,64,msg);
sprintf(msg,"8=%3X",shuju[3]);
LCD_ShowString(200,32,msg);
}
/**********************采樣***********************/
void CaiYang(void)
{
u8 i,j;
u16 shuju_zc[4];
u16 shuju1_zc[4];
GPIO_SetBits(GPIOA, GPIO_Pin_7);//打開AD_Ctrl1，開始轉換//2/4/6/8
delay(10);//延時
ADC_Configuration((u32)shuju,1);//轉換第一組
GPIO_ResetBits(GPIOA, GPIO_Pin_7);//關閉AD_Ctrl1
delay(10);//延時
GPIO_SetBits(GPIOA, GPIO_Pin_6);//打開AD_Ctrl0，開始轉換//1/3/5/7
delay(10);//延時
ADC_Configuration((u32)shuju1,2); //轉換第二組
GPIO_ResetBits(GPIOA, GPIO_Pin_6);//關閉AD_Ctrl0
for(i=0;i<4;i++)
{
shuju_zc[i] = 0;
shuju1_zc[i] = 0;
}
for(i=0;i<3;i++)
{
GPIO_SetBits(GPIOA, GPIO_Pin_7);//打開AD_Ctrl1，開始轉換//2/4/6/8
delay(10);//延時
ADC_Configuration((u32)shuju,1);//轉換第一組
GPIO_ResetBits(GPIOA, GPIO_Pin_7);//關閉AD_Ctrl1
delay(10);//延時
GPIO_SetBits(GPIOA, GPIO_Pin_6);//打開AD_Ctrl0，開始轉換//1/3/5/7
delay(10);//延時
ADC_Configuration((u32)shuju1,2); //轉換第二組
GPIO_ResetBits(GPIOA, GPIO_Pin_6);//關閉AD_Ctrl0
for(j=0;j<4;j++)
{
shuju_zc[j] += shuju[j];
shuju1_zc[j] += shuju1[j];
}
}
for(i=0;i<4;i++)
{
shuju[i] = shuju_zc[i]/3;
shuju1[i] = shuju1_zc[i]/3;
}
qian=shuju1[0];
zuo=shuju1[1];
hou=shuju1[2];
you=shuju1[3];
zuoshang=shuju[0];
zuoxia=shuju[1];
youxia=shuju[2];
youshang=shuju[3];
zuoshang2=shuju[0]&0xf00;
zuoxia2=shuju[1]&0xf00;
youxia2=shuju[2]&0xf00;
youshang2=shuju[3]&0xf00;
zuoshang3=shuju[0]&0xfc0;
zuoxia3=shuju[1]&0xfc0;
youxia3=shuju[2]&0xf80;
youshang3=shuju[3]&0xf80;
}
/******************************************************
*入口參數：無
*出口參數：無
*功能：PA9外部中斷服務函數，實現LED燈循環點亮間隔的改變
******************************************************/
void EXTI9_5_IRQHandler(void)
{
EXTI_ClearITPendingBit(EXTI_Line9);//清除PA9掛起位
if((GPIO_ReadInputData(GPIOA)&0x0200) == 0)
{
delay(500000);//延時去抖動
if((GPIO_ReadInputData(GPIOA)&0x0200) == 0)
{
QiBu_Init2();
CaiYang();
while(qian>0x200)
{
CaiYang();
delay(10000);
}
Init_Motor(400,ZhengZhuan,400,ZhengZhuan);//直行
delay(15000000);
}
}
}
const u16 juli[201]={
2500,2487,2475,2463,2450,2439,2427,2415,
2403,2392,2380,2369,2358,2347,2336,2325,
2314,2304,2293,2283,2272,2262,2252,2242,
2232,2222,2212,2202,2192,2183,2173,2164,
2155,2145,2136,2127,2118,2109,2100,2092,
2083,2074,2066,2057,2049,2040,2032,2024,
2016,2008,2000,1992,1984,1976,1968,1960,
1953,1945,1937,1930,1923,1915,1908,1901,
1893,1886,1879,1872,1865,1858,1851,1845,
1838,1831,1824,1818,1811,1805,1798,1792,
1785,1779,1773,1766,1760,1754,1748,1742,
1736,1730,1724,1718,1712,1706,1700,1694,
1689,1683,1677,1672,1666,1661,1655,1650,
1644,1639,1633,1628,1623,1618,1612,1607,
1602,1597,1592,1587,1582,1577,1572,1567,
1562,1557,1552,1547,1543,1538,1533,1529,
1524,1519,1515,1510,1506,1501,1497,1492,
1488,1483,1479,1474,1470,1466,1461,1457,
1453,1449,1445,1440,1436,1432,1428,1424,
1420,1416,1412,1408,1404,1400,1396,1392,
1388,1385,1381,1377,1373,1369,1366,1362,
1358,1355,1351,1347,1344,1340,1336,1333,
1329,1326,1322,1319,1315,1312,1308,1305,
1302,1298,1295,1291,1288,1285,1282,1278,
1275,1272,1269,1265,1262,1259,1256,1253,
1250
};