robot.cpp

吸尘机器人全覆盖算法仿真源码,希望对大家有用

#include"robot.h"
#include<fstream>
#include<vector>
using namespace std;
int Robot::Move(){
	//Moving the robot,detecting the environment and saving
	//it to Electronic Map(EM).
	//Return Value:
	//1=Successful;0=There are obstacle in current direction;
	//3=Unexpectable fail;
	int direct;int moved;int i;int step;	
	bool btemp=(bMoveFlag_Move20PixelsFinished)?true:false;
    
	           if(btemp){
				       
		                 
							     AdjustMap();
								 if(!visited) visited=1;
				                 bMoveFlag_Move20PixelsFinished=false;
								 bMoveOutsideFlag_MoveFinished=true;
			                     //bMoveFlag_NeedDetectEnvi=false;
				                 return 1;//successfully moved 20 pixels/1 robot unit.
						 
			   }
			   if(bMoveFlag_NeedDetectEnvi){
	                    for(i=1;i<=3;i++){
      	                //Are there obstacles in the other three direction?
		                      if(Detect(direct=NextDirection(i))){
		                              SetMap(RDToVD(direct),1);
		                              //the difference between CurrentDirection and CurrentDirectionInEM
		                              //is DiffereceOfRDandVD.Thus,you can set EM according to 
		                              //CurrentDirection which is a direction in real world.
							  }//if
						}//for
						bMoveFlag_NeedDetectEnvi=false;
						
	
			   }//if
			   if(!btemp){   
				   if(bMoveFlag_NewStart){
				             if(Detect(CurrentDirection)){
								   bMoveFlag_Move20PixelsFinished=false;
								   bMoveFlag_NeedDetectEnvi=true;
								   bMoveOutsideFlag_MoveFinished=true;
			                       SetMap(CurrentDirectionInEM,1);
				                   return 0;//encounter obstacle
							 }
						     bMoveFlag_NewStart=false;
				   }
				        bMoveOutsideFlag_MoveFinished=false;
		                moved=PhyMove(4-nMoveFlag_HasMoved4pixels);
		                nMoveFlag_HasMoved4pixels+=moved;
		                if(nMoveFlag_HasMoved4pixels==4){
			                    switch(CurrentDirection){
			                            case 1:
				                           CurrentPoint.y++;
				                           break;
			                            case 2:
				                           CurrentPoint.x++;
				                           break;
			                            case 3:
				                           CurrentPoint.y--;
				                           break;
			                            case 4:
				                           CurrentPoint.x--;
				                           break;
								}
				                //set places where the robot has cleaned to 2
				                for(int i=CurrentPoint.x-2;i<=CurrentPoint.x+2;i++){
					                   for(int j=CurrentPoint.y-2;j<=CurrentPoint.y+2;j++){
                                                if(!*(realmap+map_width2*i+j))
							                           *(realmap+map_width2*i+j)=2;
									   }
								}
								nMoveFlag_HasMoved4pixels=0;
						}//if
                        nMoveFlag_HasMoved20pixels+=moved;
		                if(nMoveFlag_HasMoved20pixels==20){
			                     bMoveFlag_NeedDetectEnvi=true;
			                     bMoveFlag_Move20PixelsFinished=true;
								 bMoveFlag_NewStart=true;
			                     nMoveFlag_HasMoved20pixels=0;
						}
			   }//else
return 2;
}//Move
////////////////////////////////////
int Robot::MoveA(){
	*(realmap+CurrentPoint.x*21+CurrentPoint.y)=9;
	//mark the robot's track in real world.
	if(!Detect(CurrentDirection)){
		if(/*!RealMove()*/true) return 3;
		else{AdjustMap();}
		return 1;
	}else{SetMap(CurrentDirectionInEM,1);return 0;}
}

/////////////////////////////////////////////////////////////////
//!!!!!!!!!!!!!!!!!!!!!!!!!MoveOutside!!!!!!!!!!!!!!!!!!!!!!!!!!!
/////////////////////////////////////////////////////////////////
int Robot::MoveOutside(){
	int result;
	if((CurrentPointInEM.x==StartPointInEM.x)&&(CurrentPointInEM.y==StartPointInEM.y)&&visited
		){
#pragma cMsg(uncompleted1)
		if(MoveOutsideSucceed()) {MarkMap();
			return 1;}}
		
	if(SeeMagnet){
		   //机器人遇到了磁铁(设置在墙附近),则执行紧贴墙行走的算法
	      if(bMoveOutsideFlag_MoveFinished){
		        if(!runflag){
			          ClearMap();
		              visited=0;CurrentPointInEM.x=1;
		              CurrentPointInEM.y=1;StartPointInEM.x=1;StartPointInEM.y=1;
			          bMoveFlag_NeedDetectEnvi=true;
					  bMoveFlag_NewStart=true;
                      bMoveFlag_Move20PixelsFinished=false;
			           if(Detect(NextDirection(3))){
			                   //机器人须逆时针行走
			                   runflag=1;
					  }else if(Detect(NextDirection(1))){
			                   //机器人须顺时针行走
			                   runflag=2;}
				}else{
					 bMoveFlag_NeedDetectEnvi=true;
			         if(runflag==1){
				
				           if(Detect(CurrentDirection)) AnticlockwiseTurn();
						   if(!Detect(NextDirection(1))) ClockwiseTurn();
						   Move();

					 }//if(runflag...)
			         if(runflag==2){
				
				           if(Detect(CurrentDirection)) ClockwiseTurn();
						   if(!Detect(NextDirection(3))) AnticlockwiseTurn();
					       Move();
					 }//if(runflag...)
				}
         }else{Move();}//if(b...)
	}else{
		 if(!(result=Move())){//obstacle
			     if(!SeeObstacleBefore){
		             //If It's the first time that the robot see an obstacle,
		             //robot will clear all its memory(EM).
                     ClearMap();
		             SeeObstacleBefore=1;visited=0;CurrentPointInEM.x=1;
		             CurrentPointInEM.y=1;StartPointInEM.x=1;StartPointInEM.y=1;
				     DifferenceOfRDandVD=CurrentDirection-CurrentDirectionInEM;
				     ClockwiseTurn();
				  }else{ClockwiseTurn();}
		}
	}//else
    return 0;
}
/////////////////////////////////////////////////////////////////
bool Robot::MoveOutsideSucceed(){
	/*
	ofstream out("c:\map2.txt");
	for(int iTemp=0;iTemp<40;iTemp++){
		for(int jTemp=0;jTemp<40;jTemp++){
			out<<map[iTemp][jTemp]<<"-";
		}
		out<<endl;
	}
	*/
	int direction=0;int i=0,j=0;bool start=false;int flag=0;bool turned=false;
	for(i=39;map[i][1]!=2;i--);
	direction=4;j=1;
	int iStart=i;int jStart=j;
	while((((iStart!=i)||(jStart!=j))||!start)&&map[i][j]==2){
		start=true;
		switch(direction){
		case 1:
			if(!turned&&map[i-1][j]!=1) {
				if(!flag){direction=4;flag=0;turned=true;break;}
				flag++;
			}
			if(flag>=2) return false;
            if(map[i][j+1]==1) {direction=2;break;}
		    if(j<39) j++;
			turned=false;
		    break;
	    case 2:
		    if(!turned&&map[i][j+1]!=1) {
				if(!flag){direction=1;flag=0;turned=true;break;}
				flag++;
			}
			if(flag>=2) return false;
			if(map[i+1][j]==1) {direction=3;break;}
		    if(i<39) i++;
			turned=false;
		    break;
	    case 3:
		    if(!turned&&map[i+1][j]!=1) {
				if(!flag){direction=2;flag=0;turned=true;break;}
				flag++;
			}
			if(flag>=2) return false;
			if(map[i][j-1]==1) {direction=4;break;}
		    if(j>1) j--;
			turned=false;
		    break;
	    case 4:
		    if(!turned&&map[i][j-1]!=1) {
				if(!flag){direction=3;flag=0;turned=true;break;}
				flag++;
			}
			if(flag>=2) return false;
			if(map[i-1][j]==1) {direction=1;break;}
		    if(i>1) i--;
			turned=false;
		    break;
		}
	}
	if(map[i][j]!=2) return false;
	else return true;
}
////////////////////////////////////////////////////////
int Robot::MoveRectangularly(){
	//Moving Rectangularly in order to clean the rest of the room
	//The robot will moving anticlockwise by default.
	map[CurrentPointInEM.x][CurrentPointInEM.y]=6;
	while((map[CurrentPointInEM.x+1][CurrentPointInEM.y]==0)||
		  (map[CurrentPointInEM.x][CurrentPointInEM.y-1]==0)||
		  (map[CurrentPointInEM.x-1][CurrentPointInEM.y]==0)||
		  (map[CurrentPointInEM.x][CurrentPointInEM.y+1]==0)||
		  (map[CurrentPointInEM.x][CurrentPointInEM.y]==0)){
		//There are place which is uncleaned around CurrentPointInEM
        if(!DetectA(NextDirectionInEM(1))) ClockwiseTurn();
		if((!MoveA())||DetectA(CurrentDirectionInEM))
			AnticlockwiseTurn();
	}
	map[CurrentPointInEM.x][CurrentPointInEM.y]=7;
	return 1;
}
/////////////////////////////////////////////////////////////////
int Robot::AnticlockwiseTurn(){
    if(RotateAC())//robot rotates anticlockwise
	{
		CurrentDirectionInEM=NextDirectionInEM(3);
		return 1;
	}else{
		MessageBox(GetActiveWindow(),"turn error","",MB_OK);
		return 0;}//unknow error
}//AnticlockwiseTurn()

int Robot::ClockwiseTurn(){
	if(RotateC()){//robot rotates clockwise
		CurrentDirectionInEM=NextDirectionInEM(1);
		return 1;
}else {
		MessageBox(GetActiveWindow(),"turn error","",MB_OK);
		return 0;}

}//ClockwiseTurn()
/////////////////////////////////////////////////////////////////
//AdjustMap()
////////////////////////////////////////////////////////////////
bool Robot::AdjustMap(){
	if((CurrentPointInEM.y==1)&&(CurrentDirectionInEM==3)){
		//Electronic map array is going to be out of bound?
#pragma cMsg(Attention:Robot::AdjustMap need to be fixed later)
		for(int i=38;i>=0;i--){
			//Suppose that the Electronic map is large enough,i.e.
			//the eletronic map is head and shoulder above the real map.
			for(int j=0;j<40;j++){
				map[j][i+1]=map[j][i];
				map[j][i]=0;
                //moving the element of EM one unit to right,i.e.
				//direction 1 which is oppsite direction 3.
			}
		}
		StartPointInEM.y++;CurrentPointInEM.y++;
	}//if

	if((CurrentPointInEM.x==1)&&(CurrentDirectionInEM==4)){
		for(int j=38;j>=0;j--){
			for(int i=0;i<40;i++){
               map[j+1][i]=map[j][i];
			   map[j][i]=0;
			}
		}
		StartPointInEM.x++;CurrentPointInEM.x++;
	}//if
    map[CurrentPointInEM.x][CurrentPointInEM.y]=2;//cleaned
	   switch(CurrentDirectionInEM){
	    case 1:CurrentPointInEM.y++;break;
	    case 2:CurrentPointInEM.x++;break;
	    case 3:CurrentPointInEM.y--;break;
	    case 4:CurrentPointInEM.x--;break;
	    default:return false;
	   }

    return true;
}//AdjustMap()

void Robot::ClearMap(){
	for(int i=0;i<40;i++)
		for(int j=0;j<40;j++)
			map[i][j]=0;
}
//////////////////////FindIdealpath//////////////////////////////
int Robot::FindIdealpath(int xstart,int ystart,int xend,int yend){
	//中点线算法找出理想路径,未完成
     point pstart;pstart.x=xstart;pstart.y=ystart;
	 point ptemp;
	 point pend;  pend.x=xend;    pend.y=yend;
	 idealpath.push_back(pstart);
     
	 int dy=-(xend-xstart); int dx=yend-ystart;
//	 double k=dy/dx;//the slope

	 int d=0;
	 int ycurr=xstart; int xcurr=ystart;
	 //the slope is positive
     if((dy<=dx)&&(dy>=0)){
		 //the slope is lesser than 1 and larger than 0
		 //the line is in the first quardant.
		 d=2*dy-dx;//d0
		 while(xcurr<yend){
		    if(d<0){
			  d+=2*dy;
			  xcurr++;
			}else{
			  d+=2*(dy-dx);
			  xcurr++;ycurr--;
			}
		    ptemp.x=ycurr;ptemp.y=xcurr;
		    idealpath.push_back(ptemp);
		 }//while
		 return 1;
	 }
#pragma cMsg(uncompleted2)
	 if((dy>dx)&&(dx>0)){
		 //the slope is larger than 1.
		 //the line is in the first quardant.
		 d=dy-2*dx;//d0
		 while(ycurr>xend){
			 if(d<0){
				 d+=2*(dy-dx);
				 xcurr++;ycurr--;
			 }
			 else{
				 d+=2*(-dx);
				 ycurr--;
			 }
		 ptemp.x=ycurr;ptemp.y=xcurr;
		 idealpath.push_back(ptemp);
		 }
		 return 1;}
	 //the slope is negative
	 if((dy>(-dx))&&(dx<0)){
	     //the slope is lesser than -1;
		 //the line is in the second quardant.
		 d=-dy-2*dx;
		 while(ycurr>xend){
			 if(d>=0){//d<0?
				 d+=-dy-dx;
				 xcurr--;ycurr--;
              }else{
				 d+=(-dx);
				 ycurr--;
			  }
		 ptemp.x=ycurr;ptemp.y=xcurr;
		 idealpath.push_back(ptemp);
		 }
		 return 1;}
	 if((dy<(-dx))&&(dx<0)&&(dy>=0)){
		 //the slope is larger than -1,lesser than 0
		 //the line is in the second quardant.
		 d=2*(-dy)-dx;
		 while(xcurr>yend){
			 if(d>=0){//d<0?
				 d+=2*(-dy);
				 xcurr--;
               }else{
				 d+=2*(-dy-dx);
				 xcurr--;ycurr--;
			   }
		 ptemp.x=ycurr;ptemp.y=xcurr;
		 idealpath.push_back(ptemp);
		 }
	     return 1;}
	 if(((-dy)<(-dx))&&(dx<0)&&(dy<0)){
		 //the slope is lesser than 1,and the line is in the 
		 //third quardant(dx<0,dy<0)
		 d=2*(-dy)+dx;
		 while(xcurr>yend){
			 if(d>0){//d<=0?
				 d+=2*(-dy+dx);
				 ycurr++;xcurr--;
            }else{
				 d+=2*(-dy);
				 xcurr--;
			}
		 ptemp.x=ycurr;ptemp.y=xcurr;
		 idealpath.push_back(ptemp);
		 }
	     return 1;}
	 if(((-dy)>(-dx))&&(dx<0)&&(dy<0)){
		 //the slope is larger than 1,and the line is in the 
		 //third quardant(dx<0,dy<0)
		 d=(-dy)+2*dx;
		 while(ycurr<xend){
			 if(d>0){//d<=0?
				 d+=2*dx;
				 ycurr++;
            }else{
				 d+=2*(-dy+dx);
				 xcurr--;ycurr++;
			}
		 ptemp.x=ycurr;ptemp.y=xcurr;
		 idealpath.push_back(ptemp);
		 }
		 return 1;}
	 if(((-dy)<dx)&&(dx>0)&&(dy<0)){
		 //the slope is lesser than -1,and the line is in the 
		 //fourth quardant(dx>0,dy<0)
		 d=2*(dy)+dx;
		 while(xcurr<yend){
			 if(d<=0){
				 d+=2*(dy+dx);
				 ycurr++;xcurr++;
            }else{
				 d+=2*(dy);
				 xcurr++;
			}
		 ptemp.x=ycurr;ptemp.y=xcurr;
		 idealpath.push_back(ptemp);
		 }
		 return 1;}
	 if(((-dy)>dx)&&(dx>0)&&(dy<0)){
		 //the slope is larger than -1,and the line is in the 
		 //fourth quardant(dx>0,dy<0)
		 d=dy+2*dx;
		 while(ycurr<xend){
			 if(d<=0){
				 d+=2*dx;
				 ycurr++;
            }else{
				 d+=2*(dy+dx);
				 xcurr++;ycurr++;
			}
		 ptemp.x=ycurr;ptemp.y=xcurr;
		 idealpath.push_back(ptemp);
		 }
		 return 1;}
	 if(dx==0){
		 //the slope is infinite
		 while(ycurr!=xend){
			 ptemp.x=ycurr;ptemp.y=xcurr;
//			 map[ptemp.x][ptemp.y]=5;//just for testing
			 if(ycurr>xend) ycurr--;
			 else ycurr++;
			 idealpath.push_back(ptemp);
		 }
		 ptemp.x=ycurr;ptemp.y=xcurr;
		 idealpath.push_back(ptemp);//pend
	     return 1;
	 }
	 return 1;
}
//////////////////MarkMap//////////////////////////////
int Robot::MarkMap(){
/*
	int i,j;int temp=0;
	int temp_inc_num=0;//consecutivly increased number of temp
	                 //in this algorithm ,temp can not increase
	                 //for three times.
	bool btemp=false;
	for(i=0;i<40;i++){
		for(j=0;j<40;j++){
			if(map[i][j]==1){
				temp++;temp_inc_num++;
				if(map[i][j+1]!=1)btemp=true;
			}else{
				if(btemp)temp=temp-temp_inc_num+1;
				if(!(temp%2)){
                   map[i][j]=1;
				   temp_inc_num=0;
				}//if
				btemp=false;
			}//else
            
		}//for
        temp=0;temp_inc_num=0;

	}//for
	return 1;
*/
	int i,j;
	for(i=0;i<40;i++){