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