PROSAGA码农传奇-乐高机器人-使机器人指向正确的方向

我正在从带有无线电的基站指挥机器人。基站使用机器人上的AR标签（带有openCV）从高架摄像机获取位置/方向信息。而且，base会根据位置信息（A *，相机中的每个网格为30 x 30像素）计算机器人到达目标所应采用的路径。我的机器人只能在其中心点向左/向右转，然后向前/向后前进。机器人包括带有两个Lego NXT电机的Arduino Uno。我使用以下代码使机器人指向正确的方向。但是，当机器人接近它应该行进的角度时，它不会停止前进，而是试图无限地固定其方向。
<pre><code> 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)
</code></pre>我使用的辅助函数。我通过使用机器人上已知的两个点并在这两个点之间绘制一条线来计算机器人的方向。因此，线变得平行于th方向。
<pre><code> 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
</code></pre>我不知道我的代码在逻辑上是否有问题。如果可以，我该怎么办？如果代码正确，并且问题在于系统的延迟/设置，我还可以通过哪些其他方式控制机器人？感谢您的帮助。