C++AGV路径规划与运行仿真程序.docx
《C++AGV路径规划与运行仿真程序.docx》由会员分享,可在线阅读,更多相关《C++AGV路径规划与运行仿真程序.docx(21页珍藏版)》请在冰豆网上搜索。
C++AGV路径规划与运行仿真程序
程序代码
#include
#include
#include
#include
#include
#include
#include
#defineMAX100
#definemaxD99999
classCFixedMap
{
structCoordinate
{
intx;
inty;
intnum;
};//记录点的结构体
structcombp
{
intx1,y1;
intx2,y2;
};//连通点坐标
structCombname
{
intnum1;
intnum2;
};//连通点名
public:
intmVexNum;//顶点数
intmEdgNum;//边数
doublemMatrix[MAX][MAX];//邻接矩阵
Combnamecomb[MAX];//连通点名
combpcp[MAX];//连通点坐标
CoordinatemVexs[MAX];//顶点集合
intgetx(intn1);
intgety(intn2);
voidInput();//以文件方式输入地图信息
voidShowMap();//easyx显示地图
};
intCFixedMap:
:
getx(intn1)
{
intx=0;
for(inti=0;iif(n1==mVexs[i].num)
x=mVexs[i].x;
returnx;
}
intCFixedMap:
:
gety(intn2)
{
inty=0;
for(inti=0;iif(n2==mVexs[i].num)
y=mVexs[i].y;
returny;
}
voidCFixedMap:
:
Input()
{
intpanju;
charfname[10]={"map2.txt"};
cout<<"是否载入默认地图?
(1:
是,2:
否)";
cin>>panju;
if(panju==2)
{
cout<<"请输入地图信息文件名:
";
cin>>fname;
}
ifstreamfile(fname);
charuse1[100];
charuse2[100];
charuse3[100];
charuse4[100];//use数组用来储存提示信息。
file>>use1;
file>>mVexNum;//端点个数
file>>use2;
file>>mEdgNum;//边数
file>>use3;
for(inti=0;i{
file>>mVexs[i].num>>mVexs[i].x>>mVexs[i].y;
}
file>>use4;
for(i=0;i{
file>>comb[i].num1>>comb[i].num2;
}
file.close();
}
//easyx显示地图
voidCFixedMap:
:
ShowMap()
{
initgraph(800,600,SHOWCONSOLE|NOCLOSE);//初始化绘图环境
setbkcolor(WHITE);//设置背景色
cleardevice();//用背景色清空屏幕
setfillcolor(BLACK);//设置填充色
//画顶点
for(inti=0;i{
fillcircle(mVexs[i].x,mVexs[i].y,5);
settextcolor(BLACK);
chars[MAX];
sprintf(s,"%d",i+1);
outtextxy(mVexs[i].x-10,mVexs[i].y-20,s);
}
//给实际连通的顶点连线
setlinecolor(RGB(0,0,0));
setlinestyle(PS_SOLID,6,NULL,0);
for(i=0;i{
cp[i].x1=getx(comb[i].num1);
cp[i].y1=gety(comb[i].num1);
cp[i].x2=getx(comb[i].num2);
cp[i].y2=gety(comb[i].num2);
line(cp[i].x1,cp[i].y1,cp[i].x2,cp[i].y2);
}
}
classCPathPlan:
publicCFixedMap
{
public:
intpath[MAX];//存放最后一个点
doubledist[MAX];//存放路径距离
intv0;//起始点
introute[40];//路径
intrnum;//路径顶点数
Coordinaterpoints[MAX];//路径点集
voidMaptoGraph();
voidDijstra();
voidPutpath();
};
voidCPathPlan:
:
MaptoGraph()
{
rnum=0;
inta,b,x,y;
for(inti=0;i{
for(intj=0;j{
if(i==j)
{
mMatrix[i][j]=0;
}
else
{
mMatrix[i][j]=maxD;
}
}
}
for(i=0;i{
if(comb[i].num1)
{
a=comb[i].num1-1;
b=comb[i].num2-1;
x=mVexs[a].x-mVexs[b].x;
y=mVexs[a].y-mVexs[b].y;
mMatrix[a][b]=sqrt(x*x+y*y);
mMatrix[b][a]=sqrt(x*x+y*y);
}
}
}
voidCPathPlan:
:
Dijstra()
{
cout<<"输入起点:
";
cin>>v0;
v0--;
ints[MAX];
intv;
inti;
intj;
intw;
doublemin;
for(v=0;v{
s[v]=0;//0表示未求出最短路径
dist[v]=mMatrix[v0][v];//开始时假定为最短路径
if(dist[v]=v0)
path[v]=v0;//直达情况
elsepath[v]=-1;//无直达路径
}
dist[v0]=0;//初始时v0属于s集,v0到v0路径最短
s[v0]=1;
for(i=1;i再假设由两个中转点到达会近些,验证。
穷举
{
min=maxD;
for(w=0;wif(s[w]==0&&dist[w]{
v=w;//经点w中转
min=dist[w];
}
s[v]=1;//将v并入S,由v0到达v点最短距离为min,假设由v0到v再由v到其余各点,更新当前最后一个点及距离
for(j=0;jif(s[j]==0&&(min+mMatrix[v][j]{
dist[j]=min+mMatrix[v][j];
path[j]=v;//点的序号
}
}
}
voidCPathPlan:
:
Putpath()
{
intopp[20];
intz;
inti=0;
cout<<"输入终点:
";
cin>>z;
z--;
intnext;
ofstreamfile2("fops.txt");
if(dist[z]=v0)//如果有路径
{
opp[i++]=mVexs[z].num;//终点
next=path[z];//第一个点
while(next!
=v0)
{
opp[i++]=mVexs[next].num;
next=path[next];//下一个点
}
opp[i]=mVexs[v0].num;
file2<<"路径长度为:
"<}
elseif(z!
=v0)//无路径
file2<for(intj=0;j
route[j]=opp[i-j];
file2<<"路径为:
";
for(j=0;j
file2<";
rnum=i+1;
file2<file2.close();
for(i=0;i{
rpoints[i].x=getx(route[i]);
rpoints[i].y=gety(route[i]);
rpoints[i].num=route[i];
}
}
classCAGV
{
intspeed;//AGV速度
intwidth;//AGV宽度
intlength;//AGV长度
public:
CAGV();
intgetl();
intgetw();
intgets();
};
CAGV:
:
CAGV()
{
intpanju;
cout<<"是否自动设置AGV信息?
(1:
是,2:
否)";
cin>>panju;
speed=8;
width=6;
length=6;
if(panju==2)
{
cout<<"输入AGV的长:
";
cin>>length;
cout<<"输入AGV的宽:
";
cin>>width;
cout<<"输入AGV的速度(1-10):
";
cin>>speed;
}
}
intCAGV:
:
getl()
{
returnlength;
}
intCAGV:
:
getw()
{
returnwidth;
}
intCAGV:
:
gets()
{
returnspeed;
}
classCAGVShow:
publicCPathPlan,publicCAGV
{
public:
#definek(abs(y2-y1))/(abs(x1-x2))
inta,b,c,d;
intx1,x2,y1,y2;
inti,j;
intl,w,s;
voidshow();
};
voidCAGVShow:
:
show()
{
l=getl();
w=getw();
s=11-gets();
for(into=0;o{
if(o+1==rnum)break;
x1=rpoints[o].x;
x2=rpoints[o+1].x;
y1=rpoints[o].y;
y2=rpoints[o+1].y;
if(y1!
=y2&&x1!
=x2)
{
if(x2>x1&&y2>y1){
for(j=1;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);
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(x2{
for(j=1;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);
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;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);
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(x2>x1&&y2{
for(j=1;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);
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);
}
}
}
elseif(y1==y2)
{
if(x2>x1)
{
for(j=1;j<(x2-x1);j++)
{
setfillcolor(RGB(0,0,0));
rectangle(x1+j-l,y1+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(x2{
for(j=1;j<(x1-x2);j++)
{
setfillcolor(RGB(0,0,0));
rectangle(x1-j-l,y1+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);
}
}
}
elseif(x1==x2)
{
if(y2>y1)
{
for(j=1;j<(y2-y1);j++)
{
setfillcolor(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,5);
}
Sleep(s);
}
}
if(y2{
for(j=1;j<(y1-y2);j++)
{
setfillcolor(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,5);
}
Sleep(s);
}
}
}
}
cout<<"成功显示!
!
\n";
getch();//按任意键继续
closegraph();//关闭图形界面
}
voidmain()
{
CAGVShowa;
intpanju;
a.Input();
cout<<"多次演示?
(1:
是,2:
否)";
cin>>panju;
if(panju==1)
{
for(inti=0;i<20;i++)
{
a.ShowMap();
a.MaptoGraph();
a.Dijstra();
a.Putpath();
a.show();
panju=0;
cout<<"是否结束?
(1:
结束)";
cin>>panju;
if(panju==1)
break;
}
}
else
{
a.ShowMap();
a.MaptoGraph();
a.Dijstra();
a.Putpath();
a.show();
}
}
相关文件
//map2.txt
点的个数(输入所有数据前请换行):
16
连线条数:
26
依次输入所有点的编号,x,y坐标(用空格隔开):
150502250503550504750505501506250150755015087501509503501025035011550350127503501350550142505501555055016750550
输入所有互相连通的点的编号:
1223345667