There are so many problems, it's hard to make a suggestion. You are trying to do too many things together. You should learn how to do each thing first, by itself: GPS, compass, obstacle detection and driving. Then start combining them, one piece at a time.
Here's the list for your One Big Program:
* Don't use SoftwareSerial. You have a Mega, so hook the GPS to Serial1, on pins 18 & 19.
* Indent your code properly. Just press control-T in the IDE editor and it will auto-format it for you.
* Use good variable names. Instead of "ss" for the GPS serial port, call it "gpsPort". Since you need to use Serial1, you can do this:
#define gpsPort Serial1 // an "alias"
* Why are you calling getDistance all the time? You usually don't use the result for anything. Most of the code in loop happens very quickly, less than 1 millisecond. Just call it when you need to, in the "take action" section.
* Printing too much slows your program waaaaaaaay doooooooown.
* Break your loop into subroutines. It's too long to understand.
* Don't use delay in loop. It makes your program slow, and it will lose GPS characters.
* Find out how to use your compass. You are not using x,y,z to determine the current orientation. That orientation should be compared to the bearing to the waypoint. When they're different (subtracted), you'll know which way to drive.
* Don't use an int for heading angle. Radians can be less than 1, so it will truncate 0.35 radians to 0.
* Don't set the GPS update to 200ms (5Hz). Start with 1000ms (1 update per second) so you can see what's happening.
* Use NeoGPS, and you won't have to call myDelay and feedgps all over the place. It's more advanced, but it is smaller, faster and more accurate than all other libraries. It can do the correct distance and angle calculations.
I attached a version your sketch with all those suggestions. If you want to try it, NeoGPS is available from the Arduino IDE Library Manager, under the menu Sketch -> Include Library -> Manage Libraries. Even if you don't try it, be sure to read the Troubleshooting page for other tips for GPS sketches.
But I strongly suggest you work on the individual programs first, before trying to debug the OBP.
Cheers,
/dev
sustakas.ino (7.68 KB)