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

原创

zhangrelay 2022-04-02 16:08:35 博主文章分类：机器人仿真 ©著作权

©著作权归作者所有：来自51CTO博客作者zhangrelay的原创作品，请联系作者获取转载授权，否则将追究法律责任

室内清扫机器人简易代码，由于时间紧张，地图构建和智能规划算法还没有研究，

给出已经完成的部分内容，抛砖引玉吧。

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

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

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

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