📄 navigation.lst
字号:
C51 COMPILER V8.02 NAVIGATION 05/06/2008 15:45:50 PAGE 1
C51 COMPILER V8.02, COMPILATION OF MODULE NAVIGATION
OBJECT MODULE PLACED IN Navigation.OBJ
COMPILER INVOKED BY: C:\Keil\C51\BIN\C51.EXE Navigation.c LARGE BROWSE DEBUG OBJECTEXTEND
line level source
1 /*****************************************************************/
2 /*函数名称： GPS_Navigation.c */
3 /*函数功能： 小车导航，实现最短路径 */
4 /*有无返回： 无 */
5 /*修改记录： 无修改记录 */
6 /*编写作者： t483-4-19chenyong */
7 /*编写日期： 2008-3-12 */
8 /*****************************************************************/
9
10
11 #include "common.h"
12 #include "TS12864A.h"
13 #include "navigation.h "
14 #include "delay.h"
15 #include "map.h"
16 #include "key.h"
17 #include "astar.h"
18 #include "window.h"
19
20 /*****************************************************************/
21 /*
22 /----------GPS导航电子地图(测试版本)
23 /
24 /-------------------------------------------
25 | S,*,*,*,*,*,*,*,*,*,*,*,*,*,*,*, |
26 | *,*,*,*,*,B,*,*,*,*,*,*,*,*,*,*, |
27 | *,*,*,*,*,*,*,*,*,*,*,*,*,*,E,*, |
28 | *,*,*,*,*,*,*,*,*,*,*,*,*,*,*,*, |
29 | *,*,*,A,*,*,*,*,*,*,*,*,*,*,*,*, |
30 | *,*,*,*,*,*,*,*,*,*,C,*,*,*,*,*, |
31 | *,*,*,*,*,*,F,*,*,*,*,*,*,*,*,*, |
32 | *,*,*,*,*,*,*,*,*,*,*,*,*,*,D,*, |
33 -------------------------------------------
34 说明： A，B，C，D，E，F为用户要到达的终点，S为起点。通过按键盘上的按键
35 实现小车的最短路径搜索，并通过液晶显示行驶的步数。
36
37 */
38
39
40 struct MapData
41 {
42 unsigned char Gps_Map_Length[Map_Length];
43 unsigned char Gps_Map_Width[Map_Width];
44 };
45
46 xdata struct MapData;
47 extern unsigned char key;
48 unsigned int Position=1165;
49
50
51 static byte_t maze[MAZE_HEIGHT][MAZE_WIDTH] = {
52
53 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
54 1,0,1,0,1,1,1,0,1,1,1,0,0,1,0,1,
55 0,0,1,0,1,0,0,1,1,0,1,1,0,1,1,1,
C51 COMPILER V8.02 NAVIGATION 05/06/2008 15:45:50 PAGE 2
56 1,0,1,1,1,1,1,1,1,1,1,1,1,0,1,1,
57 1,1,1,1,0,1,1,0,1,0,1,0,1,1,1,1,
58 1,1,0,1,1,1,1,1,1,0,1,0,1,1,0,0,
59 0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,
60 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
61 };
62
63 /*
64 方向信号量查询表
65 0x01(0000 0001) : 上
66 0x02(0000 0010) : 下
67 0x04(0000 0100) : 左
68 0x08(0000 1000) : 右
69 */
70
71 static byte_t d_signal[4] = {0x01, 0x02, 0x04, 0x08};
72
73 /*
74 方向信号量使用表
75 如果指定方向已经走过,那么使用“与”运算去处该方向
76 0x0E(0000 1110) : 上
77 0x0D(0000 1101) : 下
78 0x0B(0000 1011) : 左
79 0x07(0000 0111) : 右
80 */
81
82 static byte_t d_spend[4] = {0x0E, 0x0D, 0x0B, 0x07};
83
84 /* 指定方向移动偏量 */
85 static int move[4][2] = { {0, -1}, {0, 1}, {-1, 0}, {1, 0} };
86
87 /* 打印迷宫用的符号 */
88 static byte_t symbolic[3] = {66,64,69};//{66,64,69};
89
90 /* 打印地图*/
91 static byte_t symbol[3] = {0,1,2};
92
93 /* 求两点间的距离 */
94 uint_t distance( uint_t pos1X, uint_t pos1Y, uint_t pos2X, uint_t pos2Y )
95 {
96 1
97 1 uint_t ret = 0;
98 1
99 1 /* 距离公式 */
100 1 ret = (uint_t)sqrt((pow((double)((int)pos1X - (int)pos2X),2) + pow((double)((int)pos1Y - (int)pos2Y),2
-)));
101 1
102 1 return ret;
103 1
104 1 }
105
106
107
108 /* 压缩坐标 */
109 uint_t create_pos( uint_t pX, uint_t pY )
110 {
111 1 uint_t ret = 0;
112 1 /* 将pX赋给ret,这样pX坐标在ret的低八位 */
113 1 ret = pX;
114 1
115 1 /* 将pX坐标移到高八位去,这样低位就能存放pY */
116 1 ret <<= 8;
C51 COMPILER V8.02 NAVIGATION 05/06/2008 15:45:50 PAGE 3
117 1 /* 将pY存放放到ret的低八位,并保持高八位的数据不变 */
118 1 ret |= pY;
119 1
120 1 return ret;
121 1 }
122
123 /****************************************************************************/
124 /*估计函数
125 /*-p : 当前移动到的节点指针
126 /*-quit_x
127 /*-quit_y : quit_x 和 quit_y表示迷宫出口坐标
128 /*-maze : 地图矩阵
129 /****************************************************************************/
130
131 path_t * evaluate( path_t *p, uint_t quit_x, uint_t quit_y, byte_t maze[MAZE_HEIGHT][MAZE_WIDTH] )
132 {
133 1 uint_t pX, pY;
134 1
135 1 /* 用于接收四个方向离开出口的距离,以便选择最近的方向移动 */
136 1 int dis[4];
137 1 int minDis = 32767;
138 1 int minId = -1;
139 1
140 1 path_t *pnode = (path_t *)0;
141 1
142 1 register int i;
143 1
144 1 /* 计算当前节点的坐标 */
145 1 pX = p->pos >> 8;
146 1 pY = p->pos & 0x00FF;
147 1
148 1 memset(dis, (int)-1, sizeof(int)*4);
149 1
150 1 /* 计算每个方向离开出口的距离,一次存放到dis数组,若没有i方向,则dis[i]任保留-1 */
151 1 for( i = 0; i < 4; ++i ) {
152 2 if( (p->direct & d_signal[i]) >> i == 0x01 )
153 2 dis[i] =(int)distance(pX + move[i][0], pY + move[i][1], quit_x, quit_y);
154 2 }
155 1
156 1 /* 获得最短距离的方向 */
157 1 for(i = 0; i < 4; ++i) {
158 2 if(dis[i] != -1 && dis[i] < minDis) {
159 3 minId = i;
160 3 minDis = dis[i];
161 3 }
162 2 }
163 1
164 1 /* 若没有可用的方向,则通知寻径函数折回 */
165 1 if(minId == -1)
166 1 return (path_t *)0;
167 1
168 1 /* 用去最近距离方向的信号量 */
169 1 p->direct &= d_spend[minId];
170 1 /* 在移动到新位置之前,在旧位置处留下足迹 */
171 1 maze[pY][pX] = (byte_t)PATH_FOOTPRINT;
172 1
173 1 /* 构建新的路径节点 */
174 1 pnode = (path_t *)malloc(sizeof(path_t));
175 1 assert(pnode);
176 1
177 1 /* 计算下一个位置的坐标 */
178 1 pX += move[minId][0];
C51 COMPILER V8.02 NAVIGATION 05/06/2008 15:45:50 PAGE 4
179 1 pY += move[minId][1];
180 1
181 1 pnode->pos = create_pos(pX, pY);
182 1 pnode->prev = p;
183 1 pnode->next = (path_t *)0;
184 1 pnode->direct = 0;
185 1
186 1 /* 在新位置处,计算下一个位置可用的移动方向 */
187 1 for(i = 0; i < 4; ++i) {
188 2 if(maze[pY + move[i][1]][pX + move[i][0]] != PATH_BLOCK && maze[pY + move[i][1]][pX + move[i][0]]
-!= PATH_FOOTPRINT) {
189 3 /* 若尝试的下一个位置不是障碍物或自己走过的足迹,则视为可用方向,获得该方向的信号量 */
190 3 pnode->direct |= d_signal[i];
191 3 }
192 2 }
193 1
194 1 return pnode;
195 1
196 1 }
197
198 /****************************************************************************/
199 /*== A*算法寻径函数
200 /*-eX
201 /*-eY :入口坐标
202 /*-qX
203 /*-qY :出口坐标
204 /*-maze :矩阵
205 /****************************************************************************/
206
207 path_t * AStar(uint_t eX, uint_t eY, uint_t qX, uint_t qY, byte_t maze[MAZE_HEIGHT][MAZE_WIDTH])
208 {
209 1 register int i;
210 1
211 1 /* 压缩坐标 */
212 1 uint_t quit_pos = create_pos(qX, qY);
213 1
214 1 /* 构建入口路径节点,视为路径链的头 */
215 1 path_t *head = (path_t *)malloc(sizeof(path_t));
216 1 path_t *p = (path_t *)0;
217 1 path_t *back = (path_t *)0;
218 1 assert(head);
219 1
220 1 p = head;
221 1 p->direct = 0;
222 1 p->pos = create_pos(eX,eY);
223 1 p->next = (path_t *)0;
224 1 p->prev = (path_t *)0;
225 1
226 1 /* 创建入口处的可用方向 */
227 1 for(i = 0; i < 4; ++i) {
228 2 if(maze[eY + move[i][1]][eX + move[i][0]] != PATH_BLOCK)
229 2 /* 若无障碍物则获得该方向的信号量 */
230 2 p->direct |= d_signal[i];
231 2 }
232 1
233 1 do {
234 2
235 2 /* 获得下个路径的节点指针 */
236 2 back = evaluate(p, qX, qY, maze);
237 2 if(back) {
238 3 p->next = back;
239 3 p = p->next;
C51 COMPILER V8.02 NAVIGATION 05/06/2008 15:45:50 PAGE 5
240 3 }
241 2
242 2 /* 无路可走则折回 */
243 2 if(p->direct == 0 && p != head && p->pos != quit_pos) {
244 3 back = p->prev;
245 3 back->next = (path_t *)0;
246 3
247 3 /* 清楚脚印 */
248 3 maze[p->pos & 0x00FF][p->pos >> 8] = (byte_t)PATH_WALKON;
249 3
250 3 free(p);
251 3 p = back;
252 3 }
253 2
254 2 /* 若走不出迷宫,即折回到入口,且入口处的可用方向全部尝试过 */
255 2 if(p == head && p->direct == 0) {
256 3 free(head);
257 3 return (path_t *)0;
258 3 }
259 2
260 2 } while( p->pos != quit_pos );
261 1
262 1 /* 在出口处留下脚印,便于打印 */
263 1 maze[p->pos & 0x00FF][p->pos >> 8] = (byte_t)PATH_FOOTPRINT;
264 1
265 1 return head;
266 1
267 1 }
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -