C++AGV 路径规划与运行仿真程序.docx

1、C+AGV 路径规划与运行仿真程序C+AGV 路径规划与运行仿真程序程序代码#include#include#include #include#include#include#include#define MAX 100#define maxD 99999class CFixedMap struct Coordinate int x; int y; int num; ;/记录点的结构体 struct combp int x1,y1; int x2,y2; ;/连通点坐标 struct Combname int num1; int num2; ;/连通点名 public: int mVexNum

2、; / 顶点数 int mEdgNum; / 边数 double mMatrixMAXMAX; / 邻接矩阵 Combname combMAX; /连通点名 combp cpMAX; /连通点坐标 Coordinate mVexsMAX; / 顶点集合 int getx(int n1); int gety(int n2); void Input();/以文件方式输入地图信息 void ShowMap();/easyx显示地图;int CFixedMap:getx(int n1) int x=0; for(int i=0;imVexNum;i+) if(n1=mVexsi.num) x=mVex

3、si.x; return x;int CFixedMap:gety(int n2) int y=0; for(int i=0;imVexNum;i+) if(n2=mVexsi.num) y=mVexsi.y; return y;void CFixedMap:Input() int panju; char fname10=map2.txt; coutpanju; if(panju=2) coutfname; ifstream file(fname); char use1100; char use2100; char use3100; char use4100; /use数组用来储存提示信息。

4、fileuse1; filemVexNum; /端点个数 fileuse2; filemEdgNum; /边数 fileuse3; for(int i=0;imVexsi.nummVexsi.xmVexsi.y; fileuse4; for(i=0;icombi.num1combi.num2; file.close();/easyx显示地图void CFixedMap:ShowMap() initgraph(800,600,SHOWCONSOLE| NOCLOSE); /初始化绘图环境 setbkcolor(WHITE); /设置背景色 cleardevice(); /用背景色清空屏幕 set

5、fillcolor(BLACK); /设置填充色 /画顶点 for(int i=0;imVexNum;i+) fillcircle(mVexsi.x,mVexsi.y,5); settextcolor(BLACK); char sMAX; sprintf(s, %d, i+1); outtextxy(mVexsi.x-10,mVexsi.y-20,s); /给实际连通的顶点连线 setlinecolor(RGB(0,0,0); setlinestyle(PS_SOLID,6,NULL,0); for(i=0;imEdgNum;i+) cpi.x1=getx(combi.num1); cpi.y

6、1=gety(combi.num1); cpi.x2=getx(combi.num2); cpi.y2=gety(combi.num2); line(cpi.x1,cpi.y1,cpi.x2,cpi.y2); class CPathPlan:public CFixedMappublic: int pathMAX; /存放最后一个点 double distMAX; /存放路径距离 int v0; /起始点 int route40; /路径 int rnum; /路径顶点数 Coordinate rpointsMAX;/路径点集 void MaptoGraph(); void Dijstra();

7、 void Putpath();void CPathPlan:MaptoGraph() rnum=0; int a,b,x,y; for(int i=0;imVexNum;i+) for(int j=0;jmVexNum;j+) if(i=j) mMatrixij=0; else mMatrixij=maxD; for(i=0;imEdgNum;i+) if (combi.num1) a=combi.num1-1; b=combi.num2-1; x=mVexsa.x-mVexsb.x; y=mVexsa.y-mVexsb.y; mMatrixab=sqrt(x*x+y*y); mMatrix

8、ba=sqrt(x*x+y*y); void CPathPlan:Dijstra() coutv0; v0-; int sMAX; int v; int i; int j; int w; double min; for(v=0;vmVexNum;v+) /初始化S,dist,path sv=0; /0表示未求出最短路径 distv=mMatrixv0v; /开始时假定为最短路径 if(distvmaxD & v!=v0) pathv=v0; /直达情况 else pathv=-1; /无直达路径 distv0=0; /初始时v0属于s集，v0到v0路径最短 sv0=1; for(i=1;imV

9、exNum;i+) /假设由一个中转点到达会近些，验证。再假设由两个中转点到达会近些，验证。穷举 min=maxD; for(w=0;wmVexNum;w+) /找一个最近到的点，下标装入v if(sw=0 & distwmin) /点w不属于s集且离v0更近 v=w; /经点w中转 min=distw; sv=1; /将v并入S,由v0到达v点最短距离为min,假设由v0到v再由v到其余各点，更新当前最后一个点及距离 for(j=0;jmVexNum;j+) if(sj=0 & (min+mMatrixvjdistj) /如果多经过一个点v到点j的距离更短，更新 distj=min+mMat

10、rixvj; pathj=v; /点的序号 void CPathPlan:Putpath() int opp20; int z; int i=0; coutz; z-; int next; ofstream file2(fops.txt); if(distzmaxD & z!=v0) /如果有路径 oppi+=mVexsz.num; /终点 next=pathz; /第一个点 while(next!=v0) oppi+=mVexsnext.num; next=pathnext; /下一个点 oppi=mVexsv0.num; file2路径长度为:distzn; /起点v0和最短路径 else

11、 if(z!=v0) /无路径 file2mVexsz.num-mVexsv0.numno pathn; for(int j=0;ji+1;j+) routej=oppi-j; file2路径为:; for(j=0;ji;j+) file2routej; rnum=i+1; file2routei; file2.close(); for(i=0;irnum;i+) rpointsi.x=getx(routei); rpointsi.y=gety(routei); rpointsi.num=routei; class CAGV int speed; /AGV速度 int width; /AGV宽

12、度 int length; /AGV长度public: CAGV(); int getl(); int getw(); int gets();CAGV:CAGV() int panju; coutpanju; speed=8; width=6; length=6; if(panju=2) coutlength; coutwidth; coutspeed; int CAGV:getl() return length;int CAGV:getw() return width;int CAGV:gets() return speed;class CAGVShow:public CPathPlan,p

13、ublic CAGVpublic:#define k (abs(y2-y1)/(abs(x1-x2) int a,b,c,d; int x1,x2,y1,y2; int i,j; int l,w,s; void show(); ;void CAGVShow:show() l=getl(); w=getw(); s=11-gets(); for(int o=0;ox1&y2y1) for(j=1;jx2-x1;j+) a=x1+j-l; b=y1+j*k+w; c=x1+j+l; d=y1+j*k-w; setfillcolor(RGB(0,0,0); rectangle(a,b,c,d); c

14、learrectangle(a-1,b-k,c,d-k); setlinecolor(RED); line(x1+j,y1+j*k,x2,y2); setlinecolor(GREEN); line(x1,y1,x1+j,y1+j*k); if(j%2=0) setfillcolor(RGB(255,255,0); circle(x2,y2,5); Sleep(s); if(x2x1&y2y1) for(j=1;jx1-x2;j+) a=x1-j-l; b=y1-j*k+w; c=x1-j+l; d=y1-j*k-w; setfillcolor(RGB(0,0,0); rectangle(a,

15、b,c,d); clearrectangle(a-1,b-k,c,d-k); setlinecolor(RED); line(x1-j,y1-j*k,x2,y2); setlinecolor(GREEN); line(x1,y1,x1-j,y1-j*k); if(j%2=0) setfillcolor(RGB(255,255,0); circle(x2,y2,5); Sleep(s); if(x2y1) for(j=1;jx1&y2y1) for(j=1;jx1) for(j=1;j(x2-x1);j+) setfillcolor(RGB(0,0,0); rectangle(x1+j-l,y1

16、+w,x1+j+l,y1-w); clearrectangle(x1+j-l-1,y1+w,x1+j+l-1,y1-w); setlinecolor(RED); line(x1+j,y1,x2,y2); setlinecolor(GREEN); line(x1,y1,x1+j,y1); if(j%2=0) setfillcolor(RGB(255,0,255); circle(x2,y2,5); Sleep(s); if(x2x1) for(j=1;jy1) for(j=1;j(y2-y1);j+) setfillcolor(BLUE); rectangle(x1-l,y1+j+w,x1+l,

17、y1+j-w); setfillcolor(WHITE); rectangle(x1,y1+j+w-1,x1+1,y1+j-1-w); clearrectangle(x1-l-1,y1+(j-1)+w,x1+l-1,y1+(j-1)-w); setlinecolor(RED); line(x1,y1+j,x2,y2); setlinecolor(GREEN); line(x1,y1,x1,y1+j); if(j%2=0) setfillcolor(RGB(255,255,0); circle(x2,y2,5); Sleep(s); if(y2y1) for(j=1;j(y1-y2);j+) s

18、etfillcolor(BLUE); rectangle(x1-l,y1-j+w,x1+l,y1-j-w); setfillcolor(WHITE); rectangle(x1,y1-j+w-1,x1+1,y1-j-1-w); clearrectangle(x1-l-1,y1-(j-1)+w,x1+l-1,y1-(j-1)-w); setlinecolor(RED); line(x1,y1-j,x2,y2); setlinecolor(GREEN); line(x1,y1,x1,y1-j); if(j%2=0) setfillcolor(RGB(255,255,0); circle(x2,y2

19、,5); Sleep(s); cout成功显示!n; getch(); / 按任意键继续 closegraph(); / 关闭图形界面 void main() CAGVShow a; int panju; a.Input(); coutpanju; if(panju=1) for(int i=0;i20;i+) a.ShowMap(); a.MaptoGraph(); a.Dijstra(); a.Putpath(); a.show(); panju=0; coutpanju; if(panju=1) break; else a.ShowMap(); a.MaptoGraph(); a.Dijstra(); a.Putpath(); a.show(); 相关文件/map2.txt点的个数(输入所有数据前请换行): 16连线条数：26依次输入所有点的编号，x,y坐标（用空格隔开）：1 50 50 2 250 50 3 550 50 4 750 50 5 50 150 6 250 150 7 550 150 8 750 150 9 50 350 10 250 350 11 550 350 12 750 350 13 50 550 14 250 550 15 550 550 16 750 550输入所有互相连通的点的编号：