I am commanding a robot from a base station with radio. Base station takes location/orientation information from an overhead camera using the AR tag on the robot (with openCV). Moreover, base calculates the path robot should take to reach the target from location information (A* with each grid being 30 by 30 pixels in camera). My robot can only turn left/right (on its central point) and go forward/backward. Robot includes an Arduino Uno with two Lego NXT motors.
I use the following code to get robot to point at right direction. However, when the robot gets close to the angle that it is supposed to travel to, instead of stopping an going forward it tries to fix its orientation infinitely.
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)
My helper function that are used. I calculate orientation of robot by using two points known on the robot and drawing a line in between those two point. Hence, line becomes parallel with th orientation.
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
I can't figure out if my code is problematic in logic. If so what can I do about it? If code is fine and the problem is the delay/setup of the system, what are the other ways I can control the robot?
Thank you for the help.
Solved the problem by changing the image library for AR tag recognition. We were using 2 tags for one robot. It is significantly slower and fail prone to detect two tags. Updated it to only have one tag. Moreover, switched from angle based calculations to vector based which is way simpler to understand.