I'm just not entirely sure of the break down and how to fully integrate that into my code!
initialize everything
forever {
read sensors
calculate whether to turn left, right, or straight
output motor control
if (am I at current goal?) {
set next goal as the goal
if (no more goal) {
break;
}
}
}
Turn of motors
Play victory melody
Your "read sensors" function probably needs to deal with asynchronous results. For example, if your GPS only emits one sample per second, you can't afford to wait for it. Instead, update the "current GPS position" field only if there's a new reading waiting in the input buffer, else grab what you can and continue (keeping the data you've gotten so far until next time.)