Hello everyone,
I'd like to calculate the bearing angle and the distance between heading point and target point with three coordinates for an arduino navigation.
I have coordinates as shown in the below attached image.
Where (x1, y1) is the heading point, (x3, y3) is the target point.
Thanks in advance.
This site has a wealth of useful formulas and information about navigation.
This Angle Library may be useful
...R
What's wrong with this logic, robot only makes left turn
xc = radians(x0); // target heading point x
yc = radians(y0); // target heading point y
xt = radians(xt); // target heading point x
yt = radians(yt); // target heading point x
// Heading angle
heading = atan2(sin(yt-yc)*cos(xt),cos(xc)*sin(xt)-sin(xc)*cos(xt)cos(yt-yc)),23.1415926535;
// Convert from radians to degrees
heading = heading*180/3.1415926535;
if (heading > 180) {
heading -= 360;
}
if (heading < -180) {
heading += 360;
}
if ( heading < -2 ) {
// turn left
digitalWrite(Motor1,LOW);
digitalWrite(Motor2, LOW);
analogWrite(PWM1, 145);
analogWrite(PWM2, 145);
}
if ( heading > 2 ) {
// turn right
digitalWrite(Motor1,HIGH);
digitalWrite(Motor2, HIGH);
analogWrite(PWM1, 145);
analogWrite(PWM2, 145);
}
else {
// go straight
digitalWrite(Motor1,LOW);
digitalWrite(Motor2, HIGH);
analogWrite(PWM1, 180);
analogWrite(PWM2, 180);
}
These statements are not executing
if (heading > 180) {
heading -= 360;
}
if (heading < -180) {
heading += 360;
}
Please read the sticky post, "How to use this forum," posted near the top of the subject listings for nearly every section of the forum. In particular, please read the part about using code tags. After that, please edit your post to put your code inside code tags.
rosdino:
What's wrong with this logic
I can't tell. It looks like the first thing that it does is try to convert some x and y coordinates to radians. If that's what it's doing, it's unlikely to work, since coordinates are in units of distance, and radians describe angles.
I think that (xt, yt) are the coordinates of the target point, and (xc, yc) are the coordinates of the robot's current position. Can you confirm that?
I think that the intent is ultimately to move the robot to the target point, (xt, yt). If that's so, the code will need some way of updating the robots position. I don't see that update; maybe it happens outside the snippet. Can you confirm that the sketch is intended to move the robot to (xt, yt)? Can you tell us how the robot's position is updated?
This line baffles me:
heading = atan2(sin(yt-yc)*cos(xt),cos(xc)*sin(xt)-sin(xc)*cos(xt)*cos(yt-yc)),2*3.1415926535;
as far as I can tell, this will return the result of the atan2 calculation, and discard the rest. What do you expect from "2*3.1415926535" at the end of the line? I'm not going to try to address the math, until we know whether xt, yt, xc, and yc are angles, or distances.
We'd typically expect that you would describe what you're doing, and describe esoteric formulae that you use, rather than ask us to divine your intent from a partial listing.
These statements are not executing
How can you tell?
Hi, sorry for my stupid post.
I tried to use the logic given here.
http://forum.arduino.cc/index.php?topic=128233.msg964757#msg964757
I am trying to head robot towards the target point.
I can't tell. It looks like the first thing that it does is try to convert some x and y coordinates to radians. If that's what it's doing, it's unlikely to work, since coordinates are in units of distance, and radians describe angles.
Yes you are right, I think I am doing it wrong
I think that the intent is ultimately to move the robot to the target point, (xt, yt). If that's so, the code will need some way of updating the robots position. I don't see that update; maybe it happens outside the snippet. Can you confirm that the sketch is intended to move the robot to (xt, yt)? Can you tell us how the robot's position is updated?
Yes the final aim is to move robot towards the target point, but right now I am planning to head towards the target point.
as far as I can tell, this will return the result of the atan2 calculation, and discard the rest. What do you expect from "2*3.1415926535" at the end of the line? I'm not going to try to address the math, until we know whether xt, yt, xc, and yc are angles, or distances.
xt, yt, xc, and yc are distances.
I have three coordinates in distance as shown in the image(first post) so I am looking for a formula for bearing and distance between current point and target point.
I used formulas given here, but they are for lat and long coordinates
http://www.movable-type.co.uk/scripts/latlong.html
My robot coordinates are distances.
My robot coordinates are distances.
Coordinates are not distances. You need coordinates (X and Y values).
From a starting point (X1,Y1), a bearing angle and a distance, you can calculate a target point (X2,Y2). See the movable-type link "Destination point given distance and bearing from start point".
My coordinates are Cartesian.
Do I need to convert them to polar coordinates?
Sorry I'm getting confused here
jremington:
Coordinates are not distances. You need coordinates (X and Y values).From a starting point (X1,Y1), a bearing angle and a distance, you can calculate a target point (X2,Y2). See the movable-type link "Destination point given distance and bearing from start point".
I've both current points and target points but as coordinates.
I've mounted 3 led's on robot, and track these points using IR camera located at the top of the arena.
If you have Cartesian coordinates then the calculations are much simpler. It is just plane trigonometry.
To calculate a new position from a bearing angle theta (measured from the X axis) and distance:
X2 = X1 + distancecos(theta); //theta in radians
Y2 = Y1 + distancesin(theta);
What do you really want to do?
jremington:
If you have Cartesian coordinates then the calculations are much simpler. It is just plane trigonometry.
To calculate a new position from a bearing angle theta (measured from the X axis) and distance:X2 = X1 + distancecos(theta); //theta in radians
Y2 = Y1 + distancesin(theta);What do you really want to do?
Thanks dude.
So using above equation, have to find theta which is bearing angle?
my aim is to navigate robot in certain points.
Sorry, it still isn't clear what you want to do.
jremington:
Sorry, it still isn't clear what you want to do.
I want to drive robot to different waypoints and come back to original position.
something like this.
start(x1, y1)=>(x2, y2)=>(x3, y3)=>(x1, y1) stop
From (x1,y1) drive at angle theta12 until you reach (x2,y2).
theta12 = atan2(y2-y1, x2-x1); //in radians
Repeat for each leg, making appropriate choices for (x1, y1) and (x2, y2).