Saturday, 22 November 2014

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
  steer.attach(5,1000,2000);  // Steering servo crap
  pinMode(4, OUTPUT); // status LED
  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)
      lat = Serial1.parseFloat();
      latd= lat/100;
    //Failsafe code if the GPS looses fix can be inserted here
    Serial2.println(lat, DEC);
  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
    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("  ");
  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.

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.

No comments:

Post a Comment