All cleaned up :)

As I mentioned I would do in the last post, I got Raven's wiring all cleaned up. It actually looks more like spagetti in the pictures than what it was before, but it is much tidier and easy to trace and see in real life.

 I mounted the lipo semi-permenately (or at least much more than it was before) to the underside of the plate, sandwhiched between the GPS antenna wire and the roll cage support on the truck. In addition there is some other new stuff on the underside of the brain plate since last update besides the wiring. I added my HC-05 module to the breadboard and wired it up to serial2 on the mega (dang its nice having more than one serial port), haven't detailed the wiring yet when I had taken this picture, but it will be soon. I also added a AA battery for the GPS, it uses it to save the settings, as well as some data about the last fix to make getting a fix faster when it powers back up. I simply soldered some hookup wire to both ends of the battery and taped it to the front there. Also new is the power switch, from a model airplane for the radio reciever. It makes a nice easy switch to interface as it has predone 0.1" standard servo headers and a nice mounting solution. And lastly, I mounted my reset switch nice and accessably. Here is a closer up of the electronics:

To the right is all power and reset circuit wiring, middle is the steering servo connector, and on the right side, GPS, Compass, Bluetooth module and status LED wiring. I currently have the status LED wired to the Mega, with a little rutine in the code that looks for GPS validity and lights the LED if the data is valid, and does not if it is not. I originally had that hooked up to the GPS's fix output but I was having some real problems with the GPS's indicator, sometimes it worked sometimes it didn't. So now I ignore it and watch the light instead.
This is what the top of the brain plate looks like now:

A little cleaner with no LiPo awkwardly strapped to the top and wires coming out everywhere. Here is a closer shot of the front where all the interesting stuff is:

Note power switch on the left, status LED in the middle (back of the compass tower) and reset switch on the right all mounted nicely.

Ive been wondering and plotting how to get some telemitry data back from Raven while he is out on a run for troubleshooting and just interst's sake, but I was stumped for a little while. I don't even own an LCD screen and have little desire for one, my nRF24L01 modules are still wayyy too confusing, one day I will sit down and learn how to use them but not today, and unfortunately my SD card shield does not have assignable pins, so it will only work on an UNO (I have since ordered a microSD reader module from ebay for integration into Raven at some point just to log the lat and long and have a datafile I can plot on google earth) But then I remembered I have a HC-05. Perfect! coupled with either my laptop plus a bluetooth dongle or one of my phones plus an android terminal emulator, I have super easy telemitry data being sent to a nice screen :)

BAM! Its beautiful. Now I can see what he is thinking while he is out running about.

Alright, that's it for hardware at the moment, but I have also conqoured some software. Last post I mentioned that I needed to make the steering code better, and also get the GPS spitting out at 5Hz (I can make it spit out at up to 10Hz, but Adafruit says it only gets position updates every 5Hz anyway) Well those two turned out to be a little interconnected. First I actually did something totally unrelated to those. I was looking at ways to clean my code up a bit and I realized that I had the whole loop inside the if(serial1.avalible) statement, effectively making it so that it would only update the steering angle every time the GPS spat out a sentance, about once per second. So I remidied that and everything started functioning much better. Much much better. Amazing how things work out well when you actually program right. Next up was the rediscovering of the "Mini GPS" tool. Pretty much its something that connects to the GPS module via serial and lets you change a bunch of settings. And its linked in the Adafruit manual. Well I feel stupid :P Anyway, got it up and running, changed my GPS to spit out at 5Hz, only GPGLL (previously I had GPRMC which tells you time, lattitude, longitude, heading, speed, and date. I may use those additional ones at a later date, but for now the only ones I am using are the lat and long. GPGLL gives you only lat, long, and time.) and at 38400 baud. And coupled with that, was the battery backup. Because without the battery everytime power is disconnected to the GPS, it looses its configuration settings and returns to defualt. Awesome! GPS is spitting out what I want, my code is running smooth, and so I moved onto getting the proportional steering. Map() is amazing, I love it :) Here is the result:

Ahh, so much smoother and nicer. Aside from those changes, I did a little optimization, moved the servo pin and LED pin up so as to keep the interrupt pins free, changed to bluetooth debugging, and made some GPGLL related changes. Here is the code Raven is running at the moment (also note the rudimentary distance to waypoint estimation code. Needs work but kinda functions.):

#include <Servo.h> //Steering servo setup
Servo steer;

#include "Wire.h"  //Crap for the compass
#include "I2Cdev.h"
#include "HMC5883L.h"
HMC5883L mag;
int16_t mx, my, mz;

float latdest = 49.0819104;  // Initial destionation waypoint coordiantes.
float londest = -117.5801223;
//=== Variables used===
float heading = 0;  // HMC5883L heading
float tim = 0;  // GPS time
float lat = 0;  // GPS lattituede
float lon = 0;  // GPS longitude
int latd = 0;  // GPS lattitude degrees
float latm = 0;  // GPS lattitude decimal minutes
int lond = 0;  // GPS longitude degrees
float lonm = 0;  // GPS longitude decimal minutes
float deltay = 0;  // Difference in degrees between next waypoint and current location, Y direction
float deltax = 0;  // Difference in degrees between next waypoint and current location, X direction
float deltaym = 0;  // Difference in meters between next waypoint and current location, Y direction
float deltaxm = 0;  // Difference in meters between next waypoint and current location, X direction
double dist = 0;  // Distance in meters between next waypoint and current location
float hdgrad = 0;  // Computed required heading in radians
float hdgdeg = 0;  // Computed required heading in degrees
int err = 0;  // Computed difference between current heading and required heading
int sv = 1500;  // Servo steering value in microseconds

void setup()
{
  Wire.begin();   // More compass crap
  mag.initialize();
  steer.attach(5,1000,2000);  // Steering servo crap
  pinMode(4, OUTPUT); // status LED
  steer.writeMicroseconds(1500); 
  Serial2.begin(9600);  // Debugging over Bluetooth using an HC-05
  Serial1.begin(38400);  // Ultimate GPS is connected to serial 1. I used the tool "Mini GPS" to set the output to 5Hz, GPGLL only, 38400 Baud for this program.
}

void loop()
{
  while(latd != 49)  // This little chunck of code just forces the program into a loop here until valid GPS data is obtained. (Calculated using the lattitude I live at)
  {                  // A feedback LED is used to tell if the GPS has a fix at a glance. The lattitude is also spit out over the serial port when the GPS does not have a fix.
    if (Serial1.available() > 49)
    {
      Serial1.find("$GPGLL");
      lat = Serial1.parseFloat();
      latd= lat/100;
    }
    //Failsafe code if the GPS looses fix can be inserted here
    digitalWrite(4,LOW);
    Serial2.println(lat, DEC);
    delay(10);
  }
  digitalWrite(4,HIGH);
 
  mag.getHeading(&mx, &my, &mz);  // Reading the compass.
    heading = atan2(my, mx);
    if(heading < 0)
      heading += 2 * M_PI;
      heading = heading * 180/M_PI;
 
  if (Serial1.available() > 49)  // Parsing the GPS data
  {
    Serial1.find("$GPGLL");
    lat = Serial1.parseFloat();
    lon = Serial1.parseFloat();
    tim = Serial1.parseFloat();
    latd= lat/100;  // Converting the Degrees, decimal minutes given by the GPS into decimal degrees.
    latm = lat - latd*100;
    latm = latm/60;
    lat = latd + latm;   
    lond= lon/100;
    lonm = lon - lond*100;
    lonm = lonm/60;
    lon = lond + lonm;
    lon = 0 - lon;
  }
  Serial2.print(lat, DEC);  // Debugging the lat and long
  Serial2.print("  ");
  Serial2.print(lon, DEC);
  Serial2.print("  ");
 
  deltay = latdest - lat;  // Determining the change in degrees between next waypoint and current location
  deltax = londest - lon;
 
  deltaym = deltay*111211.71;  // Calculating the distance to next waypoint and debugging. (Calculated based on the lattitude I live on)(needs improvment)
  deltaym = abs(deltaym);
  deltaxm = deltax*73019.76;
  dist = sqrt(sq(deltaym)+sq(deltaxm));
  Serial2.print(dist, DEC);
  Serial2.print("  ");
 
  hdgrad = atan2(deltax,deltay);  // Calculating heading required to travel to next waypoint.
  hdgdeg = hdgrad*180/3.14;
  if (hdgdeg < 0)
    hdgdeg = hdgdeg + 360;
 
  err = heading - hdgdeg;  // Calculating error between current heading and required heading and debugging.
  err = ((((err) % 360) + 540) % 360) - 180;
  Serial2.print(err, DEC);
  Serial2.print("  ");

  err = err/2;  // Driving the steering servo baised on the err value. Reasonably proportional.
  sv = map(err,-90,90,1040,1960);
  Serial2.print(sv, DEC);
  Serial2.print("  ");
  steer.writeMicroseconds(sv);
  
  Serial2.println();  // Houskeeping. Making sure the serial data is formatted nicely for the phone screen, and the delay to keep the code from running too fast.
  delay(5);
}

Well, no nice body shot today cause really nothing has changed on the outside, but perhaps that will change soon. Its late November in Canada, and there is a couple inches of snow on the ground now, so I doubt Ill be running many test runs outside until spring, electronics+snow=not great. Which means that perhaps soon Ill be mounting some sonars on the top and gettting the obstical avoidance code running. Which also means giving raven control of his own throttle... Warning, rogue robot ahead. I may also mount my MPU6060 and see about using the acceleromiter and gyro for tilt compensation and smoothing of the compass data. Gotta keep looking ahead :) Thats all for now tho folks.

Raven Progress

I have accomplished a fair amount on Raven these last few days. First off, I wired everything up, at the moment a little messily (thats next on the list, clean up the wiring) but functional.

The battery awkwardly zip tied to the top of the brain plate is a 3 cell lipo temporarily there just to power the control electronics and nothing else, even the steering servo is powered by the truck's batteries. I also added a yet to be mounted switch connected to the arduino's reset circut to make reseting it a little easier. As mentioned I will be cleaning up the wiring as I am not happy with it's current state, but thats a little later. In anticipation of having even more wiring added and being uncertain about the breadboard's ability to hold everything together I also ordered a prototyping shield from ebay which has not yet arrived.

I (gasp!) also worked on the software and actually got him up and running. This is a video of my very first test run. The space around my house is severly limited and coupled with the GPS's 30 foot accuracy, well it didn't go stellar. But thats OK! I include the footage because not everything always goes right.

With that success/failure/whatever_at_least_its_driving behind me I input the coordinates of the very center of a park close to me, curtisy of google maps:

And went for a drive over there. Here he is all ready to go:

The code that I am running (which will be attached at the end of the post) is, well frankly, very poorly coded and ultra rudimentary, as I have said before I am a terrible programmer (and not like those people who post and say that they are bad programmers, but their code is great, no, I am a genuinely terrible programmer. But you only get better by doing :)) But it runs and functions and I understand it all which is important. I am not using any proportionality on the steering, just a couple if commands to see if the error between the current heading and the required heading is greater than ten degrees (or in the last video 20) out in either direction and if so turning the wheels full left or right accordingly. At the moment I also have not given Raven control of his throttle either, to avoid a rogue robot running around out of hand:

Yeah, not a good thing :) anyway, for now I just use the transmitter to control the throttle while Raven does the steering. Here is a video showing raven's steering response to being turned around:

And here is a rather long video of him driving around trying to get to the middle of the park from various places around the perimiter. Of course some quirks, but on the whole he seemed to get there pretty well, considering the code he is running. The general area of the orbits seemed to be right in the middle of the park like google maps suggested. The video immediately below this one is the same run but shot from the gopro mounted on the front of Raven (It fell off near the end) Both videos are long, but if you watch the first little, you can see some low speed navigation, and if you go more to the end, I start running it at a higher speed.

This is a much shorter video that I shot after those two were taken and I changed the cut off values to 20 degrees away from the goal instead of 10. As you can see it performs much more poorly, due to that change but also because I was running this test at mostly higher speed. It was fun and interesting to see the change.

This is the code currently running on the Mega. If you would like to use it, in part or whole, feel free.

#include <Servo.h> //Steering servo setup
Servo steer;

#include "Wire.h"  //Crap for the compass
#include "I2Cdev.h"
#include "HMC5883L.h"
HMC5883L mag;
int16_t mx, my, mz;

float latdest = 49.1037056;  // Initial destionation waypoint coordiantes.
float londest = -117.5573271;

float heading = 0;  // HMC5883L heading
float tim = 0;  // GPS time
float lat = 0;  // GPS lattituede
float lon = 0;  // GPS longitude
float spd = 0;  // GPS speed
float hdg = 0;  // GPS heading
int latd = 0;  // GPS lattitude degrees
float latm = 0;  // GPS lattitude decimal minutes
int lond = 0;  // GPS longitude degrees
float lonm = 0;  // GPS longitude decimal minutes
float deltay = 0;  // Difference in degrees between next waypoint and current location, Y direction
float deltax = 0;  // Difference in degrees between next waypoint and current location, X direction
float deltaym = 0;  // Difference in meters between next waypoint and current location, Y direction
float deltaxm = 0;  // Difference in meters between next waypoint and current location, X direction
double dist = 0;  // Distance in meters between next waypoint and current location
float hdgrad = 0;  // Computed required heading in radians
float hdgdeg = 0;  // Computed required heading in degrees
int err = 0;  // Computed difference between current heading and required heading
int sv = 1500;  // Servo steering value in microseconds

void setup()
{
  Wire.begin();   //More compass crap
  mag.initialize();
  steer.attach(3,1000,2000);  //Steering servo crap
  steer.writeMicroseconds(1500); 
  Serial.begin(115200);  // Debugging over USB
  Serial1.begin(9600);  // Ultimate GPS is connected to serial 1
}

void loop()
{
  mag.getHeading(&mx, &my, &mz);  // Reading the compass.
    heading = atan2(my, mx);
    if(heading < 0)
      heading += 2 * M_PI;
      heading = heading * 180/M_PI;
 
  if (Serial1.available() > 62)  // Parsing and debugging the GPS data
  {
    Serial1.find("$GPRMC");
    tim = Serial1.parseFloat();
    lat = Serial1.parseFloat();
    lon = Serial1.parseFloat();
    spd = Serial1.parseFloat();
    hdg = Serial1.parseFloat();
    /*Serial.print("Time: ");
    Serial.print(tim, DEC);
    Serial.print(" Lat: ");
    Serial.print(lat, DEC);
    Serial.print(" Long: ");
    Serial.print(lon, DEC);
    Serial.print(" Speed: ");
    Serial.print(spd, DEC);
    Serial.print(" Heading: ");
    Serial.println(hdg, DEC);*/
   
    latd= lat/100;  // Converting the Degrees, decimal minutes given by the GPS into decimal degrees and also debugging over the serial port.
    latm = lat - latd*100;
    latm = latm/60;
    lat = latd + latm;   
    lond= lon/100;
    lonm = lon - lond*100;
    lonm = lonm/60;
    lon = lond + lonm;
    lon = 0 - lon;
    Serial.print(lat, DEC);
    Serial.print("  ");
    Serial.print(lon, DEC);
    Serial.print("  ");
   
    deltay = latdest - lat;  // Determining the change in degrees between next waypoint and current location
    deltax = londest - lon;
   
    deltaym = deltay*111211.71;  // Calculating the distance to next waypoint and debugging. (Calculated based on the lattitude I live on)(needs improvment)
    deltaym = abs(deltaym);
    deltaxm = deltax*73019.76;
    dist = sqrt(sq(deltaym)+sq(deltaxm));
    Serial.print(dist, DEC);
    Serial.print("  ");
   
    hdgrad = atan2(deltax,deltay);  // Calculating heading required to travel to next waypoint and debugging it.
    hdgdeg = hdgrad*180/3.14;
    if (hdgdeg < 0)
      hdgdeg = hdgdeg + 360;
    Serial.print(hdgdeg, DEC);
    Serial.print("  ");
    Serial.print(heading, DEC);
    Serial.print("\t");
   
    err = heading - hdgdeg;  // Calculating error between current heading and required heading and debugging.
    err = ((((err) % 360) + 540) % 360) - 180;
    Serial.print(err, DEC);
    Serial.print("  ");
  
    if(err > 10)
    {
      Serial.print("Left  ");
      steer.writeMicroseconds(1960);
    }
    else if(err < -10)
    {
      Serial.print("right  ");
      steer.writeMicroseconds(1040);
    }
    else
    {
      Serial.print("center  ");
      steer.writeMicroseconds(1500);
    }
    Serial.println();

    /*sv = map(err,-180,180,1040,1960);
    Serial.print(sv, DEC);
    Serial.print("  ");
    steer.writeMicroseconds(sv);*/
  }
  delay(10);
}

And thats the progress to date, next on my list of things to do is clean up the wiring, make it all tidy and not the rats nest that it is now, plus mount the reset switch. Thats for hardware, on the software side, there is a couple things. I need to make the steering code a little better. I will probably start with just a greater block of steering commands that set the steering less extreme when the error angle is less extreme, and then perhaps get some proportionality going using the map() function. Another big thing that needs to happen is I need to find a way to get my GPS to start spitting out at 5Hz. I don't really want to use Adafruit's library unless absoulutely necessary, but I need to figure that out. Nav side, I will get a distance calculating equation working that determines how far from the waypoint Raven is and trigger it to start driving towards the next waypoint when he reaches a certain distance away. I will worry about sensing and avoiding objects and tilt compensating the compass along with filtering the GPS data better at a later date. But for now not bad! He is driving towards waypoints by himself! Thats further than I have ever gotten with GPS guided vehicles before!

Raven

An update of yesterday's post, I have gotten some more work done on my GPS guided vehicle, which I have also decided to name Raven. I got those mounting holes drilled up for the hardware plate that I mentioned yesterday, and also mounted some electronics to it as well. That was a little interesting, I don't have any circuit board standoffs so I had to get creative. As is usually the case, I ended up building the required parts from modified lego, in this case some small 1x1 stud round "light" pieces. with a hole drilled thru the center. Here are two modified pieces next two unmodified ones:

And them on the screws and Mega:

In addition to the Mega, I also screwed down my ultimate GPS (with more lego spacers) and zip tied on a breadboard for connections. I would have screwed the breadboard on too but the mounting holes were in an inconvinent place. I also mounted my external GPS antenna and it's excessively long connector to the mounting plate:

A closeup of the Mega and the lego standoffs. They actually work very very well:

And the Ultimate GPS mount:

Because the GPS only has two mounting screws, I used a 1x4 and a 2x4 lego plate and drilled in the topmost stud of each so that the plate would support the GPS along it's whole length. I drilled two holes and zip tied the other end as well for security. Lastly, I made a hightened mount for the compass module to keep it far away from the motor and related interference. Of course also out of lego.

Note the GPS antenna on the left side of the picture. It is magnetic and the plate is steel of some sort, so it needs no mounting other than what it has built in. Here is a picture of it mounted on the Flux:

Admittedly, it looks a little goofy in this picture, but it does look better in real life, and besides, I am building a GPS guided vehicle, not a show truck. And it kind of reminds me of the look of a lot of the vehicles that competed in the DARPA Grand Challenge. Form follows function in my opinion. Anyway, no more software progress, this was a hardware night, and thats all for now.

Purpose built GPS guided vehicle beginnings. And also hello again :)

Well, it has been a long while since I last posted. In the meantime, I have graduated, worked at a summer camp for the summer, finally got my driver's licence and a car, got a full time job in the HVAC&R trade, moved out from home into my own place, and generally been busy with life :) Due to all of that, I haven't got to work on any robots for quite a while but now I am back. Anyway onto the good stuff :)

With some of the proceeds from the afforementioned job, I recently purchased this beast:

 An HPI Savage Flux 2350. Decent little unit, runs on two 2s LiPo packs wired in series, upgraded with a 2.4 GHz radio, has enough power to do a backflip from a standstill, that kind of thing. Also handily a 4x4 which is exccelent at running over rough terrain. No longer am I bound by cheap RC cars that have no proportional steering and throttle! In preperation for turning the Flux into a GPS guided vehicle, I have elected to mount a metal plate to the four posts that the plastic shells normally mount on, reinforced with some attatchment to the center roll bar. Remember I said that I worked in the HVAC&R trade? Well this little chunk of metal is from a filter rack for a new furnace that we installed and didn't need, so I saved it from the scrap metal pile.

 The black plastic piece is where the plate will mount on the truck. Their height can be adjusted in the mounts on the truck. I have not yet drilled the holes cause my drill is at work, but soon.

Lastly, I have worked on the software side a lot too, thats where I usually procrastonate, but Im getting the jump on this one and doing both at the same time. I broke out my Arduino Mega 2560 clone, Adafruit Ultimate GPS, and HMC5883L compass, wired them all up and started coding.

The compass is wired to the Mega`s I2C port, and the GPS is going to serial1. I have an external antenna wired to the GPS also, and is sitting on one of my windowsills for better reception. Currently I am using Jeff Rowberg`s EXCCELENT i2cdevlib library for the compass but not adafruit`s library for the GPS. I am very unskilled at programming and I could not for the life of me understand what was going on in most of the sketches, and also found that it didn`t like to parse at 5 or 10 Hz which I really need. I like to be able to understand everything in my code so even if it is inefficent, I would rather find a way that I can understand. Last year around this time I had succuss parsing the data from the GPS using the new Serial.parseFloat command and I have just been using that because it seems to work very well, and I can understand it. Here is the code running on my Mega right now, if you would like to try it, it should compile correctly and run provided you have Jeff`s library installed. Drop me a line if it doesn`t and you would like to try.

#include "Wire.h"  //Crap for the compass
#include "I2Cdev.h"
#include "HMC5883L.h"
HMC5883L mag;
int16_t mx, my, mz;

float latdest = 49.1020021;  // Initial destionation waypoint coordiantes.
float londest = -117.5518276;

void setup()
{
  Wire.begin();   //More compass crap
    mag.initialize();
 
  Serial.begin(115200);  // Debugging over USB
  Serial1.begin(9600);  // Ultimate GPS is connected to serial 1
}

void loop()
{
  mag.getHeading(&mx, &my, &mz);  // Reading and debugging the compass reading.
    /*Serial.print("mag:\t");
    Serial.print(mx); Serial.print("\t");
    Serial.print(my); Serial.print("\t");
    Serial.print(mz); Serial.print("\t");*/
    float heading = atan2(my, mx);
    if(heading < 0)
      heading += 2 * M_PI;
      heading = heading * 180/M_PI;
    /*Serial.print("heading:\t");
    Serial.println(heading);*/
 
  if (Serial1.available() > 62)  // This little chunck of code just parses the GPS data and sends it to the USB port for debugging.
  {
    Serial1.find("$GPRMC");
    float tim = Serial1.parseFloat();
    float lat = Serial1.parseFloat();
    float lon = Serial1.parseFloat();
    float spd = Serial1.parseFloat();
    float hdg = Serial1.parseFloat();
    /*Serial.print("Time: ");
    Serial.print(tim, DEC);
    Serial.print(" Lat: ");
    Serial.print(lat, DEC);
    Serial.print(" Long: ");
    Serial.print(lon, DEC);
    Serial.print(" Speed: ");
    Serial.print(spd, DEC);
    Serial.print(" Heading: ");
    Serial.println(hdg, DEC);*/
   
    int latd= lat/100;  // Converting the Degrees, decimal minutes given by the GPS into decimal degrees and also debugging over the serial port.
    float latm = lat - latd*100;
    latm = latm/60;
    lat = latd + latm;   
    int lond= lon/100;
    float lonm = lon - lond*100;
    lonm = lonm/60;
    lon = lond + lonm;
    lon = 0 - lon;
    Serial.print(lat, DEC);
    Serial.print("  ");
    Serial.print(lon, DEC);
    Serial.print("  ");
   
    float deltay = latdest - lat;  // Comparing current location to next waypoint and calculating heading needed to travel there. Also debugging it.
    float deltax = londest - lon;
    /*Serial.print(deltay, DEC);
    Serial.print("  ");
    Serial.print(deltax, DEC);
    Serial.print("  ");*/
    float hdgrad = atan2(deltax,deltay);
    /*Serial.print(hdgrad, DEC);
    Serial.print("  ");*/
    float hdgdeg = hdgrad*180/3.14;
    /*Serial.print(hdgdeg, DEC);
    Serial.print("  ");*/
    if (hdgdeg < 0)
      hdgdeg = hdgdeg + 360;
    Serial.print(hdgdeg, DEC);
    Serial.print("  ");
    Serial.print(heading, DEC);
    Serial.print("\t");
   
    int err = heading - hdgdeg;  // Calculating error between headings and debugging.
    err = ((((err) % 360) + 540) % 360) - 180;
    Serial.print(err, DEC);
    Serial.print("  ");
   
    if(err > 5)
    {
      Serial.print("Left  ");
    }
    else if(err < -5)
    {
      Serial.print("right  ");
    }
    else
    {
      Serial.print("center  ");
    }
   
    Serial.println();
  }
  delay(1);
}

Up near the top are the latitude and longitude of the waypoint you want to go to. I use google maps to find my waypoints. (The waypoint set there is not where I live, by the way) Change those to whatever you like, and this will start spitting out your current latitude, longitude, heading you need to travel to get to your destinaion, current heading given by the compass, difference between the two, and weather you should turn left, right, or stay centered out of the serial port at 115200 baud. If you uncomment out all that other stuff, it will spit a bunch of other data from the intermediate steps too amongst other things. It seems to be running well, and at some point I need to mount my GPS and compass a little more securely and take the assembly with my laptop outside to pretend to be a GPS guided vehicle and see how it does more thouroghly. Anyway its a start! Ill post updates as I make progress.