📄 navigation.lst
字号:
C51 COMPILER V8.02 NAVIGATION 05/25/2008 15:06:03 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"
*** WARNING C318 IN LINE 15 OF common.h: can't open file 'conio.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 extern unsigned int Position;
49
50 byte_t maze[MAZE_HEIGHT][MAZE_WIDTH] = {
51 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
52 1,0,1,0,1,1,1,0,1,1,1,0,0,1,0,1,
53 0,0,1,0,1,0,0,1,1,0,1,1,0,1,1,1,
54 1,0,1,1,1,1,1,1,1,1,1,1,1,0,1,1,
C51 COMPILER V8.02 NAVIGATION 05/25/2008 15:06:03 PAGE 2
55 1,1,1,1,0,1,1,0,1,0,1,0,1,1,1,1,
56 1,1,0,1,1,1,1,1,1,0,1,0,1,1,0,0,
57 0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,
58 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
59 };
60 byte_t maze0[MAZE_HEIGHT][MAZE_WIDTH] = {
61 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
62 1,0,1,0,1,1,1,0,1,1,1,0,0,1,0,1,
63 0,0,1,0,1,0,0,1,1,0,1,1,0,1,1,1,
64 1,0,1,1,1,1,1,1,1,1,1,1,1,0,1,1,
65 1,1,1,1,0,1,1,0,1,0,1,0,1,1,1,1,
66 1,1,0,1,1,1,1,1,1,0,1,0,1,1,0,0,
67 0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,
68 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
69 };
70
71 /*
72 方向信号量查询表
73 0x01(0000 0001) : 上
74 0x02(0000 0010) : 下
75 0x04(0000 0100) : 左
76 0x08(0000 1000) : 右
77 */
78
79 byte_t d_signal[4] = {0x01, 0x02, 0x04, 0x08};
80
81 /*
82 方向信号量使用表
83 如果指定方向已经走过,那么使用“与”运算去处该方向
84 0x0E(0000 1110) : 上
85 0x0D(0000 1101) : 下
86 0x0B(0000 1011) : 左
87 0x07(0000 0111) : 右
88 */
89
90 byte_t d_spend[4] = {0x0E, 0x0D, 0x0B, 0x07};
91
92 /* 指定方向移动偏量 */
93 int move[4][2] = { {0, -1}, {0, 1}, {-1, 0}, {1, 0} };
94
95 /* 打印迷宫用的符号 */
96 byte_t symbol[3] = {0,1,2};
97
98 /* 求两点间的距离 */
99
100 uint_t distance( uint_t pos1X, uint_t pos1Y, uint_t pos2X, uint_t pos2Y ) {
101 1
102 1 uint_t ret = 0;
103 1
104 1 /* 距离公式 */
105 1 ret = (uint_t)sqrt((pow((double)((int)pos1X - (int)pos2X),2) + pow((double)((int)pos1Y - (int)pos2Y),2
-)));
106 1
107 1 return ret;
108 1
109 1 }
110
111 /* 压缩坐标 */
112 uint_t create_pos( uint_t pX, uint_t pY ) {
113 1 uint_t ret = 0;
114 1 /* 将pX赋给ret,这样pX坐标在ret的低八位 */
115 1 ret = pX;
C51 COMPILER V8.02 NAVIGATION 05/25/2008 15:06:03 PAGE 3
116 1
117 1 /* 将pX坐标移到高八位去,这样低位就能存放pY */
118 1 ret <<= 8;
119 1 /* 将pY存放放到ret的低八位,并保持高八位的数据不变 */
120 1 ret |= pY;
121 1
122 1 return ret;
123 1 }
124
125 /*
126 == 估计函数 ===========================================
127 -p : 当前移动到的节点指针
128 -quit_x
129 -quit_y : quit_x 和 quit_y表示迷宫出口坐标
130 -maze : 迷宫矩阵
131 =======================================================
132 */
133 path_t * evaluate( path_t *p, uint_t quit_x, uint_t quit_y, byte_t maze[MAZE_HEIGHT][MAZE_WIDTH] ) {
134 1 uint_t pX, pY;
135 1
136 1 /* 用于接收四个方向离开出口的距离,以便选择最近的方向移动 */
137 1 int dis[4];
138 1 int minDis = 32767;
139 1 int minId = -1;
140 1
141 1 path_t *pnode = (path_t *)0;
142 1
143 1 register int i;
144 1
145 1 /* 计算当前节点的坐标 */
146 1 pX = p->pos >> 8;
147 1 pY = p->pos & 0x00FF;
148 1
149 1 memset(dis, (int)-1, sizeof(int)*4);
150 1
151 1 /* 计算每个方向离开出口的距离,一次存放到dis数组,若没有i方向,则dis[i]任保留-1 */
152 1 for( i = 0; i < 4; ++i ) {
153 2 if( (p->direct & d_signal[i]) >> i == 0x01 )
154 2 dis[i] =(int)distance(pX + move[i][0], pY + move[i][1], quit_x, quit_y);
155 2 }
156 1
157 1 /* 获得最短距离的方向 */
158 1 for(i = 0; i < 4; ++i) {
159 2 if(dis[i] != -1 && dis[i] < minDis) {
160 3 minId = i;
161 3 minDis = dis[i];
162 3 }
163 2 }
164 1
165 1 /* 若没有可用的方向,则通知寻径函数折回 */
166 1 if(minId == -1)
167 1 return (path_t *)0;
168 1
169 1 /* 用去最近距离方向的信号量 */
170 1 p->direct &= d_spend[minId];
171 1 /* 在移动到新位置之前,在旧位置处留下足迹 */
172 1 maze[pY][pX] = (byte_t)PATH_FOOTPRINT;
173 1
174 1 /* 构建新的路径节点 */
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -