Python 让机器人指向正确的方向

我正在用无线电从基站指挥一个机器人。基站使用机器人上的AR标签（带openCV）从头顶摄像机获取位置/方位信息。此外，base根据位置信息计算机器人到达目标所需的路径（A*，相机中的每个网格为30×30像素）。我的机器人只能向左/向右（在其中心点）并向前/向后移动。该机器人包括一个带有两个乐高NXT马达的Arduino Uno

我使用下面的代码让机器人指向正确的方向。然而，当机器人接近它应该行进的角度时，它不会停止前进，而是试图无限地固定它的方向

    def correctOrientation(self, rx: int, ry: int):
        #returns direction robot needs to point.
        direction = self.getDirection((self.sx, self.sy), (rx, ry))
        #method to stop robot.
        self.comms.stop()

        anglediff = (self.angle - direction + 180 + 360) % 360 - 180

        while not (abs(anglediff) < 15):
            #Decides which way to turn.
            if self.isTurnLeft(self.angle, direction):
                self.comms.turnLeft()
            else:
                self.comms.turnRight()
            #Put sleeps because there is a delay in camera feed. Allows it to get the location right
            time.sleep(0.3)
            self.comms.stop()
            #Updates position
            self.getPos()
            time.sleep(1)
            #Calculates orientation of robot and updates it
            self.angle = self.calcOrientation()
            anglediff = (self.angle - direction + 180 + 360) % 360 - 180
            print(anglediff)
            time.sleep(1)

def校正方向（self，rx:int，ry:int）：
#返回机器人需要指向的方向。
方向=self.getDirection（（self.sx，self.sy），（rx，ry））
#方法停止机器人。
self.comms.stop（）
角度差=（自身角度-方向+180+360）%360-180
而不是（abs（角度差）<15）：
#决定转向哪个方向。
如果self.isturnlight（self.angle，direction）：
自动通信左转（）
其他：
自动通信右转（）
#由于摄影机馈送延迟，因此放置睡眠。允许它获得正确的位置
睡眠时间（0.3）
self.comms.stop（）
#更新位置
self.getPos（）
时间。睡眠（1）
#计算机器人的方向并更新它
self.angle=self.calcOrientation（）
角度差=（自身角度-方向+180+360）%360-180
打印（anglediff）
时间。睡眠（1）

我使用的助手函数。我使用机器人上已知的两点并在两点之间画一条线来计算机器人的方向。因此，直线与th方向平行

    def isTurnLeft(self, angle, touchAngle):
        diff = touchAngle - angle
        if diff < 0:
            diff += 360
        if diff < 180:
           return False
        else:
            return True

    def calcOrientation(self) -> float:
        return self.getDirection(self.marker[0], self.marker[3])

    def getDirection(self, source: Tuple[int], target: Tuple[int]) -> float :
        return (math.degrees(math.atan2(target[1] - source[1], target[0] - source[0]))+360)%360

def isTurnLeft（自身、角度、接触角度）：
diff=接触角-角度
如果差异<0：
差值+=360
如果差值<180：
返回错误
其他：
返回真值
def calcOrientation（自）->浮动：
返回self.getDirection（self.marker[0]，self.marker[3]）
def getDirection（self，source:Tuple[int]，target:Tuple[int]）->float:
返回（数学度（数学atan2（目标[1]-源[1]，目标[0]-源[0]））+360）%360

我不知道我的代码在逻辑上是否有问题。如果是，我能做些什么？如果代码没有问题，问题是系统的延迟/设置，我还可以用什么方法控制机器人

---

感谢您的帮助。

通过更改AR标签识别的图像库解决了此问题。我们为一个机器人使用了两个标签。检测两个标记的速度明显较慢且容易失败。将其更新为只有一个标记。此外，从基于角度的计算切换到基于向量的计算，这更容易理解。

仅凭这段代码很难准确地说出。但我的猜测是，如果电机已经左转，就让它运行，而不是用

self.comms.turnlight（）

反复设置它。

如果self.isturnlight（self.angle，direction）：self.comms.turnlight（）

对我来说是多余的，或者解释一下你希望它做什么？它是否来回移动以尝试修复它的位置？如果是这样的话，那么可能是超过了15度的温差。试着缩短睡眠时间。更好的方法是：使旋转时间依赖于角度差。角度差越大，旋转时间越长。另外，您是否记录了任何值？一个简单的

打印
角度值将有助于您了解发生了什么，您可以继续。我已经记录了一些值。看来我的角度计算有点偏差。然而，由于我转动的方式，要让机器人朝正确的方向转动需要很多时间。正如你所说，我已经使时间取决于角度差，这使它变得更好。编辑：我还可以为此尝试哪些其他选项？船上方向规可靠吗？