Created
December 19, 2012 05:36
-
-
Save michelleboisson/4334624 to your computer and use it in GitHub Desktop.
This reads the heading from HMC6352, reads your current location (hardcoded),
and your destination (hardcoded), and turns the pin high pointing to your destination
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
//Compass Bracelet GPS | |
//This reads the heading from HMC6352, reads your current location (hardcoded), | |
//and your destination (hardcoded), and turns the pin high pointing to | |
//your destination | |
//HMC6352 compass is hooked up this way: SDA is on analog input pin 4, and SCL is on analog pin 5 | |
//To customize, change destinationLat, destinationLon, currentLat, currentLon | |
#include <SoftwareSerial.h> | |
#include <Wire.h> | |
int HMC6352SlaveAddress = 0x42; | |
int HMC6352ReadAddress = 0x41; //"A" in hex, A command is: | |
int headingInt; | |
int realNorth = 0; | |
int notificationDirection = 0; | |
const float destinationLat = 55.597261; | |
const float destinationLon = 12.979427; | |
float currentLat = 40.729483; | |
float currentLon = -73.993513; | |
//float currentLat, currentLon; | |
int directionToDestination = 0; | |
int EPin = 13;//east | |
int SPin = 12;//south | |
int WPin = 9;//west | |
int NPin = 8;//north | |
SoftwareSerial nss(3, 2); | |
void setup(){ | |
// "The Wire library uses 7 bit addresses throughout. | |
//If you have a datasheet or sample code that uses 8 bit address, | |
//you'll want to drop the low bit (i.e. shift the value one bit to the right), | |
//yielding an address between 0 and 127." | |
HMC6352SlaveAddress = HMC6352SlaveAddress >> 1; // I know 0x42 is less than 127, but this is still required | |
while( millis() < 500) { delay(10); } // The HMC6343 needs a half second to start from power up | |
pinMode(WPin, OUTPUT); | |
pinMode(NPin, OUTPUT); | |
pinMode(EPin, OUTPUT); | |
pinMode(SPin, OUTPUT); | |
Serial.begin(9600); | |
Wire.begin(); | |
} | |
void loop(){ | |
//"Get Data. Compensate and Calculate New Heading" | |
Wire.beginTransmission(HMC6352SlaveAddress); | |
Wire.write(HMC6352ReadAddress); // The "Get Data" command | |
Wire.endTransmission(); | |
//time delays required by HMC6352 upon receipt of the command | |
//Get Data. Compensate and Calculate New Heading : 6ms | |
delay(6); | |
directionToDestination = calculateDirection(currentLat, currentLon,destinationLat,destinationLon) * 10; | |
Serial.print("dir: "); | |
Serial.println(directionToDestination); | |
readCompass(); | |
realNorth = 3600-headingInt; | |
notificationDirection = realNorth + directionToDestination; | |
if(notificationDirection > 3600) | |
{ | |
notificationDirection = notificationDirection - 3600; | |
} | |
showDirection(notificationDirection); | |
delay(500); | |
} | |
void showDirection(int heading){ | |
//North | |
if (heading > 330 || heading < 30 ){ | |
digitalWrite(NPin, HIGH); | |
digitalWrite(EPin, LOW); | |
digitalWrite(SPin, LOW); | |
digitalWrite(WPin, LOW); | |
} | |
//NorthEast | |
if (heading > 30 && heading < 60 ){ | |
digitalWrite(NPin, HIGH); | |
digitalWrite(EPin, HIGH); | |
digitalWrite(SPin, LOW); | |
digitalWrite(WPin, LOW); | |
} | |
//East | |
if (heading > 60 && heading < 120 ){ | |
digitalWrite(EPin, HIGH); | |
digitalWrite(SPin, LOW); | |
digitalWrite(WPin, LOW); | |
digitalWrite(NPin, LOW); | |
} | |
//SouthEast | |
if (heading > 120 && heading < 150 ){ | |
digitalWrite(EPin, HIGH); | |
digitalWrite(SPin, HIGH); | |
digitalWrite(NPin, LOW); | |
digitalWrite(WPin, LOW); | |
} | |
//South | |
if (heading > 150 && heading < 210 ){ | |
digitalWrite(SPin, HIGH); | |
digitalWrite(NPin, LOW); | |
digitalWrite(WPin, LOW); | |
digitalWrite(EPin, LOW); | |
} | |
//SouthWest | |
if (heading > 210 && heading < 240 ){ | |
digitalWrite(SPin, HIGH); | |
digitalWrite(WPin, HIGH); | |
digitalWrite(EPin, LOW); | |
digitalWrite(NPin, LOW); | |
} | |
//West | |
if (heading > 240 && heading < 300 ){ | |
digitalWrite(WPin, HIGH); | |
digitalWrite(SPin, LOW); | |
digitalWrite(EPin, LOW); | |
digitalWrite(NPin, LOW); | |
} | |
//NorthhWest | |
if (heading > 300 && heading < 330 ){ | |
digitalWrite(NPin, HIGH); | |
digitalWrite(WPin, HIGH); | |
digitalWrite(SPin, LOW); | |
digitalWrite(EPin, LOW); | |
} | |
} | |
void readCompass(){ | |
Wire.requestFrom(HMC6352SlaveAddress, 2); //get the two data bytes, MSB and LSB | |
//"The heading output data will be the value in tenths of degrees | |
//from zero to 3599 and provided in binary format over the two bytes." | |
byte MSB = Wire.read(); | |
byte LSB = Wire.read(); | |
float headingSum = (MSB << 8) + LSB; //(MSB / LSB sum) | |
headingInt = headingSum / 10; | |
Serial.print(headingInt); | |
Serial.println(" degrees"); | |
} | |
float calculateDirection (float lat1, float long1, float lat2, float long2) { | |
// returns initial course in degrees (North=0, West=270) from | |
// position 1 to position 2, both specified as signed decimal-degrees | |
// latitude and longitude. | |
float dlon = radians(long2-long1); | |
lat1 = radians(lat1); | |
lat2 = radians(lat2); | |
float a1 = sin(dlon) * cos(lat2); | |
float a2 = sin(lat1) * cos(lat2) * cos(dlon); | |
a2 = cos(lat1) * sin(lat2) - a2; | |
a2 = atan2(a1, a2); | |
if (a2 < 0.0) { | |
a2 += TWO_PI; | |
} | |
return degrees(a2); | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment