室内清扫机器人环境搭建与算法测试简易代码_扫地机器人模型matlab源代码

MazeBuilder  
//zhangrelay
AddiRobotCreate    ir1  	/Position:0  0.1  0
	/Procedure_Bumper_SensorNotify:wall1
/
AddNewEntity    enthwf  	/Position:0  0.1  -0.1
	/Orientation:0    0    0
	/ParentEntity:ir1
AddBoxShape  
	/Dimensions:0.001  0.001  0.001  	/Mass:0.01
AddInfraredRangeEntity    hwf
	/Position:0  0.1  -0.1
	/ParentEntity:ir1	

AddNewEntity    enthwl  	/Position:-0.1  0.1  0
	/Orientation:0    90    0
	/ParentEntity:ir1
AddBoxShape  
	/Dimensions:0.001  0.001  0.001  	/Mass:0.01
AddInfraredRangeEntity    hwl
	/ParentEntity:enthwl	

AddNewEntity    enthwr  	/Position:0.1  0.1  0
	/Orientation:0    270    0
	/ParentEntity:ir1
AddBoxShape  
	/Dimensions:0.001  0.001  0.001  	/Mass:0.01
AddInfraredRangeEntity    hwr
	/ParentEntity:enthwr	
/
//AddInfraredRangeEntity    hwl
//	/Position:-0.3  0.2  0.0
//	/ParentEntity:ir1

//AddInfraredRangeEntity    hwr
//	/Position:0.3  0.2  0.0
//	/ParentEntity:ir1

FlushScript
double iswall=0
double hwfd=0
double hwld=0
double hwrd=0
double SimTime=10000
double turnf=0
double turnfl=0
wait 40000

call proc_main  with concur

Procedure  proc_main

	call proc_robot_0

End

Procedure  wall1
//	   print value.Pressed. ToString()
	   iswall=4
End

Procedure  proc_robot_0

	for (i = 0; i < SimTime; i++)
	{
		hwfd=hwf.Get()
		hwrd=hwr.Get()
		hwld=hwl.Get()
//		print "f" + hwfd. ToString()
//		print "l" + hwld. ToString()
//		print "r" + hwrd. ToString()
		if(hwfd>0.4)
		{
			iswall=0
		}
		if(hwfd<0.2)
		{
			if(turnfl==0)
			{
				turnf=1
			}
			if(turnfl==1)
			{
				turnf=2
			}
			if(turnfl==2)
			{
				turnf=1
			}
			if(hwld<0.2)
			{
				turnf=3
			}
			if(hwrd<0.2)
			{
				turnf=4
			}
			if(turnfl==3)
			{
				if(turnf==3)
				{
					turnf=34
				}
			}
			if(turnfl==4)
			{
				if(turnf==4)
				{
					turnf=44
				}
			}
		}
		if(iswall<1)
		{
			ir1.Go(0.4)
//			ir1.Go(0)
			if(turnf==1)
			{
				ir1.Go(0)
				ir1.RotateDegrees(90, 0.4)
				hwfd=hwf.Get()
				if(hwfd>0.4)
				{
					ir1.GoTo(0.2, 0.4)
				}
				else
				{
					ir1.GoTo((hwfd-0.14), 0.2)
				}
				ir1.RotateDegrees(90, 0.4)
				turnfl=turnf
				turnf=0
			}
			if(turnf==2)
			{
				ir1.Go(0)
				ir1.RotateDegrees(-90, 0.4)
				hwfd=hwf.Get()
				if(hwfd>0.4)
				{
					ir1.GoTo(0.2, 0.4)
				}
				else
				{
					ir1.GoTo((hwfd-0.14), 0.2)
				}
				ir1.RotateDegrees(-90, 0.4)
				turnfl=turnf
				turnf=0
			}
			if(turnf==3)
			{
				ir1.Go(0)
				ir1.RotateDegrees(-90, 0.4)
				turnfl=turnf
				turnf=0
			}		
			if(turnf==34)
			{
				ir1.Go(0)
				ir1.RotateDegrees(-90, 0.4)
				ir1.GoTo(0.2, 0.4)
				ir1.RotateDegrees(-90, 0.4)
				turnfl=2
				turnf=0
			}	
			if(turnf==4)
			{
				ir1.Go(0)
				ir1.RotateDegrees(90, 0.4)
				turnfl=turnf
				turnf=0
			}		
			if(turnf==44)
			{
				ir1.Go(0)
				ir1.RotateDegrees(90, 0.4)
				ir1.GoTo(0.2, 0.4)
				ir1.RotateDegrees(90, 0.4)
				turnfl=1
				turnf=0
			}	
		}
		else
		{
			ir1.Go(0)
		}
		wait 50
	}

End

StartSimulationEngine  /FileName:"ch001.xml"
// zhangrelay
AddiRobotCreate    ir1  	/Position:7  0.04  12
	/Orientation:0    270    0
	/Procedure_Bumper_SensorNotify:wall1
/
AddNewEntity    enthwf  	/Position:0  0.1  -0.1
	/Orientation:0    0    0
	/ParentEntity:ir1
AddBoxShape  
	/Dimensions:0.001  0.001  0.001  	/Mass:0.01
AddInfraredRangeEntity    hwf
	/Position:0  0.1  -0.1
	/ParentEntity:ir1	

AddNewEntity    enthwl  	/Position:-0.1  0.1  0
	/Orientation:0    90    0
	/ParentEntity:ir1
AddBoxShape  
	/Dimensions:0.001  0.001  0.001  	/Mass:0.01
AddInfraredRangeEntity    hwl
	/ParentEntity:enthwl	

AddNewEntity    enthwr  	/Position:0.1  0.1  0
	/Orientation:0    270    0
	/ParentEntity:ir1
AddBoxShape  
	/Dimensions:0.001  0.001  0.001  	/Mass:0.01
AddInfraredRangeEntity    hwr
	/ParentEntity:enthwr	
/
//AddInfraredRangeEntity    hwl
//	/Position:-0.3  0.2  0.0
//	/ParentEntity:ir1

//AddInfraredRangeEntity    hwr
//	/Position:0.3  0.2  0.0
//	/ParentEntity:ir1

FlushScript
double iswall=0
double hwfd=0
double hwld=0
double hwrd=0
double SimTime=10000
double turnf=0
double turnfl=0
wait 40000

call proc_main  with concur

Procedure  proc_main

	call proc_robot_0

End

Procedure  wall1
//	   print value.Pressed. ToString()
	   iswall=4
End

Procedure  proc_robot_0

	for (i = 0; i < SimTime; i++)
	{
		hwfd=hwf.Get()
		hwrd=hwr.Get()
		hwld=hwl.Get()
		print "f" + hwfd. ToString()
		print "l" + hwld. ToString()
		print "r" + hwrd. ToString()
		if(hwfd>0.4)
		{
			iswall=0
		}
		if(hwfd<0.2)
		{
			if(turnfl==0)
			{
				turnf=1
			}
			if(turnfl==1)
			{
				turnf=2
			}
			if(turnfl==2)
			{
				turnf=1
			}
			if(hwld<0.2)
			{
				turnf=3
			}
			if(hwrd<0.2)
			{
				turnf=4
			}
			if(turnfl==3)
			{
				if(turnf==3)
				{
					turnf=34
				}
			}
			if(turnfl==4)
			{
				if(turnf==4)
				{
					turnf=44
				}
			}
		}
		if(iswall<1)
		{
			ir1.Go(0.4)
//			ir1.Go(0)
			if(turnf==1)
			{
				ir1.Go(0)
				ir1.RotateDegrees(90, 0.4)
				hwfd=hwf.Get()
				if(hwfd>0.4)
				{
					ir1.GoTo(0.2, 0.4)
				}
				else
				{
					ir1.GoTo((hwfd-0.14), 0.2)
				}
				ir1.RotateDegrees(90, 0.4)
				turnfl=turnf
				turnf=0
			}
			if(turnf==2)
			{
				ir1.Go(0)
				ir1.RotateDegrees(-90, 0.4)
				hwfd=hwf.Get()
				if(hwfd>0.4)
				{
					ir1.GoTo(0.2, 0.4)
				}
				else
				{
					ir1.GoTo((hwfd-0.14), 0.2)
				}
				ir1.RotateDegrees(-90, 0.4)
				turnfl=turnf
				turnf=0
			}
			if(turnf==3)
			{
				ir1.Go(0)
				ir1.RotateDegrees(-90, 0.4)
				turnfl=turnf
				turnf=0
			}		
			if(turnf==34)
			{
				ir1.Go(0)
				ir1.RotateDegrees(-90, 0.4)
				ir1.GoTo(0.2, 0.4)
				ir1.RotateDegrees(-90, 0.4)
				turnfl=2
				turnf=0
			}	
			if(turnf==4)
			{
				ir1.Go(0)
				ir1.RotateDegrees(90, 0.4)
				turnfl=turnf
				turnf=0
			}		
			if(turnf==44)
			{
				ir1.Go(0)
				ir1.RotateDegrees(90, 0.4)
				ir1.GoTo(0.2, 0.4)
				ir1.RotateDegrees(90, 0.4)
				turnfl=1
				turnf=0
			}	
		}
		else
		{
			ir1.Go(0)
		}
		wait 50
	}

End