📄 navigation.lst
字号:
170 3 minDis = dis[i];
171 3 }
172 2 }
173 1
174 1 /* 若没有可用的方向,则通知寻径函数折回 */
175 1 if(minId == -1)
176 1 return (path_t *)0;
177 1
178 1 /* 用去最近距离方向的信号量 */
C51 COMPILER V8.02 NAVIGATION 05/21/2008 10:52:26 PAGE 4
179 1 p->direct &= d_spend[minId];
180 1 /* 在移动到新位置之前,在旧位置处留下足迹 */
181 1 maze[pY][pX] = (byte_t)PATH_FOOTPRINT;
182 1
183 1 /* 构建新的路径节点 */
184 1 pnode = (path_t *)malloc(sizeof(path_t));
185 1 assert(pnode);
186 1
187 1 /* 计算下一个位置的坐标 */
188 1 pX += move[minId][0];
189 1 pY += move[minId][1];
190 1
191 1 pnode->pos = create_pos(pX, pY);
192 1 pnode->prev = p;
193 1 pnode->next = (path_t *)0;
194 1 pnode->direct = 0;
195 1
196 1 /* 在新位置处,计算下一个位置可用的移动方向 */
197 1 for(i = 0; i < 4; ++i) {
198 2 if(maze[pY + move[i][1]][pX + move[i][0]] != PATH_BLOCK && maze[pY + move[i][1]][pX + move[i][0]]
-!= PATH_FOOTPRINT) {
199 3 /* 若尝试的下一个位置不是障碍物或自己走过的足迹,则视为可用方向,获得该方向的信号量 */
200 3 pnode->direct |= d_signal[i];
201 3 }
202 2 }
203 1
204 1 return pnode;
205 1
206 1 }
207
208 /****************************************************************************/
209 /*== A*算法寻径函数
210 /*-eX
211 /*-eY :入口坐标
212 /*-qX
213 /*-qY :出口坐标
214 /*-maze :矩阵
215 /****************************************************************************/
216
217 path_t * AStar(uint_t eX, uint_t eY, uint_t qX, uint_t qY, byte_t maze[MAZE_HEIGHT][MAZE_WIDTH])
218 {
219 1 register int i;
220 1
221 1 /* 压缩坐标 */
222 1 uint_t quit_pos = create_pos(qX, qY);
223 1
224 1 /* 构建入口路径节点,视为路径链的头 */
225 1 path_t *head = (path_t *)malloc(sizeof(path_t));
226 1 path_t *p = (path_t *)0;
227 1 path_t *back = (path_t *)0;
228 1 assert(head);
229 1
230 1 p = head;
231 1 p->direct = 0;
232 1 p->pos = create_pos(eX,eY);
233 1 p->next = (path_t *)0;
234 1 p->prev = (path_t *)0;
235 1
236 1 /* 创建入口处的可用方向 */
237 1 for(i = 0; i < 4; ++i) {
238 2 if(maze[eY + move[i][1]][eX + move[i][0]] != PATH_BLOCK)
239 2 /* 若无障碍物则获得该方向的信号量 */
C51 COMPILER V8.02 NAVIGATION 05/21/2008 10:52:26 PAGE 5
240 2 p->direct |= d_signal[i];
241 2 }
242 1
243 1 do {
244 2
245 2 /* 获得下个路径的节点指针 */
246 2 back = evaluate(p, qX, qY, maze);
247 2 if(back) {
248 3 p->next = back;
249 3 p = p->next;
250 3 }
251 2
252 2 /* 无路可走则折回 */
253 2 if(p->direct == 0 && p != head && p->pos != quit_pos) {
254 3 back = p->prev;
255 3 back->next = (path_t *)0;
256 3
257 3 /* 清楚脚印 */
258 3 maze[p->pos & 0x00FF][p->pos >> 8] = (byte_t)PATH_WALKON;
259 3
260 3 free(p);
261 3 p = back;
262 3 }
263 2
264 2 /* 若走不出迷宫,即折回到入口,且入口处的可用方向全部尝试过 */
265 2 if(p == head && p->direct == 0) {
266 3 free(head);
267 3 return (path_t *)0;
268 3 }
269 2
270 2 } while( p->pos != quit_pos );
271 1
272 1 /* 在出口处留下脚印,便于打印 */
273 1 maze[p->pos & 0x00FF][p->pos >> 8] = (byte_t)PATH_FOOTPRINT;
274 1
275 1 return head;
276 1
277 1 }
278
279 /*****************************************************************/
280 /*函数名称： show_menu_DaoHang.c */
281 /*函数功能： GPS导航 */
282 /*有无返回： 无 */
283 /*修改记录： 无修改记录 */
284 /*入口参数：
285 start_x------------------------------------起点x坐标
286 start_y------------------------------------起点y坐标
287 end_x ------------------------------------终点x坐标
288 end_y ------------------------------------终点y坐标
289 */
290 /*编写日期： 2008-5-01 */
291 /*****************************************************************/
292
293 //void show_menu_DaoHang(unsigned char start_x,unsigned char start_y,unsigned char end_x,unsigned char end
-_y)
294 void show_menu_DaoHang()
295 {
296 1 unsigned char i=0,j=0;
297 1
298 1
299 1 path_t *pHead = AStar((uint_t)(Position/1000),(uint_t)(Position%1000/100),(uint_t)(Position%100/10)
-,(uint_t)(Position%10),maze0);
C51 COMPILER V8.02 NAVIGATION 05/21/2008 10:52:26 PAGE 6
300 1
301 1 for(i = 0; i < MAZE_HEIGHT; ++i)
302 1 {
303 2 for(j = 0; j < MAZE_WIDTH; ++j)
304 2 Display_Point(j,i,symbol[maze0[i][j]]);
305 2 }
306 1 }
307
308 void Show_Map()
309 {
310 1
311 1 unsigned char point_x=0,point_y=0;
312 1 for(point_y = 0; point_y < MAZE_HEIGHT; ++point_y)
313 1 {
314 2 for(point_x = 0; point_x < MAZE_WIDTH; ++point_x)
315 2
316 2 Display_Point(point_x,point_y,symbol[maze1[point_y][point_x]]);
317 2
318 2 /*
319 2 j-------代表x方向
320 2 i-------代表y方向
321 2 symbolic-路径显示
322 2 */
323 2
324 2 }
325 1
326 1 }
327
328
MODULE INFORMATION: STATIC OVERLAYABLE
CODE SIZE = 2621 ----
CONSTANT SIZE = 92 ----
XDATA SIZE = 556 70
PDATA SIZE = ---- ----
DATA SIZE = ---- ----
IDATA SIZE = ---- ----
BIT SIZE = ---- ----
END OF MODULE INFORMATION.
C51 COMPILATION COMPLETE. 0 WARNING(S), 0 ERROR(S)
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -