室内清扫机器人环境搭建与算法测试简易代码
室内清扫机器人环境搭建与算法测试简易代码
zhangrelay
发布于 2019-01-23 10:14:51
发布于 2019-01-23 10:14:51
室内清扫机器人简易代码，由于时间紧张，地图构建和智能规划算法还没有研究，
给出已经完成的部分内容，抛砖引玉吧。
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本文参与 腾讯云自媒体分享计划，分享自作者个人站点/博客。
原始发表：2016年05月09日，如有侵权请联系 cloudcommunity@tencent.com 删除
评论
登录后参与评论
推荐阅读