Warm tip: This article is reproduced from stackoverflow.com, please click
arduino opencv path-finding python robotics

Getting Robot to Point to Correct Direction

发布于 2020-05-08 02:21:24

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.

Questioner
Ege Elgun
Viewed
30
Ege Elgun 2020-02-24 03:05

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.