Arduino Sketch below...
Notes:
Debug print statement are still active.. very useful for debugging.. but take out when happy with code.
Code starts here, COPY from after the "//" to the end..
//**************************************************************************************************************************************
//
// Arduino Compass HUD - Use RGB LED(s) to communicate bearing to diver and other info ( Tilt/Waypoint direction)
//
// Author : johnohuk@gmail.com
//
//
// History
//
// 3_0 Feedback from first headmounted prototype,
// 2 RGB LED Version
//
// Two Hall Effect sensors to increment and decrement current waypoint.
//
// Reference the I2C Library
#include <Wire.h>
// Reference the HMC5883L Compass Library
#include <HMC5883L.h>
// Reference the ADXL345 Accelerometer Library
#include <ADXL345.h>
// Store our compass as a variable.
HMC5883L compass;
// Store our accelerometer as a variable.
ADXL345 accel;
int areConnected = 0; // Store our connection status here.
// Main NSEW Bearing LED
#define LED1_RED 3
#define LED1_BLUE 6
#define LED1_GREEN 5
// Secondary Tilt/Waypoint LED
#define LED2_RED 10
#define LED2_BLUE 9
#define LED2_GREEN 11
// Inputs for controlling current waypoint
#define HF_1 2 // Hall effect sensor #1 on Pin2
#define HF_2 12 // Hall effect sensor #2 on Pin12
#define delayTime 20
#define BoardLed 13 // digitalWrite(BoardLed , HIGH);// HIGH/LOW only
int tilt_too_muchn; // Global flag to indicate the tilt is too much to give a valid bearing
int WayPoints[13] = {-1,56,130,180,93,45,300,55,66,130,200,230,66};
int Current_WayPoint = 1;
int WayPointMax = 12; // Size of the waypoint array
int hallState1; // the current reading from the input pin
int lasthallState1 = LOW; // the previous reading from the input pin
int hallState2; // 2nd Hall effect sensor
int lasthallState2 = LOW;
int hallcount1 = 0; // Current Halleffect sensor (A3144)senses two changes incoming/outgoing so need /2
int hallcount2 = 0;
void setup()
{
pinMode(LED1_GREEN, OUTPUT); //LED #1 Primary Compass showing N,S,E and West
pinMode(LED1_BLUE, OUTPUT);
pinMode(LED1_RED, OUTPUT);
pinMode(LED2_GREEN, OUTPUT); // LED #2 Level indicator and Waypoint indicator
pinMode(LED2_BLUE, OUTPUT);
pinMode(LED2_RED, OUTPUT);
pinMode(HF_1, INPUT ); // Inputs to control Waypoint selection, increment and decrement pointer into Array
pinMode(HF_2, INPUT );
Serial.begin(9600); // Initialize the serial port.
Wire.begin(); // Start the I2C interface.
compass = HMC5883L(); // Construct a new HMC5883 compass.
accel = ADXL345(); // Construct a new ADXL345 accelerometer.
compass.EnsureConnected();
accel.EnsureConnected();
if(compass.IsConnected && accel.IsConnected)
{
areConnected = true;
Serial.println("Connected to HMC5883L and ADXL345.");
}
else
{
areConnected = false;
if(compass.IsConnected == 0)
Serial.println("Could not connect to HMC5883L.");
if(accel.IsConnected == 0)
Serial.println("Could not connect to ADXL345.");
digitalWrite(BoardLed , HIGH); //If connection fault then onboard green LED will stay on.
}
if(areConnected)
{
compass.SetScale(1.3); // Set the scale of the compass.
compass.SetMeasurementMode(Measurement_Continuous); // Set the measurement mode to Continuous
accel.SetRange(2, true); // Set the range of the accelerometer to a maximum of 2G.
accel.EnableMeasurements(); // Tell the accelerometer to start taking measurements.
}
// Test the three colours in both LED(s), ramp up each one...
for( int i = 0 ; i < 5 ; i += 1 ){
analogWrite( LED1_RED, i );
analogWrite( LED1_BLUE, 0 );
analogWrite( LED1_GREEN, 0 );
analogWrite( LED2_RED, i );
analogWrite( LED2_BLUE, 0 );
analogWrite( LED2_GREEN, 0 );
delay( 200 );
Serial.println(i);
}
digitalWrite(BoardLed , LOW); // Toggle the LED on the board itself to indicate progress..
for( int i = 0 ; i < 5 ; i += 1 ){
analogWrite( LED1_RED, 0 );
analogWrite( LED1_BLUE, i );
analogWrite( LED1_GREEN, 0 );
analogWrite( LED2_RED, 0 );
analogWrite( LED2_BLUE, i );
analogWrite( LED2_GREEN, 0 );
delay( 200 );
Serial.println(i);
}
digitalWrite(BoardLed , HIGH);
for( int i = 0 ; i < 5 ; i += 1 ){
analogWrite( LED1_RED, 0 );
analogWrite( LED1_BLUE, 0 );
analogWrite( LED1_GREEN, i );
analogWrite( LED2_RED, 0 );
analogWrite( LED2_BLUE, 0 );
analogWrite( LED2_GREEN, i );
delay( 200 );
Serial.println(i);
}
digitalWrite(BoardLed , LOW);
}
void loop()
{
int MaxVal = 5; //Full on LED=255, 5 = low, dont want LED to be distracting...
int headingTCn=0;
if(areConnected)
{
MagnetometerScaled magnetometerReadings = compass.ReadScaledAxis();
AccelerometerScaled accelerometerReadings = accel.ReadScaledAxis();
float headingNTC = CalculateHeadingNotTiltCompensated(magnetometerReadings);
float headingTC = CalculateHeadingTiltCompensated(magnetometerReadings, accelerometerReadings);
// Update RGB (LED1)
headingTCn = int ( RadiansToDegrees(headingNTC)); //changed!!
//Serial.println(headingTCn);
if (tilt_too_muchn == 1 )
{
Serial.println("Tilted too far");
analogWrite( LED1_RED, 0 );
analogWrite( LED1_BLUE, 0 );
analogWrite( LED1_GREEN, 0 );
analogWrite( LED2_GREEN, 0 ); // Clear 2nd LED2 which indicates headset is level... so all u need is to keep that orientation and twist head to get a bearing..
}
else if ( headingTCn>355 || headingTCn <05 )
{
Serial.println("North");
analogWrite( LED1_RED, 0 );
analogWrite( LED1_BLUE, 0 );
analogWrite( LED1_GREEN, MaxVal );
}
else if ( headingTCn> 84 && headingTCn <95 )
{
Serial.println("EAST");
analogWrite( LED1_RED, 0 );
analogWrite( LED1_BLUE, MaxVal );
analogWrite( LED1_GREEN, 0 );
}
else if ( headingTCn> 174 && headingTCn <185 )
{
Serial.println("SOUTH");
analogWrite( LED1_RED, MaxVal );
analogWrite( LED1_BLUE, MaxVal );
analogWrite( LED1_GREEN, MaxVal );
}
else if ( headingTCn> 264 && headingTCn <275 )
{
Serial.println("WEST");
analogWrite( LED1_RED, MaxVal );
analogWrite( LED1_BLUE, 0 );
analogWrite( LED1_GREEN, 0 );
}
else
{
// For any heading outside the above parameters
analogWrite( LED1_RED, 0 );
analogWrite( LED1_BLUE, 0 );
analogWrite( LED1_GREEN, 0 );
}
//Waypoint functionallity & Level indicator - communicated through LED2 Blue..
// Sense Inputs
hallState1 = digitalRead(HF_1);
hallState2 = digitalRead(HF_2);
Serial.print("debug - Hallstate 1 & 2, Waypoint Bearing and Index : ");
Serial.print(hallState1);
Serial.print(" ");
Serial.print(hallState2);
Serial.print(" ");
Serial.print(WayPoints[Current_WayPoint]);
Serial.print(" ");
Serial.print(Current_WayPoint);
Serial.println("");
if (hallState1 != lasthallState1)
{
hallcount1 = hallcount1 +1;
lasthallState1 = hallState1;
}
if (hallState2 != lasthallState2)
{
hallcount2 = hallcount2 +1;
lasthallState2 = hallState2;
}
if ( hallcount1 == 2 )
{
if (Current_WayPoint<WayPointMax)
{
Current_WayPoint = Current_WayPoint + 1 ;
}
signal_led( Current_WayPoint); // Flash LED_2 Red the count
hallcount1 = 0;
}
if ( hallcount2 == 2 )
{
if (Current_WayPoint>1)
{
Current_WayPoint = Current_WayPoint - 1 ;
}
signal_led( Current_WayPoint); // Flash LED_2 Red the count
hallcount2 = 0;
}
// Level indicator & Waypoint display
if (tilt_too_muchn == 0 )
{
if ( headingTCn> (WayPoints[Current_WayPoint] -5 )&& headingTCn< (WayPoints[Current_WayPoint] +5 ) )
{
analogWrite( LED2_GREEN, 0 );
analogWrite( LED2_BLUE, MaxVal );
Serial.println("On Waypoint Bearing");
}
else
{
analogWrite( LED2_BLUE, 0 );
analogWrite( LED2_GREEN, MaxVal ); // Set 2nd LED2 to green to indicate headset is level... so all u need is to keep that orientation and twist head to get a bearing..
Serial.println("Level but NO Waypoint Bearing");
}
}
}
}
void signal_led ( int pulse_count )
{
// Pulse LED_2 Red using the pulse_count to indicate to user which waypoint is currently active
analogWrite( LED2_BLUE, 0 );
analogWrite( LED2_GREEN, 0 );
analogWrite( LED2_RED, 0 );
delay( 2000 );
for( int i = 1 ; i <= pulse_count ; i += 1 )
{
analogWrite( LED2_RED, 1 );
delay( 1000 );
analogWrite( LED2_RED, 0 );
delay( 1000 );
}
analogWrite( LED2_RED, 0 );
delay( 2000 );
}
float CalculateHeadingTiltCompensated(MagnetometerScaled mag, AccelerometerScaled acc)
{
// We are swapping the accelerometers axis as they are opposite to the compass the way we have them mounted.
// We are swapping the signs axis as they are opposite.
// Configure this for your setup.
float accX = -acc.YAxis;
float accY = -acc.XAxis;
// float accY = -acc.YAxis;
// float accX = -acc.XAxis;
float rollRadians = asin(accY);
float pitchRadians = asin(accX);
// We cannot correct for tilt over 40 degrees with this algorthem, if the board is tilted as such, return 0.
// if(rollRadians > 0.78 || rollRadians < -0.78 || pitchRadians > 0.78 || pitchRadians < -0.78) //44 degress
if(rollRadians > 0.175 || rollRadians < -0.175 || pitchRadians > 0.175 || pitchRadians < -0.175) //10 degrees
{
tilt_too_muchn = 1;
return 0;
}
else
{
tilt_too_muchn = 0;
}
// Some of these are used twice, so rather than computing them twice in the algorithem we precompute them before hand.
float cosRoll = cos(rollRadians);
float sinRoll = sin(rollRadians);
float cosPitch = cos(pitchRadians);
float sinPitch = sin(pitchRadians);
// The tilt compensation algorithem.
float Xh = mag.XAxis * cosPitch + mag.ZAxis * sinPitch;
float Yh = mag.XAxis * sinRoll * sinPitch + mag.YAxis * cosRoll - mag.ZAxis * sinRoll * cosPitch;
float heading = atan2(Yh, Xh);
return heading;
}
float CalculateHeadingNotTiltCompensated(MagnetometerScaled mag)
{
// Calculate heading when the magnetometer is level, then correct for signs of axis.
float heading = atan2(mag.YAxis, mag.XAxis);
return heading;
}
float RadiansToDegrees(float rads)
{
// Correct for when signs are reversed.
if(rads < 0)
rads += 2*PI;
// Check for wrap due to addition of declination.
if(rads > 2*PI)
rads -= 2*PI;
// Convert radians to degrees for readability.
float heading = rads * 180/PI;
return heading;
}
void Output(float headingNTC, float headingTC)
{
Serial.print("Heading (Not Compensated): ");
Serial.print(RadiansToDegrees(headingNTC));
Serial.print("\tHeading (Tilt Compensated): ");
Serial.println(RadiansToDegrees(headingTC));
}
No comments:
Post a Comment