# Determining Which Way To Turn Revisted

This topic is 3888 days old which is more than the 365 day threshold we allow for new replies. Please post a new topic.

## Recommended Posts

Hi All, I'm going to have to explain the problem I'm having through example. Think that will be easiest. Up to not I have an A* alg with determines the path correctly. Then some vector post processing with works out the angles needed to turn and which direction to follow said path. My problem is working out which direction to turn on the first angle (The actual angle is alway correct). In theory, using the X product works fine, however I'm using these commands to control a real robot, so it may under turn or over turn a bit. The things I know are: 1) The path I need to follow. 2) The robots current XY position and angle. Assume these are the map cells it needs to follow: (0,0) Starting at 0 Degrees (1,1) Turn Right 45 degrees (2,2) Forward (3,3) Forward (4,4) Finish. If at (1,1) the robot has only turned 39 degrees I'm correctly working out that on the next command it will need to turn 6 degrees, similarly if its turned 51 degrees etc. Like I say it's finding the which way it's supposed to turn is my problem. X Prod doesn't work Z component doesn't always work until the robot is quite a way off course. Any one have any suggestions. This is driving me nuts Many Thanks Mark

##### Share on other sites
I admit I don't quite understand the problem as described, but if the problem is simply how to determine which direction to turn given a target position (in 2-d), the solution is straightforward:
vector2 f = robot.current_forward_direction;vector2 t = target_position - robot.current_position;float angle = atan2(perp_dot(f,t), dot(f,t));
The result of this calculation is (more or less) an angle in the range [-pi, pi] that tells you how much (and in which direction) to turn relative to your current heading.

##### Share on other sites
Sorry yes, I've been finding it hard to explain.

I've tried that. Unfortunately still same problem. I still get no difference to allow me to decide on which direction to take. Here are two examples I've just run:

1)
Starting Pos (0,0)
Turn 45 degrees.
New Pos (2,2) Angle 37 degrees
Next Command
Turn 8 Degrees
New Pos (2,2) Angle 45 degrees
OK

2)
Starting Pos (0,0)
Turn 45 degrees.
New Pos (2,2) Angle 51 degrees
Turn 6 Degrees
New Pos (2,2) Angle 57 degrees
ERROR

This is the code I'm using:

    RobotVector.X = lRobotsCell->X;    RobotVector.Y = lRobotsCell->Y;    // Work out the target vector - lPathCell is the next cell calculated by the A* alg   TargetVector.X = (lPathCell->X - lRobotsCell->X);   TargetVector.Y = (lPathCell->Y - lRobotsCell->Y);   // Find the length of the vector Length = SquareRoot(x*x + y*y)   lTargetVectorMagnatude= sqrt(static_cast<double>((TargetVector.X * TargetVector.X) + (TargetVector.Y * TargetVector.Y)));    // This is calculating the correction angle. The position of the robot is a unit vector,    // i.e. where the length is 1 (We don't have another point from which to determine the    // robots vector.    // Need to be measured anti-clockwise    double lRobotOrientationRads = Utilities::Deg2Rad(lRobotsCell->Orientation);    lPathCell->pParentCell->NavAngle = (TargetVector.X*cos(lRobotOrientationRads)) + (TargetVector.Y*sin(lRobotOrientationRads));    lPathCell->pParentCell->NavAngle = lPathCell->pParentCell->NavAngle/lTargetVectorMagnatude;    lPathCell->pParentCell->NavAngle = Utilities::Rad2Deg(acos(lPathCell->pParentCell->NavAngle));    lPathCell->Orientation = lRobotsCell->Orientation - static_cast<int>(lPathCell->pParentCell->NavAngle);    if ( lPathCell->Orientation >= 360 )    {        lPathCell->Orientation = lPathCell->Orientation - 360;    }    else if ( lPathCell->Orientation <= -360 )    {        lPathCell->Orientation = lPathCell->Orientation + 360;    }    // Depending on the sign of the determine which way to turn    // This is the problem bit...    CrossProductVector.Z = (RobotVector.X * TargetVector.Y ) - (RobotVector.Y * TargetVector.X);    lDotProduct = ((RobotVector.X * TargetVector.X) + (RobotVector.Y * TargetVector.Y));    double latanResult = atan2(CrossProductVector.Z, lDotProduct);    latanResult = Utilities::Rad2Deg(latanResult);    if (latanResult < 0)    {        lPathCell->pParentCell->NavAngle= -lPathCell->pParentCell->NavAngle;    }

Not sure if that makes things any clearer? I thought it should work with just the x prod, doesn't :(

Thanks

Mark

[Edited by - markgame66 on March 16, 2008 5:07:16 PM]

##### Share on other sites
I'd be happy to look at your code, but could you edit your post and place the code in [source] tags? That'll make it easier to read.

##### Share on other sites
Think this looks ok ;)

    RobotVector.X = lRobotsCell->X;    RobotVector.Y = lRobotsCell->Y;    // Work out the target vector - lPathCell is the next cell calculated by the A* alg   TargetVector.X = (lPathCell->X - lRobotsCell->X);   TargetVector.Y = (lPathCell->Y - lRobotsCell->Y);   // Find the length of the vector Length = SquareRoot(x*x + y*y)   lTargetVectorMagnatude= sqrt(static_cast<double>((TargetVector.X * TargetVector.X) + (TargetVector.Y * TargetVector.Y)));    // This is calculating the correction angle. The position of the robot is a unit vector,    // i.e. where the length is 1 (We don't have another point from which to determine the    // robots vector.    // Need to be measured anti-clockwise    double lRobotOrientationRads = Utilities::Deg2Rad(lRobotsCell->Orientation);    lPathCell->pParentCell->NavAngle = (TargetVector.X*cos(lRobotOrientationRads)) + (TargetVector.Y*sin(lRobotOrientationRads));    lPathCell->pParentCell->NavAngle = lPathCell->pParentCell->NavAngle/lTargetVectorMagnatude;    lPathCell->pParentCell->NavAngle = Utilities::Rad2Deg(acos(lPathCell->pParentCell->NavAngle));    lPathCell->Orientation = lRobotsCell->Orientation - static_cast<int>(lPathCell->pParentCell->NavAngle);    if ( lPathCell->Orientation >= 360 )    {        lPathCell->Orientation = lPathCell->Orientation - 360;    }    else if ( lPathCell->Orientation <= -360 )    {        lPathCell->Orientation = lPathCell->Orientation + 360;    }    // Depending on the sign of the determine which way to turn    // This is the problem bit...    CrossProductVector.Z = (RobotVector.X * TargetVector.Y ) - (RobotVector.Y * TargetVector.X);    lDotProduct = ((RobotVector.X * TargetVector.X) + (RobotVector.Y * TargetVector.Y));    double latanResult = atan2(CrossProductVector.Z, lDotProduct);    latanResult = Utilities::Rad2Deg(latanResult);    if (latanResult < 0)    {        lPathCell->pParentCell->NavAngle= -lPathCell->pParentCell->NavAngle;    }

##### Share on other sites
Should anyone stumble across this - I've figured out the solution.

I realiesed as I know the size of the bot, the direction and an initial point I can work out another point along the direction of travel. Once I have that I can get a vector for the bot. Then, as usual, use the Z component of the X prod to determine direction. YAY it works!

Mark

1. 1
Rutin
33
2. 2
3. 3
4. 4
5. 5

• 13
• 9
• 9
• 9
• 9
• ### Forum Statistics

• Total Topics
633330
• Total Posts
3011388
• ### Who's Online (See full list)

There are no registered users currently online

×