Monday 3 November 2014

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.

No comments:

Post a Comment