robot.cpp

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

		if(map[*x+1][*y]==1){
			if((map[*x+2][*y]==4)||(*x+2)>21) return 2;
			else return 1;
		}else return 0;
	case 3:
		if(map[*x][*y-1]==1){
			if((map[*x][*y-2]==4)||(*y-2)<0) return 2;
			else return 1;
		}else return 0;
	case 4:
		if(map[*x-1][*y]==1){
			if((map[*x-2][*y]==4)||(*x-2)<0) return 2;
			else return 1;
		}else return 0;
	default:
		return 4;
	}
}
int Robot::ClearPath(){
//	vector<point>::iterator it=idealpath.begin();
idealpath.clear();return 1;
}
/////////////Detect Detect2/////////////////////////////////
bool Robot::Detect2(int direction){
	//Detect2 will be called inside Detect()
	//Detect2 will detect four blocks which surround the robot
	//and have the same size of robot(5*5), on exception that 
	//when the robot is at the border of the map(important).
	//if there were obstacles existed,return true,otherwise 
	//return false.
	int x=CurrentPoint.x;int y=CurrentPoint.y;
	int i,j;bool flag=false;int **m=_realmap.getMapPt();
	switch(direction){
	case 1:
		if((y+3)==(map_width2-1)){//at the right border,and detect direction 1(right)
			flag=true;
		}else{
		    y+=3;x-=2;//strat point of detection
		    for(j=x;j<x+5;j++){
				for(i=y;i<y+5;i++){
				    if(m[j][i]==1)
					    flag=true;
                    else flag=false;}
			}//for
		}//else
		return flag;
	case 2:
		if((x+3)==(map_height2-1)){//at the bottom border,and detect direction 2(down)
			flag=true;
		}else{
		    y-=2;x+=3;
		    for(j=x;j<x+5;j++)
				for(i=y;i<y+5;i++){
			 	   if(m[j][i]==1)
				   {flag=true;}
				   else flag=false;}
		}//else
		return flag;
	case 3:
		if((y-3)==0){//at the left border,and detect direction 3(left)
			flag=true;
		}else{
		    y-=7;x-=2;
		    for(j=x;j<x+5;j++)
			   for(i=y;i<y+5;i++)
				    if(m[j][i]==1)
					    flag=true;
				    else flag=false;
		}//else
		return flag;
	case 4:

        if((x-3)==0){//at the top border,and detect direction 4(up)
			flag=true;
		}else{
		    y-=2;x-=7;
		    for(j=x;j<x+5;j++)
			    for(i=y;i<y+5;i++)
				    if(m[j][i]==1)
					   flag=true;
				    else flag=false;
		}//else
		return flag;
	}//switch
}//Detect2
int Robot::Detect(int direction){
	//If there are obstacles on the direction indicated by
	//direction argument return true,return false otherwise.
		int x=CurrentPoint.x;
		int y=CurrentPoint.y;
	//Using "flatten" pointer which is excessively condition
	//dependent.
		int **m=_realmap.getMapPt();
		for(int i=x-2;i<=x+2;i++)
			for(int j=y-2;j<=y+2;j++)
		         if(m[i][j]==4) SeeMagnet=1;
		if(direction<1||direction>4) return 2;
		if(Detect2(direction)) return 1;
		else return 0;
	}
int Robot::DetectX(int direction){
	int x=CurrentPoint.x;int y=CurrentPoint.y;
	int step =0;bool btemp=true;
	int i,j;
	switch(direction){
	case 1:
		if((y+3)==(map_width2-1)) return 0;
		x-=2;y+=3;
		for( j=y;j<y+5;j++){
			for( i=x;i<x+5;i++){
                if((*(realmap+map_width2*i+j)==1)||(*(realmap+map_width2*i+j)==2))
					btemp=false;
			}
		    if(btemp)
				step++;
			else
				break;
		}//for
		break;
	case 2:
		if((x+3)==(map_height2-1)) return 0;
		x+=3;y-=2;
		for(i=x;i<x+5;i++){
			for(j=y;j<y+5;j++){
				if((*(realmap+map_width2*i+j)==1)||(*(realmap+map_width2*i+j)==2))
					btemp=false;
			}
			if(btemp)
				step++;
			else
				break;
		}//for
		break;
	case 3:
		if((y-3)==0) return 0;
		x-=2;y-=3;
		for(j=y;j>y-5;j--){
			for(i=x;i<x+5;i++){
				if((*(realmap+map_width2*i+j)==1)||(*(realmap+map_width2*i+j)==2))
					btemp=false;
			}
			if(btemp)
				step++;
			else
				break;
		}
		break;
	case 4:
		if((x-3)==0) return 0;
		x-=3;y-=2;
		for(i=x;i>x-5;i--){
			for(j=y;j<y+5;j++){
				if((*(realmap+map_width2*i+j)==1)||(*(realmap+map_width2*i+j)==2))
					btemp=false;
			}
			if(btemp)
				step++;
			else
				break;
		}//for
		break;
	}
	return step;
}
//////////////////PhyMove RealMove///////////////////////
int Robot::PhyMove(int MaxMovedStep){//timer driven
	
	int delta;int movetemp;
	int time=robot_timer.time();

	if(bPaused) {
		nLastXMovedTime=time;
		return 0;
	}

	int timefactor=time-nLastXMovedTime;
	delta=nSpeed*timefactor/1000;//1 second ,1000 millisecond
	if(delta||nSpeed==0) nLastXMovedTime=time;
	switch(CurrentDirection){
	case 1:
		movetemp=(delta>MaxMovedStep)?MaxMovedStep:delta;		
		ActualPosition.x+=movetemp;
		break;
	case 2:
		movetemp=(delta>MaxMovedStep)?MaxMovedStep:delta;		
		ActualPosition.y+=movetemp;
		break;
	case 3:
		movetemp=(delta>MaxMovedStep)?MaxMovedStep:delta;		
		ActualPosition.x-=movetemp;
		break;
	case 4:
		movetemp=(delta>MaxMovedStep)?MaxMovedStep:delta;		
		ActualPosition.y-=movetemp;
		break;
	}//switch
	return movetemp;
}
//////////////////////RealMove//////////////////////////////
bool Robot::SideMove(){
	//nSideMoveFlag_To and nSideMoveFlag_Fro should be setted
	//nSideMoveFlag_DirectionBacup must be set to CurrentDirection!!
	//before SideMove() is called.
    if(bPaused) return false;
	int moved=0;
	if(nSideMoveFlag_To!=0){
		TurnToDirection(nSideMoveFlag_Direction);
		moved=PhyMove(nSideMoveFlag_To);
		nSideMoveFlag_To-=moved;
		if(nSideMoveFlag_To==0) {
			nSideMoveFlag_bflag=true;
			TurnToOppositeDirectionOf(nSideMoveFlag_Direction);
		}
	}
	if(nSideMoveFlag_bflag&&(nSideMoveFlag_Fro!=0)){
		moved=PhyMove(nSideMoveFlag_Fro);
		nSideMoveFlag_Fro-=moved;
	}
	if((nSideMoveFlag_To==0)&&(nSideMoveFlag_Fro==0)){
		CurrentDirection=nSideMoveFlag_DirectionBackup;
		return true;
	}else return false;
}
//////////////////////////Constructor///////////////////
Robot::Robot(int x,int y,int pixelx,int pixely,LPDIRECTDRAWSURFACE src,int direction):CurrentDirection(direction),CurrentDirectionInEM(1),
                               robot_sprite(4,20,20),rotating_sprite(8,28,28),clean_floor(1,20,20){
	
	CurrentPoint.x=x;
	CurrentPoint.y=y;
	visited=0;SeeObstacleBefore=0;
	SeeMagnet=0;runflag=0;
	
	CurrentPointInEM.x=1;
	CurrentPointInEM.y=1;
	StartPointInEM.x=1;
	StartPointInEM.y=1;
	//Difference of Real Direction and Virtual Direction.
	DifferenceOfRDandVD=direction-1;
	nLastXMovedTime=0;
	nSpeed=50;//50 pixel per second : 1 pixel 20 millisecond
	bPaused=false;
	robot_timer.start();
	
    ActualPosition.x=pixelx;
	ActualPosition.y=pixely;
	surface=src;

    nSideMoveFlag_bflag=false;
	nSideMoveFlag_To=0;
	nSideMoveFlag_Fro=0;
	nSideMoveFlag_DirectionBackup=0;
	nSideMoveFlag_Direction=0;

	nMoveFlag_HasMoved20pixels=0;
	nMoveFlag_HasMoved4pixels=0;
	bMoveFlag_Move20PixelsFinished=false;
	bMoveFlag_Move4PixelsFinished=false;
	bMoveFlag_NeedDetectEnvi=true;
	bMoveFlag_NewStart=true;
	bMoveOutsideFlag_MoveFinished=false;
//	bMoveFlag_NeedDetectSide=true;
//	bSideMoving=false;
//	bNeedSideMoving=true;
//	nSideMovedNum=0;

	bMoveTo2Flag_start=false;
	nMoveTo2Flag_phase=1;
    nMoveTo2Flag_scheme=0;

	bMoveToFlag_start=false;
	nMoveToFlag_curr=0;
	bMoveToFlag_seeObstacle=false;
	bMoveToFlag_temp=false;
	bMoveToFlag_temp1=false;
	bMoveToFlag_rotating=false;
	nMoveToFlag_flag=0;
	bMoveToFlag_evaluated=false;
	Flag_reserved=false;

	bTFAFlag_to=true;
	bTFAFlag_fro=false;
	nTFAFlag_phase=1;
	nTFAFlag_j=1;

	for(int i=0;i<40;i++){
		for(int j=0;j<40;j++)
			map[i][j]=0;
	   }//for
	}//Robot
/////////////// TuroToDirection  TurnToOppositeDirecionOf ////////////////////
bool Robot::TurnToDirection(int direction){
	if(CurrentDirection==direction) return true;
    RotateC();
	if(CurrentDirection==direction) return true;
	RotateC();
	if(CurrentDirection==direction) return true;
	RotateC();
	if(CurrentDirection==direction) return true;
	RotateC();
	if(CurrentDirection==direction) return true;
}
bool Robot::TurnToOppositeDirectionOf(int direction){
	int OppDirection=direction+2;
	if(OppDirection>4) OppDirection=OppDirection%4;
    if(TurnToDirection(OppDirection))return true;
}
/*DisplayRobot LoadRobotImage RotatingAnimation*/
bool Robot::LoadRobotImage(cGraphicEngine &cg){
	cBmpFileReader tiles;
	tiles.load("picture\\tile.bmp");
try{ 
	robot_sprite.CreateSurface(cg);
	rotating_sprite.CreateSurface(cg);
	clean_floor.CreateSurface(cg);
    
	robot_sprite.load(tiles,3,0,24);
	robot_sprite.load(tiles,0,0,52);
	robot_sprite.load(tiles,1,0,80);
	robot_sprite.load(tiles,2,0,108);

	rotating_sprite.load(tiles,0,20,52);
	rotating_sprite.load(tiles,1,48,52);
	rotating_sprite.load(tiles,2,20,80);
	rotating_sprite.load(tiles,3,48,80);
	rotating_sprite.load(tiles,4,20,108);
	rotating_sprite.load(tiles,5,48,108);
	rotating_sprite.load(tiles,6,20,24);
	rotating_sprite.load(tiles,7,48,24);

	clean_floor.load(tiles,0,60,4);

	robot_sprite.SetColorkey(RGB(255,255,255));
	rotating_sprite.SetColorkey(RGB(255,255,255));
	clean_floor.SetColorkey(RGB(255,255,255));
}catch(cgError e){
	MessageBox(GetActiveWindow(),e.errmessage.c_str(),e.errmessage2.c_str(),MB_ICONERROR);
	return false;
}
   return true;	
}//LoadRobotImage

bool Robot::DisplayRobot(){
	
	try{
	   switch(CurrentDirection){
	    case 1:
//			clean_floor.draw(surface,0,ActualPosition.x,ActualPosition.y);
		    robot_sprite.draw(surface,0,ActualPosition.x,ActualPosition.y);
		    break;
 	    case 2:
//			clean_floor.draw(surface,0,ActualPosition.x,ActualPosition.y);
		    robot_sprite.draw(surface,1,ActualPosition.x,ActualPosition.y);
		    break;
	    case 3:
//			clean_floor.draw(surface,0,ActualPosition.x,ActualPosition.y);
		    robot_sprite.draw(surface,2,ActualPosition.x,ActualPosition.y);
		    break;
	    case 4:
//			clean_floor.draw(surface,0,ActualPosition.x,ActualPosition.y);
		    robot_sprite.draw(surface,3,ActualPosition.x,ActualPosition.y);
		    break;
	      }
	   }catch(cgError e){
           MessageBox(GetActiveWindow(),e.errmessage.c_str(),e.errmessage2.c_str(),MB_ICONERROR);
		   return false;
	   }
	return true;
}

bool Robot::RotatingAnimation(LPDIRECTDRAWSURFACE surface,bool clockwise){
    //called before actual rotating happens.
	if(clockwise){
		switch(CurrentDirection){
		case 1:
 //                   clean_floor.draw(surface,0,ActualPosition.x,ActualPosition.y);				
                    rotating_sprite.draw(surface,0,ActualPosition.x-4,ActualPosition.y-4);
					
					Sleep(200);
//					clean_floor.draw(surface,0,ActualPosition.x,ActualPosition.y);
					rotating_sprite.draw(surface,1,ActualPosition.x-4,ActualPosition.y-4);

					return true;
	
		case 2:
//				    clean_floor.draw(surface,0,ActualPosition.x,ActualPosition.y);
                    rotating_sprite.draw(surface,2,ActualPosition.x-4,ActualPosition.y-4);
			
					Sleep(200);
//					clean_floor.draw(surface,0,ActualPosition.x,ActualPosition.y);
					rotating_sprite.draw(surface,3,ActualPosition.x-4,ActualPosition.y-4);
					
					return true;
				
		case 3:
//				    clean_floor.draw(surface,0,ActualPosition.x,ActualPosition.y);
                    rotating_sprite.draw(surface,4,ActualPosition.x-4,ActualPosition.y-4);
					
					Sleep(200);
//					clean_floor.draw(surface,0,ActualPosition.x,ActualPosition.y);
					rotating_sprite.draw(surface,5,ActualPosition.x-4,ActualPosition.y-4);
					
					return true;
		
		case 4:    
//			       clean_floor.draw(surface,0,ActualPosition.x,ActualPosition.y);
				   rotating_sprite.draw(surface,6,ActualPosition.x-4,ActualPosition.y-4);
					
					Sleep(200);
//					clean_floor.draw(surface,0,ActualPosition.x,ActualPosition.y);
					rotating_sprite.draw(surface,7,ActualPosition.x-4,ActualPosition.y-4);
				
					return true;
	
		}//switch
     }else{
		switch(CurrentDirection){
		case 1:
  //            clean_floor.draw(surface,0,ActualPosition.x,ActualPosition.y);        
			  rotating_sprite.draw(surface,7,ActualPosition.x-4,ActualPosition.y-4);
		
					Sleep(200);
//					clean_floor.draw(surface,0,ActualPosition.x,ActualPosition.y);
					rotating_sprite.draw(surface,6,ActualPosition.x-4,ActualPosition.y-4);
					
					return true;
		case 2:     
//			        clean_floor.draw(surface,0,ActualPosition.x,ActualPosition.y);
                    rotating_sprite.draw(surface,1,ActualPosition.x-4,ActualPosition.y-4);
					
					Sleep(200);
//					clean_floor.draw(surface,0,ActualPosition.x,ActualPosition.y);
					rotating_sprite.draw(surface,0,ActualPosition.x-4,ActualPosition.y-4);
				
					return true;
		case 3:
  //                  clean_floor.draw(surface,0,ActualPosition.x,ActualPosition.y);
			        rotating_sprite.draw(surface,3,ActualPosition.x-4,ActualPosition.y-4);
					
					Sleep(200);
	//				clean_floor.draw(surface,0,ActualPosition.x,ActualPosition.y);
					rotating_sprite.draw(surface,2,ActualPosition.x-4,ActualPosition.y-4);
					
					return true;
		case 4:
      //              clean_floor.draw(surface,0,ActualPosition.x,ActualPosition.y);
			        rotating_sprite.draw(surface,5,ActualPosition.x-4,ActualPosition.y-4);
				
					Sleep(200);
		//			clean_floor.draw(surface,0,ActualPosition.x,ActualPosition.y);
					rotating_sprite.draw(surface,4,ActualPosition.x-4,ActualPosition.y-4);
					
					return true;
		}//switch
	 }//else
}//RotatingAnimation
//////////////////////////RotateC RotateAC//////////////////////////////////
bool Robot::RotateC(){
	//Rotate clockwise.
		if(CurrentDirection>4||CurrentDirection<1)
			return false;
 //       RotatingAnimation(surface,true);
		if(CurrentDirection<4) CurrentDirection++;
		else CurrentDirection=1;
		
		return true;
     }
bool Robot::RotateAC(){
	//Rotate anticlockwise.
		if(CurrentDirection>4||CurrentDirection<1)
			return false;
//		RotatingAnimation(surface,false);
		if(CurrentDirection>1) CurrentDirection--;
		else CurrentDirection=4;
		
		return true;
	}
bool Robot::LoadRealMap(const char *filename){
	_realmap.map_read(filename);
	return true;
}

bool Robot::PreMove()
{   int ActualX=1+5*(CurrentPointInEM.x-1)+2;
    int ActualY=1+5*(CurrentPointInEM.y-1)+2;
    int xdelta=CurrentPoint.x-ActualX;
	int ydelta=CurrentPoint.y-ActualY;
    CurrentPoint.x=ActualX;
	CurrentPoint.y=ActualY;
	ActualPosition.y=(CurrentPointInEM.x-1)*20+4;
	ActualPosition.x=(CurrentPointInEM.y-1)*20+4;
	
	return true;
}