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!