HELP! How to guide my vehicle?

Hello everyone, thanks for support.

I'm trying to make a project for class about a really basic autonomous car with GPS (GY-NEO6MV2) and a Compass (HMC5883) that it goes to some waypoints at a map.

My problem is how to make the car oriented to the waypoints, here's a example of the map (really basic but hopefully you can get the idea):


I want to orient the car like: A--->B--->C--->D--->A and a loop.

The loop is not a problem for now, but how can I orient the car to get from point A to B, and from B to C and so on? With those materials?

It's a car, four wheels, back traction, and has two front wheels that makes rotate the whole car to both ways, right and left.

-Arduino Uno
-Compass (HMC5883)

Thanks in advance.

you would need to reference the preset compass bearings to the waypoints and turn the vehicle until its pointing in that direction. the rest you should be able to calculate using the GPS coordinates

Thank you for answer, but I can’t figure out how to code for orient the car toward waypoints.

I mean, for example:
//Number of Waypoints
int waypoints = {0,1,2,3};

/Actual Waypoint
int WaypointNow=0;

//Positions of Waypoints

//Waypoint A
float wALong=1,2345;
float wALati=1,2345;

//Waypoint B
float wBLong=1,2346;
float wBLati=1,2346;

//Here’s come the common code and then…


case 0:
if(GPS.Longitude <= wALong && GPS.Latitude <= wALati){

case 1:
if(GPS.Longitude <= wBLong && GPS.Latitude <= wBLati){


My problem is how to get the new waypoint heading. Orientate the car. For example, now I’m oriented the North but for go to the next waypoint that is at west, I need rotate the car to X degrees to get the angle and move forwards the vehicle to reach next waypoint.


You firstly need to calculate the position error (difference from here to next destination)
by subtracting vectors - in other words compute the differences in
latttitude and longitude, then use trig to convert that to a distance and bearing (this
conversion depends on your local lattitude).

Then the code needs to convert the bearing to magnetic bearing (magnetic
correction), and direct the drive system to steer the vehicle to that bearing
on the compass.

Periodically recalculate to update the course and monitor distance-to-target.

Sounds to me like you could use Canadian grade 11 math (Trigonometry). Look at it this way:

You have a waypoint. WayPOINT. A point. A point is the location on a grid, it's coordinates. You have the coordinates of the way point, and the coordinates of you. So all you have to do is make it into a right angle triangle (the hypotenuse being the line you will drive on to get to point B. The adjacent, the amount horizontally you will move, and the opposite - the amount vertically you will move (based off of the angle AngleToMove)). So refference Soh Cah Toa. Because we know that you will be going 1 unit up and one to the side, that is our adjacent and opposite. And since Toa deals with that we will use tan, because it is easier that use the length of the hypotenuse.

tan(AngleToMove) = 1/1

AngleToMove = tan^-1(1/1)

AngleToMove = 45° (In Degree mode, it would be 0.7853981634 in Radians)

And if we find the hypotenuse using the Pythagorean theorem 1^2 + 1^2 = c^2 ? 1.414213562, that is the distance to drive.

So, we used 2 coordinates, point A and point B. We found the angle from point A to B, 45°. You aligned the car to 45°. Then you drove 1.414213562 units, and you got to point B from A.

I hope this answers your question. The math will look different in the code, but as long as you understand what's going on it can't be hard. And if you do not, refresh yourself on math with tags: Trigonometry, Soh Cah Toa, Sine, Cos, Tan, Unit Circle, Pythagorean Theorem, and what not.

Besides if you install radar on to that car you need to know how to extrapolate the coordinates of something with the angle and hypotenuse alone haha. Math is fun.... Isn't it awesome that you can do that!

Also you could just calculate the spacial difference, in this example base on your example numbers the difference was one. So drive up one unit, then right one unit. But that will take longer and waste fuel.

Many many thanks to everybody! Answers are what I wanted since long time.

I need to wait for receive my materials and then work on it.

And yes... I need to refresh my maths... :cold_sweat: :blush: :slight_smile: