python - 使机器人指向正确的方向

我正在从带有无线电的基站指挥机器人。基站使用机器人（带有openCV）上的AR标签从高架摄像机获取位置/方向信息。此外，base会根据位置信息（A *，相机中的每个网格为30 x 30像素）计算机器人到达目标所应采用的路径。我的机器人只能在其中心点向左/向右转，然后向前/向后前进。机器人包括带有两个Lego 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)

我使用的辅助函数。我通过使用机器人上已知的两个点并在这两个点之间绘制一条线来计算机器人的方向。因此，线变得平行于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

我不知道我的代码在逻辑上是否有问题。如果可以，我该怎么办？如果代码正确，并且问题在于系统的延迟/设置，我还可以通过哪些其他方式控制机器人？

感谢您的帮助。