?? navigation.c
字號:
/*****************************************************************/
/*函數名稱： GPS_Navigation.c */
/*函數功能： 小車導航，實現最短路徑 */
/*有無返回： 無 */
/*修改記錄： 無修改記錄 */
/*編寫作者： t483-4-19chenyong */
/*編寫日期： 2008-3-12 */
/*****************************************************************/
#include "common.h"
#include "TS12864A.h"
#include "navigation.h "
#include "delay.h"
#include "map.h"
#include "key.h"
#include "astar.h"
#include "window.h"
/*****************************************************************/
/*
/----------GPS導航電子地圖(測試版本)
/
/-------------------------------------------
| S,*,*,*,*,*,*,*,*,*,*,*,*,*,*,*, |
| *,*,*,*,*,B,*,*,*,*,*,*,*,*,*,*, |
| *,*,*,*,*,*,*,*,*,*,*,*,*,*,E,*, |
| *,*,*,*,*,*,*,*,*,*,*,*,*,*,*,*, |
| *,*,*,A,*,*,*,*,*,*,*,*,*,*,*,*, |
| *,*,*,*,*,*,*,*,*,*,C,*,*,*,*,*, |
| *,*,*,*,*,*,F,*,*,*,*,*,*,*,*,*, |
| *,*,*,*,*,*,*,*,*,*,*,*,*,*,D,*, |
-------------------------------------------
說明： A，B，C，D，E，F為用戶要到達的終點，S為起點。通過按鍵盤上的按鍵
實現小車的最短路徑搜索，并通過液晶顯示行駛的步數。
*/
struct MapData
{
unsigned char Gps_Map_Length[Map_Length];
unsigned char Gps_Map_Width[Map_Width];
};
xdata struct MapData;
//unsigned int Position=1186;
extern unsigned int Position;
byte_t maze0[MAZE_HEIGHT][MAZE_WIDTH] = {
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
1,0,1,0,1,1,1,0,1,1,1,0,0,1,0,1,
0,0,1,0,1,0,0,1,1,0,1,1,0,1,1,1,
1,0,1,1,1,1,1,1,1,1,1,1,1,0,1,1,
1,1,1,1,0,1,1,0,1,0,1,0,1,1,1,1,
1,1,0,1,1,1,1,1,1,0,1,0,1,1,0,0,
0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
};
byte_t maze1[MAZE_HEIGHT][MAZE_WIDTH] = {
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
1,0,1,0,1,1,1,0,1,1,1,0,0,1,0,1,
0,0,1,0,1,0,0,1,1,0,1,1,0,1,1,1,
1,0,1,1,1,1,1,1,1,1,1,1,1,0,1,1,
1,1,1,1,0,1,1,0,1,0,1,0,1,1,1,1,
1,1,0,1,1,1,1,1,1,0,1,0,1,1,0,0,
0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
};
/*
方向信號量查詢表
0x01(0000 0001) : 上
0x02(0000 0010) : 下
0x04(0000 0100) : 左
0x08(0000 1000) : 右
*/
byte_t d_signal[4] = {0x01, 0x02, 0x04, 0x08};
/*
方向信號量使用表
如果指定方向已經走過,那么使用“與”運算去處該方向
0x0E(0000 1110) : 上
0x0D(0000 1101) : 下
0x0B(0000 1011) : 左
0x07(0000 0111) : 右
*/
byte_t d_spend[4] = {0x0E, 0x0D, 0x0B, 0x07};
/* 指定方向移動偏量 */
int move[4][2] = { {0, -1}, {0, 1}, {-1, 0}, {1, 0} };
/* 打印迷宮用的符號 */
byte_t symbolic[3] = {66,64,69};//{66,64,69};
/* 打印地圖*/
byte_t symbol[3] = {0,1,2};
/* 求兩點間的距離 */
uint_t distance( uint_t pos1X, uint_t pos1Y, uint_t pos2X, uint_t pos2Y )
{
uint_t ret = 0;
/* 距離公式 */
ret = (uint_t)sqrt((pow((double)((int)pos1X - (int)pos2X),2) + pow((double)((int)pos1Y - (int)pos2Y),2)));
return ret;
}
/* 壓縮坐標 */
uint_t create_pos( uint_t pX, uint_t pY )
{
uint_t ret = 0;
/* 將pX賦給ret,這樣pX坐標在ret的低八位 */
ret = pX;
/* 將pX坐標移到高八位去,這樣低位就能存放pY */
ret <<= 8;
/* 將pY存放放到ret的低八位,并保持高八位的數據不變 */
ret |= pY;
return ret;
}
/****************************************************************************/
/*估計函數
/*-p : 當前移動到的節點指針
/*-quit_x
/*-quit_y : quit_x 和 quit_y表示迷宮出口坐標
/*-maze : 地圖矩陣
/****************************************************************************/
path_t * evaluate( path_t *p, uint_t quit_x, uint_t quit_y, byte_t maze[MAZE_HEIGHT][MAZE_WIDTH] )
{
uint_t pX, pY;
/* 用于接收四個方向離開出口的距離,以便選擇最近的方向移動 */
int dis[4];
int minDis = 32767;
int minId = -1;
path_t *pnode = (path_t *)0;
register int i;
/* 計算當前節點的坐標 */
pX = p->pos >> 8;
pY = p->pos & 0x00FF;
memset(dis, (int)-1, sizeof(int)*4);
/* 計算每個方向離開出口的距離,一次存放到dis數組,若沒有i方向,則dis[i]任保留-1 */
for( i = 0; i < 4; ++i ) {
if( (p->direct & d_signal[i]) >> i == 0x01 )
dis[i] =(int)distance(pX + move[i][0], pY + move[i][1], quit_x, quit_y);
}
/* 獲得最短距離的方向 */
for(i = 0; i < 4; ++i) {
if(dis[i] != -1 && dis[i] < minDis) {
minId = i;
minDis = dis[i];
}
}
/* 若沒有可用的方向,則通知尋徑函數折回 */
if(minId == -1)
return (path_t *)0;
/* 用去最近距離方向的信號量 */
p->direct &= d_spend[minId];
/* 在移動到新位置之前,在舊位置處留下足跡 */
maze[pY][pX] = (byte_t)PATH_FOOTPRINT;
/* 構建新的路徑節點 */
pnode = (path_t *)malloc(sizeof(path_t));
assert(pnode);
/* 計算下一個位置的坐標 */
pX += move[minId][0];
pY += move[minId][1];
pnode->pos = create_pos(pX, pY);
pnode->prev = p;
pnode->next = (path_t *)0;
pnode->direct = 0;
/* 在新位置處,計算下一個位置可用的移動方向 */
for(i = 0; i < 4; ++i) {
if(maze[pY + move[i][1]][pX + move[i][0]] != PATH_BLOCK && maze[pY + move[i][1]][pX + move[i][0]] != PATH_FOOTPRINT) {
/* 若嘗試的下一個位置不是障礙物或自己走過的足跡,則視為可用方向,獲得該方向的信號量 */
pnode->direct |= d_signal[i];
}
}
return pnode;
}
/****************************************************************************/
/*== A*算法尋徑函數
/*-eX
/*-eY :入口坐標
/*-qX
/*-qY :出口坐標
/*-maze :矩陣
/****************************************************************************/
path_t * AStar(uint_t eX, uint_t eY, uint_t qX, uint_t qY, byte_t maze[MAZE_HEIGHT][MAZE_WIDTH])
{
register int i;
/* 壓縮坐標 */
uint_t quit_pos = create_pos(qX, qY);
/* 構建入口路徑節點,視為路徑鏈的頭 */
path_t *head = (path_t *)malloc(sizeof(path_t));
path_t *p = (path_t *)0;
path_t *back = (path_t *)0;
assert(head);
p = head;
p->direct = 0;
p->pos = create_pos(eX,eY);
p->next = (path_t *)0;
p->prev = (path_t *)0;
/* 創建入口處的可用方向 */
for(i = 0; i < 4; ++i) {
if(maze[eY + move[i][1]][eX + move[i][0]] != PATH_BLOCK)
/* 若無障礙物則獲得該方向的信號量 */
p->direct |= d_signal[i];
}
do {
/* 獲得下個路徑的節點指針 */
back = evaluate(p, qX, qY, maze);
if(back) {
p->next = back;
p = p->next;
}
/* 無路可走則折回 */
if(p->direct == 0 && p != head && p->pos != quit_pos) {
back = p->prev;
back->next = (path_t *)0;
/* 清楚腳印 */
maze[p->pos & 0x00FF][p->pos >> 8] = (byte_t)PATH_WALKON;
free(p);
p = back;
}
/* 若走不出迷宮,即折回到入口,且入口處的可用方向全部嘗試過 */
if(p == head && p->direct == 0) {
free(head);
return (path_t *)0;
}
} while( p->pos != quit_pos );
/* 在出口處留下腳印,便于打印 */
maze[p->pos & 0x00FF][p->pos >> 8] = (byte_t)PATH_FOOTPRINT;
return head;
}
/*****************************************************************/
/*函數名稱： show_menu_DaoHang.c */
/*函數功能： GPS導航 */
/*有無返回： 無 */
/*修改記錄： 無修改記錄 */
/*入口參數：
start_x------------------------------------起點x坐標
start_y------------------------------------起點y坐標
end_x ------------------------------------終點x坐標
end_y ------------------------------------終點y坐標
*/
/*編寫日期： 2008-5-01 */
/*****************************************************************/
//void show_menu_DaoHang(unsigned char start_x,unsigned char start_y,unsigned char end_x,unsigned char end_y)
void show_menu_DaoHang()
{
unsigned char i=0,j=0;
path_t *pHead = AStar((uint_t)(Position/1000),(uint_t)(Position%1000/100),(uint_t)(Position%100/10),(uint_t)(Position%10),maze0);
for(i = 0; i < MAZE_HEIGHT; ++i)
{
for(j = 0; j < MAZE_WIDTH; ++j)
Display_Point(j,i,symbol[maze0[i][j]]);
}
}
void Show_Map()
{
unsigned char point_x=0,point_y=0;
for(point_y = 0; point_y < MAZE_HEIGHT; ++point_y)
{
for(point_x = 0; point_x < MAZE_WIDTH; ++point_x)
Display_Point(point_x,point_y,symbol[maze1[point_y][point_x]]);
/*
j-------代表x方向
i-------代表y方向
symbolic-路徑顯示
*/
}
}
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -