标题: 迷宫机器人的程序，基于stm32的开发板写的，用了pid算法 [打印本页]

---

作者: qq879491397    时间: 2018-12-17 23:40
标题: 迷宫机器人的程序，基于stm32的开发板写的，用了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"
   
10. 
    
11. #define pi 3.1415926535898
    
12. #define TwoPi 6.2831853071796
    
13. 
    
14. #define ZhiXing 1
    
15. #define HouZhuan 2
    
16. #define ZuoZhuan 3
    
17. #define YouZhuan 4
    
18. 
    
19. #define ZhengZhuan 0
    
20. #define FanZhuan 1
    
21. 
    
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
    
29. 
    
30. #define X_ZhengFangXiang 1
    
31. #define Y_ZhengFangXiang 2
    
32. #define X_FuFangXiang 3
    
33. #define Y_FuFangXiang 4
    
34. 
    
35. #define YiGeTime1  96000        //95000
    
36. //#define YiGeTime2 94000
    
37. #define YiGeTime3 99000
    
38. 
    
39. #define BanGeTime1 47500
    
40. 
    
41. #define ZhongDianZuoBiao 7
    
42. 
    
43. #define Active 1
    
44. #define Dead 2
    
45. 
    
46. #define FenZhiID_MAX_3FangXiang 2
    
47. #define FenZhiID_MAX_2FangXiang 1
    
48. 
    
49. #define XunLu 0
    
50. #define FanHui 1
    
51. #define ChongCi 2
    
52. 
    
53. #define YES 1
    
54. #define NO 0
    
55. 
    
56. #define XiuZhengZhuanWanYiGeTime 85000  //90000
    
57. 
    
58. #define GenQiangYuZhi1 0x80
    
59. #define GenQiangYuZhi2 0xff
    
60. 
    
61. #define MAX_SuDu 400
    
62. #define MIN_SuDu 500
    
63. 
    
64. u16 tim2_count=0;//程序循环周期
    
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;//
    
70. 
    
71. u8 GenQiangFlag=0;//跟墙标志，做跟、右跟、双跟
    
72. u8 YunXingFlag=0;//运行标志，直行、左转、右转、后转
    
73. u8 QianJinFangXiang=0;//前进方向，Y正、Y负、X正、X负
    
74. u8 XunLuFanHuiChongCiFlag=XunLu;//寻路返回冲刺标志
    
75. 
    
76. u8 YiZouGuoGeShu=0;//已经走过的格数
    
77. u8 FenZhiDuiZhan=0;//分支堆栈
    
78. //u8 FenZhiID=0;//分支ID
    
79. u8 FenZhiID_MAX=0;//分支ID的最大值
    
80. u8 CunChuLuJingShuZuXiaBiao=0;//存储路径数组下标
    
81. u8 ChongCiLuJingXiaBiao=0;//冲刺路径下标
    
82. u32 YiGeTime_PanDuan=0;//判断一格的时间，默认为时间1，转弯后为时间2，下次满格变成时间3
    
83. u8 LianXuZhiXing_Count=0;//判断是否连续直行，连续直行需要改变一格时间
    
84. 
    
85. u8 YouShangGenQiang_YuZhi=0;//右上跟墙阈值
    
86. u8 YouXiaGenQiang_YuZhi=0;//右下跟墙阈值
    
87. u8 ZuoShangGenQiang_YuZhi=0;//左上跟墙阈值
    
88. u8 ZuoXiaGenQiang_YuZhi=0;//左下跟墙阈值
    
89. 
    
90. u8 YouShangGenQiang_Count=0;//右上跟墙计数
    
91. u8 YouXiaGenQiang_Count=0;//右下跟墙计数
    
92. u8 ZuoShangGenQiang_Count=0;//左上跟墙计数
    
93. u8 ZuoXiaGenQiang_Count=0;//左下跟墙计数
    
94. 
    
95. const u16 juli[201];//查表，速度与距离的关系
    
96. const int sin_figure[451];
    
97. 
    
98. u8 LuJingZuoBiao[128][2];//路径坐标
    
99. u8 FenZhiDianZuoBiao[64][2];//分支点坐标
    
100. u8 FenZhiID[64];//分支ID
     
101. u8 FenZhiQiWangFangXiang[64][4]; //分支期望方向
     
102. u8 LuJingQiWangFangXiang[1];//路径期望方向
     
103. 
     
104. float zuolun=0;
     
105. float youlun=0;
     
106. u16 zuolunint=0;
     
107. u16 youlunint=0;
     
108. 
     
109. int x_point=0;
     
110. int y_point=0;
     
111. u32 zuolun_juli=0;
     
112. u32 youlun_juli=0;
     
113. 
     
114. u32 hecheng_juli=0;//合成距离
     
115. 
     
116. u8 FenZhi_ZhiZhen=0;
     
117. 
     
118. 
     
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);//根据期望方向计算前进方向
     
130. void XianLuJing(void);//显示路径坐标
     
131. void GenJuLuJingQiWangFangXiangJiSuanQianJinFangXiang(void);//根据路径期望方向计算前进方向
     
132. void AnLuJingFanHui(void);//按路径返回
     
133. void GenQiangZiChengXu(void);//跟墙子程序
     
134. void AnLuJingChongCi(void);//按路径冲刺
     
135. void ZhuanWanQianDaoWei(void);//转弯前到位
     
136. void ChongCiQianXiangPanDuan(void);
     
137. void QiBu_Init2(void);
     
138. 
     
139. /******************************************************
     
140. *入口参数：无
     
141. *出口参数：无
     
142. *功能：主函数
     
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;        
     
174. 
     
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];
     
518. 
     
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;
     
589. 
     
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;
     
642. 
     
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. }
     
798. 
     
799. /**********************根据期望方向计算前进方向***********************/
     
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;//程序循环周期
     
852. 
     
853.         GenQiangFlag=0;//跟墙标志，做跟、右跟、双跟
     
854.         YunXingFlag=0;//运行标志，直行、左转、右转、后转
     
855.         XunLuFanHuiChongCiFlag=ChongCi;//寻路返回冲刺标志
     
856. 
     
857.         FenZhiDuiZhan=0;//分支堆栈
     
858. //        FenZhiID=0;//分支ID
     
859.         FenZhiID_MAX=0;//分支ID的最大值
     
860.         CunChuLuJingShuZuXiaBiao=0;//存储路径数组下标
     
861.         ChongCiLuJingXiaBiao=0;//冲刺路径下标
     
862.         LianXuZhiXing_Count=0;//判断是否连续直行，连续直行需要改变一格时间
     
863. 
     
864.         zuolun=0;
     
865.         youlun=0;
     
866.         zuolunint=0;
     
867.         youlunint=0;
     
868. 
     
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;//程序循环周期
     
904. 
     
905.         GenQiangFlag=0;//跟墙标志，做跟、右跟、双跟
     
906.         YunXingFlag=0;//运行标志，直行、左转、右转、后转
     
907.         XunLuFanHuiChongCiFlag=XunLu;//寻路返回冲刺标志
     
908. 
     
909.         FenZhiDuiZhan=0;//分支堆栈
     
910. //        FenZhiID=0;//分支ID
     
911.         FenZhiID_MAX=0;//分支ID的最大值
     
912.         CunChuLuJingShuZuXiaBiao=0;//存储路径数组下标
     
913.         ChongCiLuJingXiaBiao=0;//冲刺路径下标
     
914.         LianXuZhiXing_Count=0;//判断是否连续直行，连续直行需要改变一格时间
     
915. 
     
916.         zuolun=0;
     
917.         youlun=0;
     
918.         zuolunint=0;
     
919.         youlunint=0;
     
920. 
     
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. }
     
988. 
     
989. /**********************按路径冲刺***********************/
     
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. }
      
1018. 
      
1019. /**********************根据路径期望方向计算前进方向***********************/
      
1020. /**********************根据路径期望方向计算前进方向***********************/
      
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. }
      
1053. 
      
1054. /******************************************************
      
1055. *入口参数：无
      
1056. *出口参数：无
      
1057. *功能：定时器2中断服务函数
      
1058. ******************************************************/
      
1059. void TIM2_IRQHandler(void)
      
1060. {
      
1061.         TIM_ClearITPendingBit(TIM2,TIM_IT_Update);  //清除中断标志
      
1062.         tim2_count++;
      
1063. }
      
1064. 
      
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);//右转
      
1075.         delay(6210000);                                
      
1076.         Off_Motor(3);
      
1077.         
      
1078.         GenQiangFlag=NoWall;
      
1079. 
      
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. }
      
1106. 
      
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);//左转
      
1117.         delay(6210000);                                
      
1118.         Off_Motor(3);
      
1119.         
      
1120.         GenQiangFlag=NoWall;
      
1121. 
      
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. }
      
1148. 
      
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);//后转
      
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，开始转换//2/4/6/8        
      
1261.         delay(10);//延时               
      
1262.         ADC_Configuration((u32)shuju,1);//转换第一组
      
1263.         GPIO_ResetBits(GPIOA, GPIO_Pin_7);//关闭AD_Ctrl1
      
1264.         delay(10);//延时
      
1265.         GPIO_SetBits(GPIOA, GPIO_Pin_6);//打开AD_Ctrl0，开始转换//1/3/5/7
      
1266.         delay(10);//延时                        
      
1267.         ADC_Configuration((u32)shuju1,2);        //转换第二组        
      
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，开始转换//2/4/6/8        
      
1279.                 delay(10);//延时               
      
1280.                 ADC_Configuration((u32)shuju,1);//转换第一组
      
1281.                 GPIO_ResetBits(GPIOA, GPIO_Pin_7);//关闭AD_Ctrl1
      
1282.                 delay(10);//延时
      
1283.                 GPIO_SetBits(GPIOA, GPIO_Pin_6);//打开AD_Ctrl0，开始转换//1/3/5/7
      
1284.                 delay(10);//延时                        
      
1285.                 ADC_Configuration((u32)shuju1,2);        //转换第二组        
      
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. *入口参数：无
      
1320. *出口参数：无
      
1321. *功能：PA9外部中断服务函数，实现LED灯循环点亮间隔的改变
      
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. };
      
1371. 
      

复制代码

bisai.7z

2018-12-18 02:02 上传

点击文件名下载附件

下载积分: 黑币 -5

202.49 KB, 下载次数: 22, 下载积分: 黑币 -5

迷宫机器人

---

作者: admin    时间: 2018-12-18 02:02
补全原理图或者详细说明一下电路连接即可获得100+黑币

---

作者: qq879491397    时间: 2018-12-18 14:09

admin 发表于 2018-12-18 02:02
补全原理图或者详细说明一下电路连接即可获得100+黑币

可以。。aaaa

---

作者: qq879491397    时间: 2018-12-18 14:14
这个是原理图，大家可以参考一下

STM32F103ZET6原理图.zip

2018-12-18 14:13 上传

点击文件名下载附件

下载积分: 黑币 -5

83.8 KB, 下载次数: 16, 下载积分: 黑币 -5

原理图，大家可以看一下

---

欢迎光临 (http://www.51hei.com/bbs/)

Powered by Discuz! X3.1