1

I'm trying to get a robot to turn to face another robot, based on their respective coordinates on a global coordinate map.

This is the code I wrote, but it doesn't seem to work at all:

def calcAngleToCoords(self, curAngle, curPosition, targPosition):
    retVal = False

    if type(curPosition) is list and type(targPosition) is list:
        x_1, y_1 = curPosition
        x_2, y_2 = targPosition
        # Sets origin coordinate to zero
        x_2 = x_2 - x_1
        y_2 = y_2 - y_1

        radius = math.sqrt(y_2 ** 2 + x_2 ** 2) # Pythagorean Thereom, a^2 + b^2 = c^2 | Radius = c, y_2 = a, x_2 = b
        angle = curAngle * (math.pi / 180)

        x_1 = radius * math.cos(angle)
        y_1 = radius * math.sin(angle)

        turnArc = math.atan( (y_1 - y_2) / (x_2 - x_1) ) * (180 / math.pi)

        retVal = turnArc
        # TODO: Check to see if the angle is always clockwise.
    else:
        raise TypeError("Invalid parameter types. Requires two lists.")

    return(retVal)

Can any one tell me a better way to do this or what I'm doing wrong? It's for a project I'm working on and the deadline is coming up really soon so any help would be appreciated!

1
  • How do you know it doesn't work?
    – Peter Wood
    Jun 1, 2015 at 15:59

1 Answer 1

0

There is no need to calculate radius

    x_2 = x_2 - x_1
    y_2 = y_2 - y_1
    angle = curAngle * (math.pi / 180)
    dx = math.cos(angle)
    dy = math.sin(angle)
    turnArc = math.atan2(x_2 * dy - y_2 * dx,  x_2 * dx + y_2 * dy ) * (180 / math.pi)

Note using of atan2 function, that returns angle between -pi and pi. It correctly determines rotation direction and finds the shortest turn. If you need to rotate in clockwise direction only, add 180 if turnArc is negative

1
  • Thanks, but I'm somewhat confused. You said there is no need to calculate radius but then you included radius in your code?
    – RJGordon
    Jun 1, 2015 at 17:46

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.