The aim of my work is given in the algorithm below.
Initialize the H/W; wait until GPS module has a valid fix and then start the motor.
Get actual position from the GPS.
Calculate the distance and heading information to the next waypoint.
Compare the actual heading with the current heading angle and feed the difference to the motors.
When the actual position is close to the first waypoint, switch to the next waypoint.
If/when the last waypoint is reached, stop the motor.
If not go to step 2.
I'm calculating the distance between a pre-defined coordinate and the current location using Haversine formula. I need heading angle for the same pair of coordinates. But when I use the atan2 formula and compile using the Arduino IDE, I am getting the angle as 0.05,0.03. Am I doing it wrong? can the heading angle be calculated for points within a proximity of 10m?
formula
double y = sin(dlon) * cos(lat2);
double x = ((cos(lat1) * sin(lat2)) - (sin(lat1) * cos(lat2) * cos(dlon)));
double brng = atan2(y, x);
Related
I need to be able to find x, y coordinates at any length down an Archimedean spiral arm, given a specific distance between each loop of the arm.
I have researched previous questions on Stackoverflow, and across the Internet, and I have found three methods, which are each different, and each plot me a spiral. (I call them the first, second and third method, here.)
The first method, does plot equidistant points, with the pointdist variable = 1, but as this is increased, there is also an aberration of point distances (supposed to be equidistant) near the center of the spiral.
The third method and second method, do not correctly plot equidistant points near the center of the spiral. (See graphs below)
The third method, though, allows me to input any length down the arm and obtain x, y coordinates.
The first and second methods plot equidistant points by a process where they additively sum a variable, each cycle of a loop to plot the equidistant points. Because of how this value is built up instead of calculated from scratch using only the distance along the spiral arm variable, I can't use these two methods to find coordinates at any arbitrary length along the arm [Proofreading this, I just thought, perhaps if I initialize the length to calculate one point each time. However, all three methods have problems even with equidistant points.]
Here is output from the third method:
Here is the code of the "third method". This method uses what an answer to another sprial-related question on Stackoverflow (Placing points equidistantly along an Archimedean spiral) calls the "Clackson scroll formula", which is said to be possibly inaccurate in some ranges.
double thetamax = 10 * Math.PI;
double b = armbandwidth / (2 * Math.PI);
// "armbandwidth” value influences distance between the spiral arms, usually kept between 1 and 20
AddPoint(0,0); // Mark the origin of the spiral
// “pointdist” is the length between points to plot along the spiral, I use 0.1 to 2+
// but it doesn't reveal spiral shape with increasing values
for (double s = pointdist; s < spirallength; s += pointdist)
{
double thetai = Math.Sqrt(2 * s / b);
double xx = b * thetai * Math.Cos(thetai);
double yy = b * thetai * Math.Sin(thetai);
AddPoint(xx, yy);
}
I need to both:
Use a method that does not have aberrations in the equidistance of points along the spiral arm, given equally spaced values of lengths down the spiral arms.
Use a method that allows me to specify the width between the spiral arms (in terms of the same units used for the length along spiral arm between the points, as well).
In case it helps, and to be clear about what I've tried, here are the code and output from the other methods I've found and tested (here called "second method" and "first method") for calculating the coordinates of equidistant points along an Archimedean spiral:
Here is output from the second method (note the uneven distanced points near center):
Here is the code for the second method:
AddPoint(0,0); // Mark the origin of the spiral
double arclength = 0.8; // This value (kept between 0.1 and 20 or so) sets the distance between points to calculate coordinates for along the spiral curve
double r = arclength;
double b = armbandwidth / (2 * Math.PI); // "armbandwidth" value influences distance between the spiral arms, usually kept between 3.5 to 10
double phi = r / b;
double xx = r * Math.Cos(phi);
double yy = r * Math.Sin(phi);
AddPoint(xx, yy);
while( r <= spirallength ) // spirallength determines roughly how many iterations of points to draw
{
phi += arclength / r;
r = b * phi;
xx = r * Math.Cos(phi);
yy = r * Math.Sin(phi);
AddPoint(xx, yy);
}
Because the variable phi is additively increased each iteration, I can't pass in any length down the spiral arm to find coordinates for. (Maybe if I initialized the whole method only to a single arclength each time. - In any case, the points near center are not evenly spaced.)
Here is output from the first method (Equidistant points throughout with pointdist = 1):
Here is the code of the first method:
double separation = 4; // Value influences distance between the spiral arms, usually kept 3.5 to 10+
double angle = 0;
double r;
AddPoint(0,0); // Mark the origin of the spiral
for (double i=0; i <= spirallength; i+=pointdist) // spirallength determines pointdist spaced points to plot
{
r = Math.Sqrt(i+1);
angle += Math.Asin(1/r);
double xx = Math.Cos(angle) * r*separation;
double yy = Math.Sin(angle) * r*separation;
AddPoint(xx, yy);
}
However, when "pointdist" is increased above 1, there are aberrations in equidistance between points near the center of the spiral, even by this method. Here is the output of a graph using the "first method" and pointdist = 9:
Can anyone help me calculate x, y coordinates for any length down the spiral arm from center, for an Archimedes spiral defined by a specified width between loops of the arm?
(It should be able to have equidistant points, accurate even near the center, and be able to take a width between loops of the arm in units the same scale as those used for the distance to a point along the arm passed to the coordinates equation.)
Much appreciated!
I believe this last piece of code is the best (most simple and straightforward) approach:
constant angle variation
For a given angle
calculate the radius
convert Polar coordinates to Cartesian.
However, I understand the Archimedean spiral is defined by the formula: r = a + b * angle (where a=0 to simplify, and b controls the distance between loops.
In short, the position of particle is proportional to the angle θ as time elapses.
So what's up with that? That's not linear at all!
r = Math.Sqrt(i+1);
angle += Math.Asin(1/r);
Here's a simple way to make it.
It's not a running code. But I believe it's very easy to understand.
So just understand the logic and then code it with your own variables:
for (some incrementing t)
{
radius = t/100; //Start at radius = 0. When t = 100, radius=1.
angle = t*2*Pi/200; //It takes 200 points to make a full 360 (2Pi) turn
//Now converting from polar coordinates (r,angle) to (x,y):
x = cos(angle) * r;
y = sin(angle) * r;
AddPoint(x, y);
}
This code generates the following image:
This generates the right trajectory. However, as noticed, this does not produce equidistant points, because each angle increment is multiplied by the radius. So I suggest you to find a correction function and apply it to t. Like so:
function correction(i)
{
// I actually don't know the exact relationship between t and i
t = 1/i
// But I think it converges to t=1/i for very small increments
return t
}
for (some incrementing i)
{
t = correction(i)
...
}
I want an object to smoothly stop at a specified point. I have the initial velocity, V0, of the object, the end velocity (which is set to 0), V1, and the distance, d, required to reach the destination. V1 and V0 are measured in radians per frame, and d is also measured in radians.
I've tried using the following formula:
a = (V0*V0 - V1*V1) / (2.0 * d);
But it always seems to overshoot its target.
Edit:
Essentially, I have a wheel that starts spinning at an initial velocity of V0. The wheel has to do a certain number of spins before stopping at a specified location. The distance, d is the amount of radians required to do the specified amount of spins and stop at a specified location. The velocity is number of radians per frame.
We have enough information now for an educated guess.
The formula is correct, but only in the ideal case, where the length of an iteration time is very small. My guess is that in your loop you are updating position before velocity, so that for that time period the wheel can advance with undiminished velocity, and you overshoot. (If you updated velocity before position, you would undershoot.)
You can either make your frames shorter, which will make the overshoot less severe, or you can modify the formula to eliminate it:
a = (V0*V0) / (2.0 * d - V0*tdelta);
where tdelta is the length of time of a single frame. (I've assumed V1=0.)
I've written iPhone - Mac, Client - Server app that allows to use mouse via touchpad.
Now on every packet sent I move cursor by pecific amount of pixels (now 10px).
It isn't accurate. When i change sensitivity to 1px it's to slow.
I am wondering how to enhance usability and simulate mouse acceleration.
Any ideas?
I suggest the following procedure:
ON THE IPHONE:
Determine the distance moved in x and y direction, let's name this dx and dy.
Calculate the total distance this corresponds to: dr = sqrt(dx^2+dy^2).
Determine how much time has passed, and calculate the speed of the movement: v = dr/dt.
Perform some non-linear transform on the velocity, e.g.: v_new = a * v + b * v^2 (start with a=1 and b=0 for no acceleration, and then experiment for optimal values)
Calculate a new distance: dr_new = v_new * dt.
Calculate new distances in x/y direction:
dx_new = dx * dr_new / dr and dy_new = dy * dr_new / dr.
Send dx_new and dy_new to the Mac.
ON THE MAC:
Move the mouse by dx_new and dy_new pixels in x/y direction.
NOTE: This might jitter a lot, you can try averaging the velocity after step (3) with the previous two or three measured velocities if it jitters to much.
If you understand objective c very well, then just read the last 2 sentences. The rest of this just summarizes the last 2 sentances:
So I have two sprites, the lower arm and the upper arm. I set the anchor points to ccp(0.5f,0.0f) So lets say that the following dashes represent the lower arm, the anchorpoint is the dash in parenthesis: (-)------ . So the object is rotating around this point (the CGPoint at the moment is ccp(100,55)).
What I need is, if the lower arm is rotating around the dash in parenthesis: (-)-----o the circle represents the point I want. I'm basically connecting the two arms and trying to make the movement look nice... Both arms are 17 pixels long (which means that if the lower arm is pointing straight up, the CGPoint of the circle is ccp(100,72), and if the arm was pointing straight down, the circle would be ccp(100,38).
What equation would I use so that I could set the position of the upper arm equal to the position of the lower arm's rotating CGPoint, represented as a circle in the 2nd paragraph of this question. Like... _,/ the _ represents the lower arm, the comma represents the point I want, and the / represent the upper arm.
So lower and upper arm = 17 pixels long, anchor point for both is (0.5f,0.0f), how do I find the point opposite of the anchor point for the lower arm.
x = 100 + 17 * cos(θ)
y = 55 + 17 * sin(θ)
You need to find what the angle of rotation is. I'm not that familiar with objective c, but if you're using a rotation function there's most likely an angle component somewhere you can reference.
From there you can use trigonometry to find the components of your x and y change.
For x it will be: (anchor x) + (length of arm) * cosine(angle of rotation)
And for y it will be: (anchor y) + (length of arm) * sine(angle of rotation)
Also, make sure you know whether the angle is in radians or degrees, you might have to convert based on the sine/cosine functions.
Given a starting point, a heading, a distance, and a line segment, find the first point along this heading that is the specified distance away from this line segment.
I covered two cases, but I haven't been able to cover the last one.
First case: heading away from the line. Ignore it even if the starting point is within the specified distance.
Second case: It intersects the line. I solved it using trig and triangles. Initially didn't consider the next case.
Third case: It is heading towards the line, but it does not intersect it. I think this will solve the second case as well if it's done correctly.
Three subcases:
The minimum line distance is greater than the specified distance. Ignore it.
The minimum line distance is equal to the specified distance. Found the points already.
The minimum line distance is less than the specified distance. This means there is a perpendicular line from the along the heading to an endpoint of the line segment that is less than the distance needed. This also means that on either side of this perpendicular line will be two lines of the distance needed. One is perpendicular to the heading, while the other is closest to the same endpoint and not perpendicular to the heading. Just a matter of finding those points and seeing which one is closer to the start point.
This is where I am stuck today. Drawing it up was easy, but doing the vector calc or whatever turned out tricky.
It's possible to rephrase this as:
At what time(s) is P(t) = P0 + t*v at a distance D from the line segment L((x1,y1), (x2,y2))?
v=(sin(heading), -cos(heading)) in my case.
Shoot mang your solution doesn't always work. I found a counter example:
Line Segment = (0,0) -> (0,14)
Start Point = (19, 6) # heading -159.5 or 200.5 in west/counter-clockwise
It will intersect the line at (2.952, 0.0) so I ask, where does it it come within a distance of 0.0.
The result I get is incorrect.
http://img5.imageshack.us/i/failuref.png/
How I can tell which ones will work using your solution and which ones do not work depends whether the minimum starting distance between the point and the line segment creates a perpendicular line.
If I can post another picture in the next post, I will put the successful example.
I would have liked to post some code for Sage which produced those images, but the code tags are accepting python unfortunately.
A successful result where the minimum starting distance between the point and the line segment is perpendicular to the line segment:
http://img46.imageshack.us/i/success.png/
Hi the solution I eventually came up with.
Does the ray intersect line segments that are parallel and the specified distance D away from the line segment. Just drawing a rectangle and checking the sides parallel to the line segment.
Does the ray intersect circles of radius D at each end point of the line segment.
Minimize for total unit time to find the first point along the ray that is D away from the line segment.
Possible Border case: Is the start point within D and heads away from the line? Up to the user how to handle this case.
Thanks, that works.
I found the alpha this way:
heading = 45.0*pi/180. #heading 45 degrees.
if x1 > x2: #line segment (x1,y1)<->(x2,y2)
dx = x2 - x1
dy = y2 - y1
else:
dx = x1 - x2
dy = y1 - y2
segmentHeading = atan2(dx, dy)
if heading > 0:
alpha = segmentHeading + heading
else:
alpha = -segmentHeading + heading
t = abs( (dStart - D) / -cos(alpha) ) #-cos in python, sin in C.