標題: 迷宮機器人的程序，基于stm32的開發(fā)板寫的，用了pid算法 [打印本頁]
作者: qq879491397    時間: 2018-12-17 23:40
標題: 迷宮機器人的程序，基于stm32的開發(fā)板寫的，用了pid算法
  1. #include "stm32f10x.h"
  2. #include "stdio.h"
  3. #include "delay.h"
  4. #include "tft.h"
  5. #include "math.h"
  6. #include "stm32_init.h"
  7. #include "motor.h"
  8. #include "pid.h"
  9. #include "math.h"

  11. #define pi 3.1415926535898
  12. #define TwoPi 6.2831853071796

  14. #define ZhiXing 1
  15. #define HouZhuan 2
  16. #define ZuoZhuan 3
  17. #define YouZhuan 4

  19. #define ZhengZhuan 0
  20. #define FanZhuan 1

  22. #define NoWall 0
  23. #define YouGenQiang 1
  24. #define ZuoGenQiang 2
  25. //#define ShuangGenQiang 3
  26. #define YouGenQiangXia 4
  27. #define ZuoGenQiangXia 5
  28. //#define ShuangGenQiangXia 6

  30. #define X_ZhengFangXiang 1
  31. #define Y_ZhengFangXiang 2
  32. #define X_FuFangXiang 3
  33. #define Y_FuFangXiang 4

  35. #define YiGeTime1  96000        //95000
  36. //#define YiGeTime2 94000
  37. #define YiGeTime3 99000

  39. #define BanGeTime1 47500

  41. #define ZhongDianZuoBiao 7

  43. #define Active 1
  44. #define Dead 2

  46. #define FenZhiID_MAX_3FangXiang 2
  47. #define FenZhiID_MAX_2FangXiang 1

  49. #define XunLu 0
  50. #define FanHui 1
  51. #define ChongCi 2

  53. #define YES 1
  54. #define NO 0

  56. #define XiuZhengZhuanWanYiGeTime 85000  //90000

  58. #define GenQiangYuZhi1 0x80
  59. #define GenQiangYuZhi2 0xff

  61. #define MAX_SuDu 400
  62. #define MIN_SuDu 500

  64. u16 tim2_count=0;//程序循環(huán)周期
  65. u16 shuju[4];//定義第一組、2、4、6、8
  66. u16 shuju1[4];//定義第二組、1、3、5、7
  67. u16 qian,hou,zuo,you,zuoshang,zuoxia,youshang,youxia;//&0xf00
  68. u16 youshang2,youxia2,zuoshang2,zuoxia2;//&0xfc0,0xf80
  69. u16 youshang3,youxia3,zuoshang3,zuoxia3;//

  71. u8 GenQiangFlag=0;//跟墻標志，做跟、右跟、雙跟
  72. u8 YunXingFlag=0;//運行標志，直行、左轉(zhuǎn)、右轉(zhuǎn)、后轉(zhuǎn)
  73. u8 QianJinFangXiang=0;//前進方向，Y正、Y負、X正、X負
  74. u8 XunLuFanHuiChongCiFlag=XunLu;//尋路返回沖刺標志

  76. u8 YiZouGuoGeShu=0;//已經(jīng)走過的格數(shù)
  77. u8 FenZhiDuiZhan=0;//分支堆棧
  78. //u8 FenZhiID=0;//分支ID
  79. u8 FenZhiID_MAX=0;//分支ID的最大值
  80. u8 CunChuLuJingShuZuXiaBiao=0;//存儲路徑數(shù)組下標
  81. u8 ChongCiLuJingXiaBiao=0;//沖刺路徑下標
  82. u32 YiGeTime_PanDuan=0;//判斷一格的時間，默認為時間1，轉(zhuǎn)彎后為時間2，下次滿格變成時間3
  83. u8 LianXuZhiXing_Count=0;//判斷是否連續(xù)直行，連續(xù)直行需要改變一格時間

  85. u8 YouShangGenQiang_YuZhi=0;//右上跟墻閾值
  86. u8 YouXiaGenQiang_YuZhi=0;//右下跟墻閾值
  87. u8 ZuoShangGenQiang_YuZhi=0;//左上跟墻閾值
  88. u8 ZuoXiaGenQiang_YuZhi=0;//左下跟墻閾值

  90. u8 YouShangGenQiang_Count=0;//右上跟墻計數(shù)
  91. u8 YouXiaGenQiang_Count=0;//右下跟墻計數(shù)
  92. u8 ZuoShangGenQiang_Count=0;//左上跟墻計數(shù)
  93. u8 ZuoXiaGenQiang_Count=0;//左下跟墻計數(shù)

  95. const u16 juli[201];//查表，速度與距離的關系
  96. const int sin_figure[451];

  98. u8 LuJingZuoBiao[128][2];//路徑坐標
  99. u8 FenZhiDianZuoBiao[64][2];//分支點坐標
  100. u8 FenZhiID[64];//分支ID
  101. u8 FenZhiQiWangFangXiang[64][4]; //分支期望方向
  102. u8 LuJingQiWangFangXiang[1];//路徑期望方向

  104. float zuolun=0;
  105. float youlun=0;
  106. u16 zuolunint=0;
  107. u16 youlunint=0;

  109. int x_point=0;
  110. int y_point=0;
  111. u32 zuolun_juli=0;
  112. u32 youlun_juli=0;

  114. u32 hecheng_juli=0;//合成距離

  116. u8 FenZhi_ZhiZhen=0;


  119. void CaiYang(void);
  120. void XianShiZhi(void);
  121. int ABS_2(int x);
  122. void QianXiangPanDuan(void);//前向判斷
  123. void YouZhuanChuLi(void);
  124. void ZuoZhuanChuLi(void);
  125. void HouZhuanChuLi(void);
  126. void JiSuanZuoBiao(void);//計算坐標
  127. void QiBu_Init(void);//起步初始化
  128. void JiLuFenZhiDianJiXuanZeFangXiang(void);//記錄分支點及選擇方向
  129. void GenJuQiWangFangXiangJiSuanQianJinFangXiang(void);//根據(jù)期望方向計算前進方向
  130. void XianLuJing(void);//顯示路徑坐標
  131. void GenJuLuJingQiWangFangXiangJiSuanQianJinFangXiang(void);//根據(jù)路徑期望方向計算前進方向
  132. void AnLuJingFanHui(void);//按路徑返回
  133. void GenQiangZiChengXu(void);//跟墻子程序
  134. void AnLuJingChongCi(void);//按路徑?jīng)_刺
  135. void ZhuanWanQianDaoWei(void);//轉(zhuǎn)彎前到位
  136. void ChongCiQianXiangPanDuan(void);
  137. void QiBu_Init2(void);

  139. /******************************************************
  140. *入口參數(shù)：無
  141. *出口參數(shù)：無
  142. *功能：主函數(shù)
  143. ******************************************************/
  144. int main(void)
  145. {
  146.         
  147.         RCC_Configuration();//RCC配置
  148.         GPIO_Configuration();//GPIO配置
  149.         EXTI_Configuration();//初始化PA9外部中斷
  150.         NVIC_Configuration();//嵌套向量中斷控制器配置
  151.         TIM2_Int_Init(999,359);//tim2初始化
  152.         SPI_Configuration();
  153.         GPIO_SetBits(GPIOE,GPIO_Pin_15);//打開TFT電源
  154.         delay(0xffffff);//延時
  155.         Lcd_Init();//初始化TFT屏
  156.         LCD_Clear(0);//清屏顯示黑色        
  157.         QiBu_Init();//起步初始化
  158.         
  159.         CaiYang();
  160.         while(qian>0x200)
  161.         {
  162.                 CaiYang();
  163.                 delay(10000);
  164.         }
  165.         
  166.         Init_Motor(400,ZhengZhuan,400,ZhengZhuan);//直行
  167.         delay(15000000);
  168.         
  169.         while(1)
  170.         {
  171.                 if(tim2_count>=20)//20*5ms=100ms
  172.                 {
  173.                         tim2_count=0;        

  175.                         if(YunXingFlag==ZhiXing)//2800//3300
  176.                         {
  177.                                 zuolun_juli = juli[zuolunint-400];
  178.                                 youlun_juli = juli[youlunint-400];
  179.                                 hecheng_juli += (zuolun_juli + youlun_juli);
  180.                         }
  181.                         if(hecheng_juli>YiGeTime_PanDuan)//
  182.                         {
  183.                                 hecheng_juli=0;                        
  184.                                 JiSuanZuoBiao();
  185.                         }
  186.                         CaiYang();
  187.                         if(XunLuFanHuiChongCiFlag==XunLu)//尋路
  188.                         {
  189.                                 QianXiangPanDuan();
  190.                         }
  191.                         else if(XunLuFanHuiChongCiFlag==ChongCi)
  192.                         {
  193.                                 ChongCiQianXiangPanDuan();
  194.                         }
  195.                 }
  196.         }
  197. }        
  198. /**********************沖刺前向判斷***********************/
  199. void ChongCiQianXiangPanDuan(void)
  200. {
  201.         char msg[20];
  202.         if(qian>0x500)//qian zhuang qiang
  203.         {               
  204.                 if((you<0x200)&&(zuo<0x200))
  205.                 {
  206.                                 if(x_point==LuJingZuoBiao[ChongCiLuJingXiaBiao+1][0])
  207.                                 {
  208.                                                 if(LuJingZuoBiao[ChongCiLuJingXiaBiao+1][1]<y_point)
  209.                                                 {
  210.                                                         LuJingQiWangFangXiang[0]=Y_FuFangXiang;
  211.                                                 }
  212.                                                 else if(LuJingZuoBiao[ChongCiLuJingXiaBiao+1][1]>y_point)
  213.                                                 {
  214.                                                         LuJingQiWangFangXiang[0]=Y_ZhengFangXiang;
  215.                                                 }
  216.                                 }
  217.                                 else if(y_point==LuJingZuoBiao[ChongCiLuJingXiaBiao+1][1])
  218.                                 {
  219.                                                 if(LuJingZuoBiao[ChongCiLuJingXiaBiao+1][0]<x_point)
  220.                                                 {
  221.                                                         LuJingQiWangFangXiang[0]=X_FuFangXiang;
  222.                                                 }
  223.                                                 else if(LuJingZuoBiao[ChongCiLuJingXiaBiao+1][0]>x_point)
  224.                                                 {
  225.                                                         LuJingQiWangFangXiang[0]=X_ZhengFangXiang;
  226.                                                 }
  227.                                 }
  228.                                 else
  229.                                 {
  230.                                         Off_Motor(3);
  231.                                         LCD_ShowString(0,0,"cuo wu");
  232.                                        
  233.                                         sprintf(msg,"x_zb=%1d",LuJingZuoBiao[ChongCiLuJingXiaBiao+1][0]);
  234.                                         LCD_ShowString(0,96,msg);
  235.                                         sprintf(msg,"y_zb=%1d",LuJingZuoBiao[ChongCiLuJingXiaBiao+1][1]);
  236.                                         LCD_ShowString(0,128,msg);
  237.                                         while(1);
  238.                                 }
  239.                                 GenJuLuJingQiWangFangXiangJiSuanQianJinFangXiang();
  240.                 }
  241.                 else if(you<0x200)//bu shi fen zhi dian
  242.                 {
  243.                         YouZhuanChuLi();
  244.                 }
  245.                 else if(zuo<0x200)//bu shi fen zhi dian
  246.                 {
  247.                         ZuoZhuanChuLi();
  248.                 }
  249.                 else//shi si hu tong
  250.                 {
  251.                         HouZhuanChuLi();
  252.                 }
  253.         }
  254.         else//qian wu qiang bi
  255.         {
  256.                 if(hecheng_juli==0)//zheng ge pan duan
  257.                 {
  258.                         if((qian<0x80)&&((zuo<0x200)||(you<0x200)))
  259.                         {
  260.                                         if(x_point==LuJingZuoBiao[ChongCiLuJingXiaBiao+1][0])
  261.                                         {
  262.                                                         if(LuJingZuoBiao[ChongCiLuJingXiaBiao+1][1]<y_point)
  263.                                                         {
  264.                                                                 LuJingQiWangFangXiang[0]=Y_FuFangXiang;
  265.                                                         }
  266.                                                         else if(LuJingZuoBiao[ChongCiLuJingXiaBiao+1][1]>y_point)
  267.                                                         {
  268.                                                                 LuJingQiWangFangXiang[0]=Y_ZhengFangXiang;
  269.                                                         }
  270.                                         }
  271.                                         else if(y_point==LuJingZuoBiao[ChongCiLuJingXiaBiao+1][1])
  272.                                         {
  273.                                                         if(LuJingZuoBiao[ChongCiLuJingXiaBiao+1][0]<x_point)
  274.                                                         {
  275.                                                                 LuJingQiWangFangXiang[0]=X_FuFangXiang;
  276.                                                         }
  277.                                                         else if(LuJingZuoBiao[ChongCiLuJingXiaBiao+1][0]>x_point)
  278.                                                         {
  279.                                                                 LuJingQiWangFangXiang[0]=X_ZhengFangXiang;
  280.                                                         }
  281.                                         }
  282.                                         else
  283.                                         {
  284.                                                 Off_Motor(3);
  285.                                                 LCD_ShowString(0,0,"cuo wu");
  286.                                                 
  287.                                                 sprintf(msg,"x_zb=%1d",LuJingZuoBiao[ChongCiLuJingXiaBiao+1][0]);
  288.                                                 LCD_ShowString(0,96,msg);
  289.                                                 sprintf(msg,"y_zb=%1d",LuJingZuoBiao[ChongCiLuJingXiaBiao+1][1]);
  290.                                                 LCD_ShowString(0,128,msg);
  291.                                                 while(1);
  292.                                         }
  293.                                         GenJuLuJingQiWangFangXiangJiSuanQianJinFangXiang();
  294.                         }
  295.                         else
  296.                         {                        
  297.                                 GenQiangZiChengXu();
  298.                         }
  299.                 }
  300.                 else
  301.                 {
  302.                         GenQiangZiChengXu();
  303.                 }
  304.         }
  305. }
  306. /**********************前向判斷***********************/
  307. void QianXiangPanDuan(void)
  308. {
  309.         if(qian>0x500)//qian zhuang qiang
  310.         {               
  311.                 if((you<0x200)&&(zuo<0x200))
  312.                 {
  313.                                 JiLuFenZhiDianJiXuanZeFangXiang();
  314.                                 GenJuQiWangFangXiangJiSuanQianJinFangXiang();
  315.                 }
  316.                 else if(you<0x200)//bu shi fen zhi dian
  317.                 {
  318.                         YouZhuanChuLi();
  319.                 }
  320.                 else if(zuo<0x200)//bu shi fen zhi dian
  321.                 {
  322.                         ZuoZhuanChuLi();
  323.                 }
  324.                 else//shi si hu tong
  325.                 {
  326.                         HouZhuanChuLi();
  327.                 }
  328.         }
  329.         else//qian wu qiang bi
  330.         {
  331.                 if(hecheng_juli==0)//zheng ge pan duan
  332.                 {
  333.                         if((qian<0x80)&&((zuo<0x200)||(you<0x200)))
  334.                         {
  335.                                         JiLuFenZhiDianJiXuanZeFangXiang();
  336.                                         GenJuQiWangFangXiangJiSuanQianJinFangXiang();
  337.                         }
  338.                         else
  339.                         {                        
  340.                                 GenQiangZiChengXu();
  341.                         }
  342.                 }
  343.                 else
  344.                 {
  345.                         GenQiangZiChengXu();
  346.                 }
  347.         }
  348. }
  349. /**********************跟墻子程序***********************/
  350. void GenQiangZiChengXu(void)
  351. {        
  352. //        float you1,zuo1;
  353.         
  354.         YunXingFlag=ZhiXing;
  355.         
  356.         if(youshang>0x300)
  357.         {
  358.                 zuolunint=MIN_SuDu;
  359.                 youlunint=MAX_SuDu;
  360.         }
  361.         else if(zuoshang>0x300)
  362.         {
  363.                 zuolunint=MAX_SuDu;
  364.                 youlunint=MIN_SuDu;
  365.         }
  366.         else
  367.         {
  368.                         if(you>0x100)
  369.                         {
  370.                                         if((youshang>0x80)&&(youshang<0x300))
  371.                                         {
  372.                                                 if(GenQiangFlag != YouGenQiang)
  373.                                                 {
  374.                                                         YouShangGenQiang_Count++;
  375.                                                         YouXiaGenQiang_Count=0;
  376.                                                         ZuoShangGenQiang_Count=0;
  377.                                                         ZuoXiaGenQiang_Count=0;
  378.                                                         if(YouShangGenQiang_Count>2)
  379.                                                         {
  380.                                                                 YouShangGenQiang_Count=0;
  381.                                                                
  382.                                                                 GenQiangFlag=YouGenQiang;
  383.                                                                 PID_init2();
  384.                                                                 PID_realize(0x180,youshang3,YouGenQiang);//0x180
  385.                                                         }
  386.                                                 }
  387.                                                 else
  388.                                                 {
  389.                                                         PID_realize(0x180,youshang3,YouGenQiang);//0x180
  390.                                                 }                        
  391.                                         }
  392.                                         else if((youxia>0x80)&&(youxia<0x300))
  393.                                         {
  394.                                                 if(GenQiangFlag == YouGenQiang)
  395.                                                 {
  396.                                                         YouShangGenQiang_Count=0;
  397.                                                         YouXiaGenQiang_Count=0;
  398.                                                         ZuoShangGenQiang_Count=0;
  399.                                                         ZuoXiaGenQiang_Count=0;
  400.                                                         
  401.                                                         GenQiangFlag=YouGenQiangXia;
  402.                                                         PID_init2();
  403.                                                         PID_realize2(0x180,youxia3,YouGenQiangXia);//0x180
  404.                                                 }
  405.                                                 else if((GenQiangFlag != YouGenQiangXia)&&(youxia>0x100))
  406.                                                 {
  407.                                                         YouShangGenQiang_Count=0;
  408.                                                         YouXiaGenQiang_Count++;
  409.                                                         ZuoShangGenQiang_Count=0;
  410.                                                         ZuoXiaGenQiang_Count=0;
  411.                                                         if(YouXiaGenQiang_Count>2)
  412.                                                         {
  413.                                                                 YouXiaGenQiang_Count=0;
  414.                                                                
  415.                                                                 GenQiangFlag=YouGenQiangXia;
  416.                                                                 PID_init2();
  417.                                                                 PID_realize2(0x180,youxia3,YouGenQiangXia);//0x180
  418.                                                         }
  419.                                                 }
  420.                                                 else if(GenQiangFlag==YouGenQiangXia)
  421.                                                 {
  422.                                                         PID_realize2(0x180,youxia3,YouGenQiangXia);//0x180
  423.                                                 }                        
  424.                                         }
  425.                         }
  426.                         else if(zuo>0x100)
  427.                         {
  428.                                         if((zuoshang>0x80)&&(zuoshang<0x300))
  429.                                         {
  430.                                                 if(GenQiangFlag != ZuoGenQiang)
  431.                                                 {
  432.                                                         YouShangGenQiang_Count=0;
  433.                                                         YouXiaGenQiang_Count=0;
  434.                                                         ZuoShangGenQiang_Count++;
  435.                                                         ZuoXiaGenQiang_Count=0;
  436.                                                         if(ZuoShangGenQiang_Count>2)
  437.                                                         {
  438.                                                                 ZuoShangGenQiang_Count=0;
  439.                                                                
  440.                                                                 GenQiangFlag=ZuoGenQiang;
  441.                                                                 PID_init3();
  442.                                                                 PID_realize(0x140,zuoshang3,ZuoGenQiang);//0x180
  443.                                                         }
  444.                                                 }
  445.                                                 else
  446.                                                 {
  447.                                                         PID_realize(0x140,zuoshang3,ZuoGenQiang);//0x180
  448.                                                 }        
  449.                                         }
  450.                                         else if((zuoxia>0x80)&&(zuoxia<0x300))
  451.                                         {
  452.                                                 if(GenQiangFlag == ZuoGenQiang)
  453.                                                 {
  454.                                                         YouShangGenQiang_Count=0;
  455.                                                         YouXiaGenQiang_Count=0;
  456.                                                         ZuoShangGenQiang_Count=0;
  457.                                                         ZuoXiaGenQiang_Count=0;
  458.                                                         
  459.                                                         GenQiangFlag=ZuoGenQiangXia;
  460.                                                         PID_init3();
  461.                                                         PID_realize2(0x140,zuoxia3,ZuoGenQiangXia);//0x180
  462.                                                 }
  463.                                                 else if((GenQiangFlag != ZuoGenQiangXia)&&(zuoxia>0x100))
  464.                                                 {
  465.                                                         YouShangGenQiang_Count=0;
  466.                                                         YouXiaGenQiang_Count=0;
  467.                                                         ZuoShangGenQiang_Count=0;
  468.                                                         ZuoXiaGenQiang_Count++;
  469.                                                         if(ZuoXiaGenQiang_Count>2)
  470.                                                         {
  471.                                                                 ZuoXiaGenQiang_Count=0;
  472.                                                                
  473.                                                                 GenQiangFlag=ZuoGenQiangXia;
  474.                                                                 PID_init3();
  475.                                                                 PID_realize2(0x140,zuoxia3,ZuoGenQiangXia);//0x180
  476.                                                         }
  477.                                                 }
  478.                                                 else if(GenQiangFlag==ZuoGenQiangXia)
  479.                                                 {
  480.                                                         PID_realize2(0x140,zuoxia3,ZuoGenQiangXia);//0x180
  481.                                                 }
  482.                                         }
  483.                         }
  484.                         else if((zuo<0x80)&&(you<0x80)&&(hou<0x80))
  485.                         {
  486.                                         GenQiangFlag=NoWall;
  487. //                                        if(qian>0x80)
  488. //                                        {
  489. //                                                PID_init4();
  490. //                                                you1=youshang2;
  491. //                                                zuo1=zuoshang2;
  492. //                                                PID_realize(0,ABS(you1-zuo1),NoWall);
  493. //                                        }
  494. //                                        else
  495. //                                        {
  496.                                                 zuolunint=MAX_SuDu;
  497.                                                 youlunint=MAX_SuDu;
  498. //                                        }
  499.                         }               
  500.         }               
  501.         
  502.         if(zuolunint==0)
  503.         {
  504.                 zuolunint=400;
  505.         }
  506.         if(youlunint==0)
  507.         {
  508.                 youlunint=400;
  509.         }
  510.                         
  511.         Init_Motor(zuolunint,ZhengZhuan,youlunint,ZhengZhuan);//直行
  512. }
  513. /**********************計算坐標***********************/
  514. void JiSuanZuoBiao(void)
  515. {
  516.         u8 i,j;
  517.         char msg[20];

  519.         if(QianJinFangXiang==Y_ZhengFangXiang)
  520.         {
  521.                 y_point += 1;
  522.         }
  523.         else if(QianJinFangXiang==Y_FuFangXiang)
  524.         {
  525.                 y_point -= 1;
  526.         }
  527.         else if(QianJinFangXiang==X_ZhengFangXiang)
  528.         {
  529.                 x_point += 1;
  530.         }
  531.         else if(QianJinFangXiang==X_FuFangXiang)
  532.         {
  533.                 x_point -= 1;
  534.         }
  535.         
  536.         if(y_point<0)
  537.         {
  538.                 y_point=0;
  539.         }
  540.         else if(y_point>ZhongDianZuoBiao)
  541.         {
  542.                 y_point=ZhongDianZuoBiao;
  543.         }
  544.         if(x_point<0)
  545.         {
  546.                 x_point=0;
  547.         }
  548.         else if(x_point>ZhongDianZuoBiao)
  549.         {
  550.                 x_point=ZhongDianZuoBiao;
  551.         }
  552.         
  553. //        sprintf(msg,"FenZhi_ZhiZhen=%1d",FenZhi_ZhiZhen);
  554. //        LCD_ShowString(0,64,msg);
  555. //        sprintf(msg,"FenZhiDuiZhan=%1d",FenZhiDuiZhan);
  556. //        LCD_ShowString(0,96,msg);
  557.         
  558.         sprintf(msg,"x_point=%1d",x_point);
  559.         LCD_ShowString(0,160,msg);
  560.         sprintf(msg,"y_point=%1d",y_point);
  561.         LCD_ShowString(0,192,msg);               
  562.         
  563.         if(XunLuFanHuiChongCiFlag==XunLu)
  564.         {
  565.                         if((LuJingZuoBiao[YiZouGuoGeShu-1][0]==x_point)&&(LuJingZuoBiao[YiZouGuoGeShu-1][1]==y_point))
  566.                         {
  567.                                 return;
  568.                         }
  569.                         for(i=0;i<YiZouGuoGeShu;i++)
  570.                         {
  571.                                         if((LuJingZuoBiao[i][0]==x_point)&&(LuJingZuoBiao[i][1]==y_point))
  572.                                         {
  573.                                                 YiZouGuoGeShu=i;
  574.                                                 for(j=0;j<FenZhiDuiZhan+1;j++)
  575.                                                 {
  576.                                                                 if((FenZhiDianZuoBiao[j][0]==x_point)&&(FenZhiDianZuoBiao[j][1]==y_point))
  577.                                                                 {
  578.                                                                                 FenZhiID[j]++;
  579.                                                                                 FenZhi_ZhiZhen=j;
  580.                                                                                 break;
  581.                                                                 }
  582.                                                 }
  583.                                                 break;
  584.                                         }
  585.                         }
  586.                         
  587.                         LuJingZuoBiao[YiZouGuoGeShu][0]=x_point;
  588.                         LuJingZuoBiao[YiZouGuoGeShu][1]=y_point;

  590.                         YiZouGuoGeShu++;
  591.                         
  592.                         if((x_point==ZhongDianZuoBiao)&&(y_point==ZhongDianZuoBiao))
  593.                         {
  594.                                         Init_Motor(400,ZhengZhuan,400,ZhengZhuan);//直行
  595.                                         delay(15000000);
  596.                                         Off_Motor(3);        
  597.                                         LCD_Clear(0);//清屏顯示黑色        
  598.                                         XianLuJing();               
  599.                                         while(XunLuFanHuiChongCiFlag==XunLu);
  600.                         }
  601.         }
  602.         else if(XunLuFanHuiChongCiFlag==ChongCi)
  603.         {
  604.                 ChongCiLuJingXiaBiao++;
  605.                 if((x_point==ZhongDianZuoBiao)&&(y_point==ZhongDianZuoBiao))
  606.                 {
  607.                         Init_Motor(400,ZhengZhuan,400,ZhengZhuan);//直行
  608.                         delay(15000000);
  609.                         Off_Motor(3);
  610.                         LCD_Clear(0);//清屏顯示黑色
  611.                         LCD_ShowString(0,0,"success");
  612.                         while(1);
  613.                 }
  614.         }
  615. }
  616. /**********************記錄分支點及選擇方向***********************/
  617. void JiLuFenZhiDianJiXuanZeFangXiang(void)
  618. {
  619.         u8 i;
  620.         if( (x_point==0) && (y_point==0) )
  621.         {
  622.                 return;
  623.         }
  624.         if( (x_point==FenZhiDianZuoBiao[FenZhiDuiZhan][0]) && (y_point==FenZhiDianZuoBiao[FenZhiDuiZhan][1]) )
  625.         {
  626.                 return;
  627.         }
  628.         for(i=0;i<FenZhiDuiZhan;i++)
  629.         {
  630.                 if( (x_point==FenZhiDianZuoBiao[i][0]) && (y_point==FenZhiDianZuoBiao[i][1]) )
  631.                 {
  632. //                        FenZhiID[i]++;
  633. //                        FenZhi_ZhiZhen=i;
  634.                         return;
  635.                 }
  636.         }
  637.         
  638.         FenZhiDuiZhan++;
  639.         
  640.         FenZhiDianZuoBiao[FenZhiDuiZhan][0]=x_point;
  641.         FenZhiDianZuoBiao[FenZhiDuiZhan][1]=y_point;

  643.         FenZhi_ZhiZhen=FenZhiDuiZhan;        
  644.         
  645.         if((qian<0x80)&&(zuo<0x200)&&(you<0x200))//qian zuo you
  646.         {
  647.                 FenZhiID_MAX=FenZhiID_MAX_3FangXiang;
  648.                 if(QianJinFangXiang==Y_ZhengFangXiang)
  649.                 {
  650.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][0]=Y_ZhengFangXiang;
  651.                         
  652.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][1]=X_ZhengFangXiang;
  653.                         
  654.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][2]=X_FuFangXiang;
  655.                         
  656.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][3]=Y_FuFangXiang;
  657.                 }
  658.                 else if(QianJinFangXiang==Y_FuFangXiang)
  659.                 {
  660.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][0]=X_ZhengFangXiang;
  661.                         
  662.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][1]=Y_FuFangXiang;
  663.                         
  664.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][2]=X_FuFangXiang;
  665.                         
  666.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][3]=Y_ZhengFangXiang;
  667.                 }
  668.                 else if(QianJinFangXiang==X_ZhengFangXiang)
  669.                 {
  670.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][0]=X_ZhengFangXiang;
  671.                         
  672.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][1]=Y_ZhengFangXiang;
  673.                         
  674.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][2]=Y_FuFangXiang;
  675.                         
  676.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][3]=X_FuFangXiang;
  677.                 }
  678.                 else if(QianJinFangXiang==X_FuFangXiang)
  679.                 {
  680.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][0]=Y_ZhengFangXiang;
  681.                         
  682.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][1]=X_FuFangXiang;
  683.                         
  684.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][2]=Y_FuFangXiang;
  685.                         
  686.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][3]=X_ZhengFangXiang;
  687.                 }
  688.         }
  689.         else if((qian<0x80)&&(zuo<0x200))//qian zuo
  690.         {
  691.                 FenZhiID_MAX=FenZhiID_MAX_2FangXiang;
  692.                 if(QianJinFangXiang==Y_ZhengFangXiang)
  693.                 {
  694.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][0]=Y_ZhengFangXiang;
  695.                         
  696.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][1]=X_FuFangXiang;
  697.                         
  698.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][2]=Y_FuFangXiang;
  699.                 }
  700.                 else if(QianJinFangXiang==Y_FuFangXiang)
  701.                 {
  702.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][0]=X_ZhengFangXiang;
  703.                         
  704.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][1]=Y_FuFangXiang;
  705.                         
  706.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][2]=Y_ZhengFangXiang;
  707.                 }
  708.                 else if(QianJinFangXiang==X_ZhengFangXiang)
  709.                 {
  710.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][0]=X_ZhengFangXiang;
  711.                         
  712.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][1]=Y_ZhengFangXiang;
  713.                         
  714.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][2]=X_FuFangXiang;
  715.                 }
  716.                 else if(QianJinFangXiang==X_FuFangXiang)
  717.                 {
  718.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][0]=X_FuFangXiang;
  719.                         
  720.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][1]=Y_FuFangXiang;
  721.                         
  722.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][2]=X_ZhengFangXiang;
  723.                 }
  724.         }
  725.         else if((qian<0x80)&&(you<0x200))//qian you
  726.         {
  727.                 FenZhiID_MAX=FenZhiID_MAX_2FangXiang;
  728.                 if(QianJinFangXiang==Y_ZhengFangXiang)
  729.                 {
  730.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][0]=Y_ZhengFangXiang;
  731.                         
  732.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][1]=X_ZhengFangXiang;
  733.                         
  734.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][2]=Y_FuFangXiang;
  735.                 }
  736.                 else if(QianJinFangXiang==Y_FuFangXiang)
  737.                 {
  738.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][0]=Y_FuFangXiang;
  739.                         
  740.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][1]=X_FuFangXiang;
  741.                         
  742.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][2]=Y_ZhengFangXiang;
  743.                 }
  744.                 else if(QianJinFangXiang==X_ZhengFangXiang)
  745.                 {
  746.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][0]=X_ZhengFangXiang;
  747.                         
  748.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][1]=Y_FuFangXiang;
  749.                         
  750.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][2]=X_FuFangXiang;
  751.                 }
  752.                 else if(QianJinFangXiang==X_FuFangXiang)
  753.                 {
  754.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][0]=Y_ZhengFangXiang;
  755.                         
  756.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][1]=X_FuFangXiang;
  757.                         
  758.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][2]=X_ZhengFangXiang;
  759.                 }
  760.         }
  761.         else if((zuo<0x200) &&(you<0x200))
  762.         {
  763.                 FenZhiID_MAX=FenZhiID_MAX_2FangXiang;
  764.                 if(QianJinFangXiang==Y_ZhengFangXiang)
  765.                 {
  766.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][0]=X_ZhengFangXiang;
  767.                         
  768.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][1]=X_FuFangXiang;
  769.                         
  770.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][2]=Y_FuFangXiang;
  771.                 }
  772.                 else if(QianJinFangXiang==Y_FuFangXiang)
  773.                 {
  774.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][0]=X_ZhengFangXiang;
  775.                         
  776.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][1]=X_FuFangXiang;
  777.                         
  778.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][2]=Y_ZhengFangXiang;
  779.                 }
  780.                 else if(QianJinFangXiang==X_ZhengFangXiang)
  781.                 {
  782.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][0]=Y_ZhengFangXiang;
  783.                         
  784.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][1]=Y_FuFangXiang;
  785.                         
  786.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][2]=X_FuFangXiang;
  787.                 }
  788.                 else if(QianJinFangXiang==X_FuFangXiang)
  789.                 {
  790.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][0]=Y_ZhengFangXiang;
  791.                         
  792.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][1]=Y_FuFangXiang;
  793.                         
  794.                         FenZhiQiWangFangXiang[FenZhiDuiZhan][2]=X_ZhengFangXiang;
  795.                 }
  796.         }
  797. }

  799. /**********************根據(jù)期望方向計算前進方向***********************/
  800. void GenJuQiWangFangXiangJiSuanQianJinFangXiang(void)
  801. {
  802.         u8 FangXiangChaZhi;
  803.         
  804.         if(FenZhiQiWangFangXiang[FenZhi_ZhiZhen][FenZhiID[FenZhi_ZhiZhen]]<QianJinFangXiang)
  805.         {
  806.                 FangXiangChaZhi=QianJinFangXiang-FenZhiQiWangFangXiang[FenZhi_ZhiZhen][FenZhiID[FenZhi_ZhiZhen]];
  807.                 if(FangXiangChaZhi==2)
  808.                 {
  809.                         FenZhiID[FenZhi_ZhiZhen]++;
  810.                 }
  811.         }
  812.         else
  813.         {
  814.                 FangXiangChaZhi=FenZhiQiWangFangXiang[FenZhi_ZhiZhen][FenZhiID[FenZhi_ZhiZhen]]-QianJinFangXiang;
  815.                 if(FangXiangChaZhi==2)
  816.                 {
  817.                         FenZhiID[FenZhi_ZhiZhen]++;
  818.                 }
  819.         }
  820.         
  821.         if(FenZhiQiWangFangXiang[FenZhi_ZhiZhen][FenZhiID[FenZhi_ZhiZhen]]<QianJinFangXiang)
  822.         {
  823.                 FangXiangChaZhi=QianJinFangXiang-FenZhiQiWangFangXiang[FenZhi_ZhiZhen][FenZhiID[FenZhi_ZhiZhen]];
  824.                 switch(FangXiangChaZhi)
  825.                 {
  826.                         case 1:YouZhuanChuLi();break;//delay(1000000);
  827. //                        case 2:HouZhuanChuLi();break;
  828.                         case 3:ZuoZhuanChuLi();break;
  829.                         default:break;
  830.                 }
  831.         }
  832.         else
  833.         {
  834.                 FangXiangChaZhi=FenZhiQiWangFangXiang[FenZhi_ZhiZhen][FenZhiID[FenZhi_ZhiZhen]]-QianJinFangXiang;
  835.                 switch(FangXiangChaZhi)
  836.                 {
  837.                         case 0:break;
  838.                         case 1:ZuoZhuanChuLi();break;
  839. //                        case 2:HouZhuanChuLi();break;
  840.                         case 3:YouZhuanChuLi();break;
  841.                         default:break;
  842.                 }
  843.         }
  844. }
  845. /**********************起步初始化***********************/
  846. /**********************起步初始化***********************/
  847. void QiBu_Init2(void)
  848. {
  849.         u8 i;
  850.         
  851.         tim2_count=0;//程序循環(huán)周期

  853.         GenQiangFlag=0;//跟墻標志，做跟、右跟、雙跟
  854.         YunXingFlag=0;//運行標志，直行、左轉(zhuǎn)、右轉(zhuǎn)、后轉(zhuǎn)
  855.         XunLuFanHuiChongCiFlag=ChongCi;//尋路返回沖刺標志

  857.         FenZhiDuiZhan=0;//分支堆棧
  858. //        FenZhiID=0;//分支ID
  859.         FenZhiID_MAX=0;//分支ID的最大值
  860.         CunChuLuJingShuZuXiaBiao=0;//存儲路徑數(shù)組下標
  861.         ChongCiLuJingXiaBiao=0;//沖刺路徑下標
  862.         LianXuZhiXing_Count=0;//判斷是否連續(xù)直行，連續(xù)直行需要改變一格時間

  864.         zuolun=0;
  865.         youlun=0;
  866.         zuolunint=0;
  867.         youlunint=0;

  869.         x_point=0;
  870.         y_point=0;
  871.         zuolun_juli=0;
  872.         youlun_juli=0;
  873.         hecheng_juli=0;//合成距離
  874.         
  875.         QianJinFangXiang=Y_ZhengFangXiang;
  876.         YiZouGuoGeShu=1;
  877.         YiGeTime_PanDuan=YiGeTime1;
  878.         
  879.         for(i=0;i<64;i++)
  880.         {
  881.                 FenZhiDianZuoBiao[i][0]=0;
  882.                 FenZhiDianZuoBiao[i][1]=0;
  883.                 FenZhiID[i]=0;
  884.         }
  885.         for(i=0;i<64;i++)
  886.         {
  887.                 FenZhiQiWangFangXiang[i][0]=0;
  888.                 FenZhiQiWangFangXiang[i][1]=0;
  889.                 FenZhiQiWangFangXiang[i][2]=0;
  890.                 FenZhiQiWangFangXiang[i][3]=0;
  891.         }
  892.         LuJingQiWangFangXiang[0]=0;
  893.         
  894.         YouShangGenQiang_YuZhi=GenQiangYuZhi2;
  895.         YouXiaGenQiang_YuZhi=GenQiangYuZhi2;
  896.         ZuoShangGenQiang_YuZhi=GenQiangYuZhi2;
  897.         ZuoXiaGenQiang_YuZhi=GenQiangYuZhi2;
  898. }
  899. void QiBu_Init(void)
  900. {
  901.         u8 i;
  902.         
  903.         tim2_count=0;//程序循環(huán)周期

  905.         GenQiangFlag=0;//跟墻標志，做跟、右跟、雙跟
  906.         YunXingFlag=0;//運行標志，直行、左轉(zhuǎn)、右轉(zhuǎn)、后轉(zhuǎn)
  907.         XunLuFanHuiChongCiFlag=XunLu;//尋路返回沖刺標志

  909.         FenZhiDuiZhan=0;//分支堆棧
  910. //        FenZhiID=0;//分支ID
  911.         FenZhiID_MAX=0;//分支ID的最大值
  912.         CunChuLuJingShuZuXiaBiao=0;//存儲路徑數(shù)組下標
  913.         ChongCiLuJingXiaBiao=0;//沖刺路徑下標
  914.         LianXuZhiXing_Count=0;//判斷是否連續(xù)直行，連續(xù)直行需要改變一格時間

  916.         zuolun=0;
  917.         youlun=0;
  918.         zuolunint=0;
  919.         youlunint=0;

  921.         x_point=0;
  922.         y_point=0;
  923.         zuolun_juli=0;
  924.         youlun_juli=0;
  925.         hecheng_juli=0;//合成距離
  926.         
  927.         QianJinFangXiang=Y_ZhengFangXiang;
  928.         YiZouGuoGeShu=1;
  929.         YiGeTime_PanDuan=YiGeTime1;
  930.         LuJingZuoBiao[0][0]=0;
  931.         LuJingZuoBiao[0][1]=0;
  932.         
  933.         
  934.         for(i=0;i<128;i++)
  935.         {
  936.                 LuJingZuoBiao[i][0]=0;
  937.                 LuJingZuoBiao[i][1]=0;
  938.         }
  939.         for(i=0;i<64;i++)
  940.         {
  941.                 FenZhiDianZuoBiao[i][0]=0;
  942.                 FenZhiDianZuoBiao[i][1]=0;
  943.                 FenZhiID[i]=0;
  944.         }
  945.         for(i=0;i<64;i++)
  946.         {
  947.                 FenZhiQiWangFangXiang[i][0]=0;
  948.                 FenZhiQiWangFangXiang[i][1]=0;
  949.                 FenZhiQiWangFangXiang[i][2]=0;
  950.                 FenZhiQiWangFangXiang[i][3]=0;
  951.         }
  952.         LuJingQiWangFangXiang[0]=0;
  953.         
  954.         YouShangGenQiang_YuZhi=GenQiangYuZhi2;
  955.         YouXiaGenQiang_YuZhi=GenQiangYuZhi2;
  956.         ZuoShangGenQiang_YuZhi=GenQiangYuZhi2;
  957.         ZuoXiaGenQiang_YuZhi=GenQiangYuZhi2;
  958. }
  959. /**********************按路徑返回***********************/
  960. void AnLuJingFanHui(void)
  961. {
  962.         if(CunChuLuJingShuZuXiaBiao!=0)
  963.         {
  964.                 if(LuJingZuoBiao[CunChuLuJingShuZuXiaBiao][0]==LuJingZuoBiao[CunChuLuJingShuZuXiaBiao-1][0])
  965.                 {
  966.                                 if(LuJingZuoBiao[CunChuLuJingShuZuXiaBiao-1][1]<LuJingZuoBiao[CunChuLuJingShuZuXiaBiao][1])
  967.                                 {
  968.                                         LuJingQiWangFangXiang[0]=Y_FuFangXiang;
  969.                                 }
  970.                                 else
  971.                                 {
  972.                                         LuJingQiWangFangXiang[0]=Y_ZhengFangXiang;
  973.                                 }
  974.                 }
  975.                 else
  976.                 {
  977.                                 if(LuJingZuoBiao[CunChuLuJingShuZuXiaBiao-1][0]<LuJingZuoBiao[CunChuLuJingShuZuXiaBiao][0])
  978.                                 {
  979.                                         LuJingQiWangFangXiang[0]=X_FuFangXiang;
  980.                                 }
  981.                                 else
  982.                                 {
  983.                                         LuJingQiWangFangXiang[0]=X_ZhengFangXiang;
  984.                                 }
  985.                 }
  986.         }
  987. }

  989. /**********************按路徑?jīng)_刺***********************/
  990. void AnLuJingChongCi(void)
  991. {
  992.         if(ChongCiLuJingXiaBiao != CunChuLuJingShuZuXiaBiao)
  993.         {
  994.                 if(LuJingZuoBiao[ChongCiLuJingXiaBiao][0]==LuJingZuoBiao[ChongCiLuJingXiaBiao+1][0])
  995.                 {
  996.                                 if(LuJingZuoBiao[ChongCiLuJingXiaBiao+1][1]<LuJingZuoBiao[ChongCiLuJingXiaBiao][1])
  997.                                 {
  998.                                         LuJingQiWangFangXiang[0]=Y_FuFangXiang;
  999.                                 }
  1000.                                 else
  1001.                                 {
  1002.                                         LuJingQiWangFangXiang[0]=Y_ZhengFangXiang;
  1003.                                 }
  1004.                 }
  1005.                 else
  1006.                 {
  1007.                                 if(LuJingZuoBiao[ChongCiLuJingXiaBiao+1][0]<LuJingZuoBiao[ChongCiLuJingXiaBiao][0])
  1008.                                 {
  1009.                                         LuJingQiWangFangXiang[0]=X_FuFangXiang;
  1010.                                 }
  1011.                                 else
  1012.                                 {
  1013.                                         LuJingQiWangFangXiang[0]=X_ZhengFangXiang;
  1014.                                 }
  1015.                 }
  1016.         }
  1017. }

  1019. /**********************根據(jù)路徑期望方向計算前進方向***********************/
  1020. /**********************根據(jù)路徑期望方向計算前進方向***********************/
  1021. void GenJuLuJingQiWangFangXiangJiSuanQianJinFangXiang(void)
  1022. {
  1023. //        LianXuZhiXing_Count=0;
  1024. //                        ZhuanWanQianDaoWei();
  1025.         
  1026.         u8 FangXiangChaZhi;
  1027.         if(LuJingQiWangFangXiang[0]<QianJinFangXiang)
  1028.         {
  1029.                 FangXiangChaZhi=QianJinFangXiang-LuJingQiWangFangXiang[0];
  1030.                 switch(FangXiangChaZhi)
  1031.                 {
  1032.                         case 1:
  1033.                         YouZhuanChuLi();break;
  1034.                         case 3:
  1035.                         ZuoZhuanChuLi();break;
  1036.                         default:break;
  1037.                 }
  1038.         }
  1039.         else
  1040.         {
  1041.                 FangXiangChaZhi=LuJingQiWangFangXiang[0]-QianJinFangXiang;
  1042.                 switch(FangXiangChaZhi)
  1043.                 {
  1044.                         case 0:break;
  1045.                         case 1:
  1046.                         ZuoZhuanChuLi();break;
  1047.                         case 3:
  1048.                         YouZhuanChuLi();break;
  1049.                         default:break;
  1050.                 }
  1051.         }
  1052. }

  1054. /******************************************************
  1055. *入口參數(shù)：無
  1056. *出口參數(shù)：無
  1057. *功能：定時器2中斷服務函數(shù)
  1058. ******************************************************/
  1059. void TIM2_IRQHandler(void)
  1060. {
  1061.         TIM_ClearITPendingBit(TIM2,TIM_IT_Update);  //清除中斷標志
  1062.         tim2_count++;
  1063. }

  1065. void YouZhuanChuLi(void)
  1066. {
  1067.         if(hecheng_juli>XiuZhengZhuanWanYiGeTime)
  1068.         {
  1069.                 JiSuanZuoBiao();
  1070.         }
  1071.         hecheng_juli=0;
  1072.         
  1073.         Off_Motor(3);
  1074.         Init_Motor(400,ZhengZhuan,400,FanZhuan);//右轉(zhuǎn)
  1075.         delay(6210000);                                
  1076.         Off_Motor(3);
  1077.         
  1078.         GenQiangFlag=NoWall;

  1080.         YiGeTime_PanDuan=YiGeTime1;//
  1081.         
  1082.         
  1083.         
  1084.         if(QianJinFangXiang==Y_ZhengFangXiang)
  1085.         {
  1086.                 QianJinFangXiang=X_ZhengFangXiang;
  1087.         }
  1088.         else if(QianJinFangXiang==Y_FuFangXiang)
  1089.         {
  1090.                 QianJinFangXiang=X_FuFangXiang;
  1091.         }
  1092.         else if(QianJinFangXiang==X_ZhengFangXiang)
  1093.         {
  1094.                 QianJinFangXiang=Y_FuFangXiang;
  1095.         }
  1096.         else if(QianJinFangXiang==X_FuFangXiang)
  1097.         {
  1098.                 QianJinFangXiang=Y_ZhengFangXiang;
  1099.         }
  1100.         
  1101.         YunXingFlag=ZhiXing;
  1102.         zuolunint=MAX_SuDu;
  1103.         youlunint=MAX_SuDu;
  1104.         Init_Motor(zuolunint,ZhengZhuan,youlunint,ZhengZhuan);//直行
  1105. }

  1107. void ZuoZhuanChuLi(void)
  1108. {
  1109.         if(hecheng_juli>XiuZhengZhuanWanYiGeTime)
  1110.         {
  1111.                 JiSuanZuoBiao();
  1112.         }
  1113.         hecheng_juli=0;
  1114.         
  1115.         Off_Motor(3);
  1116.         Init_Motor(400,FanZhuan,400,ZhengZhuan);//左轉(zhuǎn)
  1117.         delay(6210000);                                
  1118.         Off_Motor(3);
  1119.         
  1120.         GenQiangFlag=NoWall;

  1122.         YiGeTime_PanDuan=YiGeTime1;//
  1123.         
  1124.         
  1125.         
  1126.         if(QianJinFangXiang==Y_ZhengFangXiang)
  1127.         {
  1128.                 QianJinFangXiang=X_FuFangXiang;
  1129.         }
  1130.         else if(QianJinFangXiang==Y_FuFangXiang)
  1131.         {
  1132.                 QianJinFangXiang=X_ZhengFangXiang;
  1133.         }
  1134.         else if(QianJinFangXiang==X_ZhengFangXiang)
  1135.         {
  1136.                 QianJinFangXiang=Y_ZhengFangXiang;
  1137.         }
  1138.         else if(QianJinFangXiang==X_FuFangXiang)
  1139.         {
  1140.                 QianJinFangXiang=Y_FuFangXiang;
  1141.         }
  1142.         
  1143.         YunXingFlag=ZhiXing;
  1144.         zuolunint=MAX_SuDu;
  1145.         youlunint=MAX_SuDu;
  1146.         Init_Motor(zuolunint,ZhengZhuan,youlunint,ZhengZhuan);//直行
  1147. }

  1149. void HouZhuanChuLi(void)
  1150. {
  1151.         if(hecheng_juli>XiuZhengZhuanWanYiGeTime)
  1152.         {        
  1153.                 JiSuanZuoBiao();
  1154.         }        
  1155.         hecheng_juli=0;
  1156.         
  1157.         Off_Motor(3);
  1158.         Init_Motor(400,FanZhuan,400,ZhengZhuan);//后轉(zhuǎn)
  1159.         delay(12410000);
  1160.         Off_Motor(3);
  1161.         
  1162.         GenQiangFlag=NoWall;
  1163.         
  1164.         YiGeTime_PanDuan=YiGeTime1;//
  1165.         
  1166.         
  1167.         
  1168.         if(QianJinFangXiang==Y_ZhengFangXiang)
  1169.         {
  1170.                 QianJinFangXiang=Y_FuFangXiang;
  1171.         }
  1172.         else if(QianJinFangXiang==Y_FuFangXiang)
  1173.         {
  1174.                 QianJinFangXiang=Y_ZhengFangXiang;
  1175.         }
  1176.         else if(QianJinFangXiang==X_ZhengFangXiang)
  1177.         {
  1178.                 QianJinFangXiang=X_FuFangXiang;
  1179.         }
  1180.         else if(QianJinFangXiang==X_FuFangXiang)
  1181.         {
  1182.                 QianJinFangXiang=X_ZhengFangXiang;
  1183.         }
  1184.         
  1185.         YunXingFlag=ZhiXing;
  1186.         zuolunint=MAX_SuDu;
  1187.         youlunint=MAX_SuDu;
  1188.         Init_Motor(zuolunint,ZhengZhuan,youlunint,ZhengZhuan);//直行
  1189. }
  1190. void ZhuanWanQianDaoWei(void)
  1191. {
  1192.         u8 i;
  1193.         if(qian>0x0)
  1194.         {
  1195.                 i=0;
  1196.                 while(qian<0x500)
  1197.                 {
  1198.                         CaiYang();
  1199.                         i++;
  1200.                         if(i>100)
  1201.                         {
  1202.                                 i=0;
  1203.                                 GenQiangZiChengXu();
  1204.                         }
  1205.                         delay(1000);
  1206.                 }        
  1207.         }
  1208. }
  1209. /**********************顯示路徑***********************/
  1210. void XianLuJing(void)
  1211. {
  1212.         char msg[20];
  1213.         u8 i;
  1214.         
  1215.         for(i=0;i<7;i++)
  1216.         {
  1217.                 sprintf(msg,"%1d",LuJingZuoBiao[0+2*i][0]);//LuJingZuoBiao
  1218.                 LCD_ShowString(0,0+32*i,msg);
  1219.                 sprintf(msg,"%1d",LuJingZuoBiao[0+2*i][1]);
  1220.                 LCD_ShowString(32,0+32*i,msg);
  1221.                
  1222.                 sprintf(msg,"%1d",LuJingZuoBiao[1+2*i][0]);
  1223.                 LCD_ShowString(200,0+32*i,msg);
  1224.                 sprintf(msg,"%1d",LuJingZuoBiao[1+2*i][1]);
  1225.                 LCD_ShowString(232,0+32*i,msg);
  1226.         }
  1227. }
  1228. /**********************顯示***********************/
  1229. void XianShiZhi(void)
  1230. {
  1231.         char msg[20];
  1232.         
  1233.         sprintf(msg,"1=%3X",shuju1[0]);
  1234.         LCD_ShowString(100,0,msg);
  1235.         
  1236.         sprintf(msg,"2=%3X",shuju[0]);
  1237.         LCD_ShowString(0,32,msg);
  1238.         sprintf(msg,"3=%3X",shuju1[1]);
  1239.         LCD_ShowString(0,64,msg);
  1240.         sprintf(msg,"4=%3X",shuju[1]);
  1241.         LCD_ShowString(0,96,msg);
  1242.         
  1243.         sprintf(msg,"5=%3X",shuju1[2]);
  1244.         LCD_ShowString(100,128,msg);
  1245.         
  1246.         sprintf(msg,"6=%3X",shuju[2]);
  1247.         LCD_ShowString(200,96,msg);
  1248.         sprintf(msg,"7=%3X",shuju1[3]);
  1249.         LCD_ShowString(200,64,msg);
  1250.         sprintf(msg,"8=%3X",shuju[3]);
  1251.         LCD_ShowString(200,32,msg);
  1252. }
  1253. /**********************采樣***********************/
  1254. void CaiYang(void)
  1255. {
  1256.         u8 i,j;
  1257.         u16 shuju_zc[4];
  1258.         u16 shuju1_zc[4];
  1259.         
  1260.         GPIO_SetBits(GPIOA, GPIO_Pin_7);//打開AD_Ctrl1，開始轉(zhuǎn)換//2/4/6/8        
  1261.         delay(10);//延時               
  1262.         ADC_Configuration((u32)shuju,1);//轉(zhuǎn)換第一組
  1263.         GPIO_ResetBits(GPIOA, GPIO_Pin_7);//關閉AD_Ctrl1
  1264.         delay(10);//延時
  1265.         GPIO_SetBits(GPIOA, GPIO_Pin_6);//打開AD_Ctrl0，開始轉(zhuǎn)換//1/3/5/7
  1266.         delay(10);//延時                        
  1267.         ADC_Configuration((u32)shuju1,2);        //轉(zhuǎn)換第二組        
  1268.         GPIO_ResetBits(GPIOA, GPIO_Pin_6);//關閉AD_Ctrl0
  1269.         
  1270.         for(i=0;i<4;i++)
  1271.         {
  1272.                 shuju_zc[i] = 0;
  1273.                 shuju1_zc[i] = 0;
  1274.         }
  1275.         
  1276.         for(i=0;i<3;i++)
  1277.         {
  1278.                 GPIO_SetBits(GPIOA, GPIO_Pin_7);//打開AD_Ctrl1，開始轉(zhuǎn)換//2/4/6/8        
  1279.                 delay(10);//延時               
  1280.                 ADC_Configuration((u32)shuju,1);//轉(zhuǎn)換第一組
  1281.                 GPIO_ResetBits(GPIOA, GPIO_Pin_7);//關閉AD_Ctrl1
  1282.                 delay(10);//延時
  1283.                 GPIO_SetBits(GPIOA, GPIO_Pin_6);//打開AD_Ctrl0，開始轉(zhuǎn)換//1/3/5/7
  1284.                 delay(10);//延時                        
  1285.                 ADC_Configuration((u32)shuju1,2);        //轉(zhuǎn)換第二組        
  1286.                 GPIO_ResetBits(GPIOA, GPIO_Pin_6);//關閉AD_Ctrl0
  1287.                
  1288.                 for(j=0;j<4;j++)
  1289.                 {
  1290.                         shuju_zc[j] += shuju[j];
  1291.                         shuju1_zc[j] += shuju1[j];
  1292.                 }
  1293.         }
  1294.         for(i=0;i<4;i++)
  1295.         {
  1296.                 shuju[i] = shuju_zc[i]/3;
  1297.                 shuju1[i] = shuju1_zc[i]/3;
  1298.         }
  1299.         qian=shuju1[0];
  1300.         zuo=shuju1[1];
  1301.         hou=shuju1[2];
  1302.         you=shuju1[3];
  1303.         zuoshang=shuju[0];
  1304.         zuoxia=shuju[1];
  1305.         youxia=shuju[2];
  1306.         youshang=shuju[3];
  1307.         
  1308.         zuoshang2=shuju[0]&0xf00;
  1309.         zuoxia2=shuju[1]&0xf00;
  1310.         youxia2=shuju[2]&0xf00;
  1311.         youshang2=shuju[3]&0xf00;
  1312.         
  1313.         zuoshang3=shuju[0]&0xfc0;
  1314.         zuoxia3=shuju[1]&0xfc0;
  1315.         youxia3=shuju[2]&0xf80;
  1316.         youshang3=shuju[3]&0xf80;
  1317. }
  1318. /******************************************************
  1319. *入口參數(shù)：無
  1320. *出口參數(shù)：無
  1321. *功能：PA9外部中斷服務函數(shù)，實現(xiàn)LED燈循環(huán)點亮間隔的改變
  1322. ******************************************************/
  1323. void EXTI9_5_IRQHandler(void)
  1324. {        
  1325.         EXTI_ClearITPendingBit(EXTI_Line9);//清除PA9掛起位
  1326.         if((GPIO_ReadInputData(GPIOA)&0x0200) == 0)
  1327.         {
  1328.                 delay(500000);//延時去抖動
  1329.                 if((GPIO_ReadInputData(GPIOA)&0x0200) == 0)
  1330.                 {
  1331.                         QiBu_Init2();
  1332.                         CaiYang();
  1333.                         while(qian>0x200)
  1334.                         {
  1335.                                 CaiYang();
  1336.                                 delay(10000);
  1337.                         }
  1338.                         Init_Motor(400,ZhengZhuan,400,ZhengZhuan);//直行
  1339.                         delay(15000000);
  1340.                 }
  1341.         }
  1342. }
  1343. const u16 juli[201]={
  1344. 2500,2487,2475,2463,2450,2439,2427,2415,
  1345. 2403,2392,2380,2369,2358,2347,2336,2325,
  1346. 2314,2304,2293,2283,2272,2262,2252,2242,
  1347. 2232,2222,2212,2202,2192,2183,2173,2164,
  1348. 2155,2145,2136,2127,2118,2109,2100,2092,
  1349. 2083,2074,2066,2057,2049,2040,2032,2024,
  1350. 2016,2008,2000,1992,1984,1976,1968,1960,
  1351. 1953,1945,1937,1930,1923,1915,1908,1901,
  1352. 1893,1886,1879,1872,1865,1858,1851,1845,
  1353. 1838,1831,1824,1818,1811,1805,1798,1792,
  1354. 1785,1779,1773,1766,1760,1754,1748,1742,
  1355. 1736,1730,1724,1718,1712,1706,1700,1694,
  1356. 1689,1683,1677,1672,1666,1661,1655,1650,
  1357. 1644,1639,1633,1628,1623,1618,1612,1607,
  1358. 1602,1597,1592,1587,1582,1577,1572,1567,
  1359. 1562,1557,1552,1547,1543,1538,1533,1529,
  1360. 1524,1519,1515,1510,1506,1501,1497,1492,
  1361. 1488,1483,1479,1474,1470,1466,1461,1457,
  1362. 1453,1449,1445,1440,1436,1432,1428,1424,
  1363. 1420,1416,1412,1408,1404,1400,1396,1392,
  1364. 1388,1385,1381,1377,1373,1369,1366,1362,
  1365. 1358,1355,1351,1347,1344,1340,1336,1333,
  1366. 1329,1326,1322,1319,1315,1312,1308,1305,
  1367. 1302,1298,1295,1291,1288,1285,1282,1278,
  1368. 1275,1272,1269,1265,1262,1259,1256,1253,
  1369. 1250
  1370. };

復制代碼

bisai.7z

202.49 KB, 下載次數(shù): 22, 下載積分: 黑幣 -5

迷宮機器人
作者: admin    時間: 2018-12-18 02:02
補全原理圖或者詳細說明一下電路連接即可獲得100+黑幣
作者: qq879491397    時間: 2018-12-18 14:09
admin 發(fā)表于 2018-12-18 02:02
補全原理圖或者詳細說明一下電路連接即可獲得100+黑幣

可以。。aaaa
作者: qq879491397    時間: 2018-12-18 14:14
這個是原理圖，大家可以參考一下

STM32F103ZET6原理圖.zip

83.8 KB, 下載次數(shù): 16, 下載積分: 黑幣 -5

原理圖，大家可以看一下



歡迎光臨 (http://www.torrancerestoration.com/bbs/) Powered by Discuz! X3.1