Saturday 16 March 2013

Dive Compass - Part #5 - Program

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