Python探索Raspberry Pi机器人平台_robotip griper python-CSDN博客

本文链接：https://blog.csdn.net/jiyotin/article/details/109173185

随机轨迹

第一代机器人吸尘器在一个无限循环中使用了一种非常简单的算法：

直行直到撞到障碍物
转一个随机角度

如果您担心这种行为的清洁质量，那可能是对的。但是从数学角度来看，如果给定无限的时间，只要机器人可以物理上到达，该算法将覆盖整个清洁区域。

随机驱动程序（下面Python程序）基本上由一个while循环组成，该循环一直运行到按下END键（KEY4）为止。

from eye import *
 from random import *

 safe=300
 LCDMenu("","","","END")

 while(KEYRead() != KEY4):
    OSWait(100)
    if(PSDGet(PSD_FRONT)>safe and PSDGet(PSD_LEFT)>safe and PSDGet(PSD_RIGHT)r>safe):
       VWStraight(100,200)
    else:
       VWStraight(-25,50)
       VWWait()
       dir=int(180*(random()-0.5))
       VWTurn(dir,45)
       VWWait()

我们在每次循环迭代中等待100毫秒（0.1秒）以减少计算开销。一条if选择，则在继续前进之前检查所有三个侧面是否有足够的空间（300毫米）。如果不是，则机器人后退一小段距离（25毫米），然后旋转随机角度。函数random（）产生一个介于0和1之间的数字，因此项180 *（random（）-0.5）产生一个介于–90和+90之间的值，该值定义了我们随机回合的可能范围。在下一个循环迭代中，如果机器人沿新方向有足够的空间，它将再次直行。

下面Python程序使用相同算法的扩展版本。它将PSD传感器的数值打印到显示器上，这要求将它们存储在变量f，l和r中。每当旋转时，它还会打印一条有关机器人动作的消息，并不断读取并显示摄像机图像。

from eye import *
from random import *

safe=300
LCDMenu("","","","END")
CAMInit(QVGA)

while(KEYRead() != KEY4):
 OSWait(100)
 img = CAMGet()
 LCDImage(img)
 f=PSDGet(PSD_FRONT)
 l=PSDGet(PSD_LEFT)
 r=PSDGet(PSD_RIGHT)
 LCDSetPrintf(18,0,"PSD L%3d",l,f,r)
 
 if(l>safe and f>safe and r>safe):
  VWStraight(100,200)
 else:
  VWStraight(-25,50)
  VWWait()
  dir = int(180*(random()-0.5))
  LCDSetPrintf(19,0,"Turn %d",dir)
  VWTurn(dir,45)
  VWWait()
  LCDSetPrintf(19,0," ")

下图中的屏幕截图显示了机器人经过几条直腿后的驾驶。我们将机器人足球运动场用作此任务的背景。

该程序的C版本与Python实现非常相似。它只是使用不同的语法。驱动结果完全相同–参见下面C程序。相机被初始化为QVGA，并且其图像与PSD读数一起显示在前，左和右。需要按KEY4（“ END”软键）以终止程序。

#include "eyebot.h"

#define SAFE 300

int main() {
  BYTE img[QVGA_SIZE];
  int dir, l, f, r;

  LCDMenu("", "", "", "END");
  CAMInit(QVGA);

  while (KEYRead() != KEY4) {
    CAMGet(img); // demo
    LCDImage(img); // only
    l = PSDGet(PSD_LEFT);
    f = PSDGet(PSD_FRONT);
    r = PSDGet(PSD_RIGHT);
    LCDSetPrintf(18, 0, "PSD L%3d F%3d R%3d", l, f, r);
    if (l > SAFE && f > SAFE && r > SAFE)
      VWStraight(100, 200); // start driving 100mm dist.
    else {
      VWStraight(-25, 50);
      VWWait(); // back up
      dir = 180 * ((float) rand() / RAND_MAX - 0.5);
      LCDSetPrintf(19, 0, "Turn %d", dir);
      VWTurn(dir, 45);
      VWWait(); // turn [-90, +90]
      LCDSetPrintf(19, 0, " ");
    }
    OSWait(100);
  } // while
  return 0;
}

由于机器人会因各种障碍而停下来，因此遇到另一个机器人时也会停下来。因此，我们现在可以安全地让多个机器人在同一编程环境中运行。为此，我们只需要为每个机械手添加一条额外的行到SIM脚本中即可。在下面程序的脚本中，我们将启动三个不同的机器人-两个LabBots和一个SoccerBot S4。此示例中的所有机械手都具有相同的可执行程序，但是您可以通过更改可执行文件名来轻松指定不同的程序。

# Environment
 world $HOME/worlds/small/Soccer1998.wld

 settings VIS TRACE

 # robotname x y phi
 LabBot 400 400 0 randomdrive.py
 S4 700 700 45 randomdrive.py
 LabBot 1000 1000 90 randomdrive.py