室内清扫机器人环境搭建与算法测试简易代码
【摘要】 室内清扫机器人简易代码，由于时间紧张，地图构建和智能规划算法还没有研究，
给出已经完成的部分内容，抛砖引玉吧。
MazeBuilder
//zhangrelay
AddiRobotCreate ir1 /Position:0 0.1 0
/Procedure_Bumper_SensorNotify:wall1
/
AddNewEntity en...
室内清扫机器人简易代码，由于时间紧张，地图构建和智能规划算法还没有研究，
给出已经完成的部分内容，抛砖引玉吧。
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文章来源: zhangrelay.blog.csdn.net，作者：zhangrelay，版权归原作者所有，如需转载，请联系作者。
原文链接：zhangrelay.blog.csdn.net/article/details/51353658
【版权声明】本文为华为云社区用户转载文章，如果您发现本社区中有涉嫌抄袭的内容，欢迎发送邮件进行举报，并提供相关证据，一经查实，本社区将立刻删除涉嫌侵权内容，举报邮箱：
cloudbbs@huaweicloud.com
- 点赞
- 收藏
- 关注作者
评论（0）