室内清扫机器人环境搭建与算法测试简易代码

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