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