Skip to content

Instantly share code, notes, and snippets.

@michelleboisson
Created December 19, 2012 05:36
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save michelleboisson/4334624 to your computer and use it in GitHub Desktop.
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
//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