CreeperBot Sketch (v1) (compass)

CreeperBot v1 (Compass)

/***************************************************************************
  This code created for Xxxxxxxx Xxxxxxx CreeperBot.
  
  Parts of code taken from various sources, namely
  1. This is a library example for the HMC5883 magnentometer/compass
     http://www.adafruit.com/products/1746
     Above modified to remove delay's 

 ***************************************************************************/

#include <Event.h>
#include <Timer.h>
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_HMC5883_U.h>

//Pin 13 LED as a watchdog, it should be flashing at the rate of our first interrupt, 1/4 second.
int watchdogLed = 13;

//COMPASS SETTINGS
int compassTimerId; //this is the ID of the timer object that triggers compas reads
Timer compassTimer; // this is the compass timer
int compassRate = 4; //number of times per second!
boolean compassReadNow = false; //set to true to have the main loop read your current heading
float declinationAngle = 0.123; //the magnetic north declination for this area
float installationAngle = PI; //we are PI radians out of phase, the compass is mounted facing south
float heading = 0; //this is what direction we are currently point, in degress from north
/* Assign a unique ID to this sensor at the same time */
Adafruit_HMC5883_Unified mag = Adafruit_HMC5883_Unified(12345);
//END OF COMPASSS SETTINGS




void displaySensorDetails(void)
{
  sensor_t sensor;
  mag.getSensor(&sensor);
  Serial.println("------------------------------------");
  Serial.print  ("Sensor:       "); Serial.println(sensor.name);
  Serial.print  ("Driver Ver:   "); Serial.println(sensor.version);
  Serial.print  ("Unique ID:    "); Serial.println(sensor.sensor_id);
  Serial.print  ("Max Value:    "); Serial.print(sensor.max_value); Serial.println(" uT");
  Serial.print  ("Min Value:    "); Serial.print(sensor.min_value); Serial.println(" uT");
  Serial.print  ("Resolution:   "); Serial.print(sensor.resolution); Serial.println(" uT");  
  Serial.println("------------------------------------");
  Serial.println("");
}

void setup(void) 
{
  Serial.begin(9600);
  Serial.println("Hilliard Bradley Crepper Bot!"); Serial.println("");
  Serial.println("Setting up Watchdog on Pin 13 LED");
  pinMode(watchdogLed, OUTPUT);    
  Serial.print("Setting up an interrupt timer for the compass ");Serial.print(compassRate);Serial.println(" times a second.");
  compassTimerId = compassTimer.every(1000/compassRate, compassInterrupt);
  Serial.println("Initializing the Compass");
  /* Initialise the sensor */
  if(!mag.begin())
  {
    /* There was a problem detecting the HMC5883 ... check your connections */
    Serial.println("Ooops, no HMC5883 detected ... Check your wiring!");
    while(1);
  }
  
  /* Display some basic information on this sensor */
  displaySensorDetails();
}

/**
* Get the current heading from the compass and return it.
*/
float getHeading()
{
  //Serial.print(millis());Serial.println(" getHeading enter");
  compassReadNow = false;
  /* Get a new sensor event */ 
  sensors_event_t event; 
  mag.getEvent(&event);
  
  /* Display the results (magnetic vector values are in micro-Tesla (uT)) */
  //Serial.print("X: "); Serial.print(event.magnetic.x); Serial.print("  ");
  //Serial.print("Y: "); Serial.print(event.magnetic.y); Serial.print("  ");
  //Serial.print("Z: "); Serial.print(event.magnetic.z); Serial.print("  ");Serial.println("uT");

  // Hold the module so that Z is pointing 'up' and you can measure the heading with x&y
  // Calculate heading when the magnetometer is level, then correct for signs of axis.
  float heading = atan2(event.magnetic.y, event.magnetic.x);
  
  // Once you have your heading, you must then add your 'Declination Angle', which is the 'Error' of the magnetic field in your location.
  // Find yours here: http://www.magnetic-declination.com/
  // Mine is: -13* 2' W, which is ~13 Degrees, or (which we need) 0.22 radians
  // If you cannot find your Declination, comment out these two lines, your compass will be slightly off.
  
  //Columbus is -7* 3'... there are PI radians in 180 degrees... so convert this
  //This is (7 + 3/60) = 7.05* * 3.1415927 radians/180* = 0.123 radians 
  heading += declinationAngle;
  //compensate for how the compass is mounted
  heading -= installationAngle;
  
  // Correct for when signs are reversed.
  if(heading < 0)
    heading += 2*PI;
    
  // Check for wrap due to addition of declination.
  if(heading > 2*PI)
    heading -= 2*PI;
   
  // Convert radians to degrees for readability.
  float headingDegrees = heading * 180/M_PI; 
  //Serial.print(millis());Serial.print(": Heading (degrees): "); Serial.println(headingDegrees);
  
  //toggle the watchdog led
  digitalWrite(watchdogLed, !digitalRead(watchdogLed));
  
  return headingDegrees;
}

void compassInterrupt() 
{
  compassReadNow = true;
}

void loop(void) 
{
  compassTimer.update();
  if (compassReadNow) 
  { 
    heading = getHeading(); 
    Serial.print(millis());Serial.print(": Heading (degrees): "); Serial.println(heading);
  }

}