Code to zero out inclinometer

Projects Programming
1

I have built an inclinometer using a 3-axis accelerometer. I need to "zero out" the degrees using a push button.

For example, if I have the inclinometer sitting on a 10 degree slant, I want to be able to press a push button and have the serial output register the degrees as zero instead of 10 degrees. Then, as I rotate the inclinometer, the degrees will register based off of the new zero baseline.

I think I need to be able to store the current degrees value when the push button is pressed and then use that stored value in the calculation I use to determine degrees.

For example, here is my calculation to calculate degrees:

    double x_Buff = float(mySensor.readAccelerometer(X_AXIS));
    double y_Buff = float(mySensor.readAccelerometer(Y_AXIS));
    double z_Buff = float(mySensor.readAccelerometer(Z_AXIS));
    int z = (atan2((- x_Buff) , sqrt(y_Buff * y_Buff + z_Buff * z_Buff)) * 57.3);

I think i want to introduce a variable, say "y", that will be set to the current degrees when the push button is pressed and then I can use the "y" variable in a new calculation such as:

x = (y - (atan2((- x_Buff) , sqrt(y_Buff * y_Buff + z_Buff * z_Buff)) * 57.3))

I am having troubles resetting my "y" variable every time I press the button on the push button. Can someone help? Is there a better way to do this?

Here is my code:

#include "NineAxesMotion.h"        	//Contains the bridge code between the API and the Arduino Environment
#include <Wire.h>			//Communicate with I2C / TWI devices

NineAxesMotion mySensor;		//Object that for the sensor
bool updateSensorData = true;         	//Flag to update the sensor data. Default is true to perform the first read before the first stream

int ledPinR1 = 5; 			// left red led
int ledPinY1 = 6; 			// left yellow led
int ledPinG1 = 8; 			// green led
int ledPinY2 = 9; 			// right yellow led
int ledPinR2 = 10;			// right red led

int buttonApin = 11;			// set zero button

void setup() 				//This code is executed once
{
  pinMode(ledPinR1, OUTPUT);
  pinMode(ledPinY1, OUTPUT);
  pinMode(ledPinG1, OUTPUT);
  pinMode(ledPinY2, OUTPUT);
  pinMode(ledPinR2, OUTPUT);
  pinMode(buttonApin, INPUT_PULLUP);  

  //Peripheral Initialization
  Serial.begin(115200);           	//Initialize the Serial Port to view information on the Serial Monitor
  I2C.begin();                    	//Initialize I2C communication to the let the library communicate with the sensor.

  //Sensor Initialization
  mySensor.initSensor();          	//The I2C Address can be changed here inside this function in the library
  mySensor.setOperationMode(OPERATION_MODE_NDOF);   //Can be configured to other operation modes as desired
  mySensor.setUpdateMode(MANUAL); 	//The default is AUTO. Changing to manual requires calling the relevant update functions prior to calling the read functions

  //Setting to MANUAL requires lesser reads to the sensor
  mySensor.updateAccelConfig();
  updateSensorData = true;

  Serial.println();
  Serial.println("Default accelerometer configuration settings...");
  Serial.print("Range: ");
  Serial.println(mySensor.readAccelRange());
  Serial.print("Bandwidth: ");
  Serial.println(mySensor.readAccelBandwidth());
  Serial.print("Power Mode: ");
  Serial.println(mySensor.readAccelPowerMode());
  Serial.println("Streaming in ...");             //Countdown
  Serial.print("3...");
  delay(1000);     //Wait for a second
  Serial.print("2...");
  delay(1000);     //Wait for a second
  Serial.println("1...");
  delay(1000);     //Wait for a second
}

void loop()				//This code is looped forever
{
    delay(50);    

    //Calcuate degrees of angle
    double x_Buff = float(mySensor.readAccelerometer(X_AXIS));
    double y_Buff = float(mySensor.readAccelerometer(Y_AXIS));
    double z_Buff = float(mySensor.readAccelerometer(Z_AXIS));
    int y = 0;
    int x = (atan2((- x_Buff) , sqrt(y_Buff * y_Buff + z_Buff * z_Buff)) * 57.3);

  if (digitalRead(buttonApin) == LOW)
  {
    //I need to code to set zero out current angle
  }

    int x = (y - (atan2((- x_Buff) , sqrt(y_Buff * y_Buff + z_Buff * z_Buff)) * 57.3));
  
  if (updateSensorData)  		//Keep the updating of data as a separate task
  {
    mySensor.updateAccel();        	//Update the Accelerometer data
    mySensor.updateLinearAccel();  	//Update the Linear Acceleration data
    mySensor.updateGravAccel();    	//Update the Gravity Acceleration data
    mySensor.updateCalibStatus();  	//Update the Calibration Status
    updateSensorData = false;
  }
  {
    Serial.print("      Pitch: ");
    Serial.print(x);
    Serial.print(" degrees    ");
    Serial.print(y);

    Serial.println();

    updateSensorData = true;
  }

  if ((x >= -5) && (x <= 5))		//Light green LED if angle is within specified degrees	
  {
    digitalWrite(ledPinR1, LOW);
    digitalWrite(ledPinY1, LOW);
    digitalWrite(ledPinG1, HIGH);
    digitalWrite(ledPinY2, LOW);
    digitalWrite(ledPinR2, LOW);
  }
  else if ((x >= 6) && (x <= 25))	//Light yellow LED if angle is within specified degrees
  {
    digitalWrite(ledPinR1, LOW);
    digitalWrite(ledPinY1, LOW);
    digitalWrite(ledPinG1, LOW);
    digitalWrite(ledPinY2, HIGH);
    digitalWrite(ledPinR2, LOW);
  }
  else if ((x >= 26) && (x <= 90))	//Light red LED if angle is within specified degrees
  {
    digitalWrite(ledPinR1, LOW);
    digitalWrite(ledPinY1, LOW);
    digitalWrite(ledPinG1, LOW);
    digitalWrite(ledPinY2, LOW);
    digitalWrite(ledPinR2, HIGH);
  }

  else if ((x >= -25) && (x <= -6))	//Light yellow LED if angle is within specified degrees
  {
    digitalWrite(ledPinR1, LOW);
    digitalWrite(ledPinY1, HIGH);
    digitalWrite(ledPinG1, LOW);
    digitalWrite(ledPinY2, LOW);
    digitalWrite(ledPinR2, LOW);
  }
  else if ((x >= -90) && (x <= -26))	//Light red LED if angle is within specified degrees
  {
    digitalWrite(ledPinR1, HIGH);
    digitalWrite(ledPinY1, LOW);
    digitalWrite(ledPinG1, LOW);
    digitalWrite(ledPinY2, LOW);
    digitalWrite(ledPinR2, LOW);
  }
  else					//if error
  {
    digitalWrite(ledPinR1, HIGH);
    digitalWrite(ledPinY1, LOW);
    digitalWrite(ledPinG1, HIGH);
    digitalWrite(ledPinY2, LOW);
    digitalWrite(ledPinR2, HIGH);
  }
}
2

Right now, you are continuously detecting whether a button is pressed, which is not particularly useful in this case.

Most people use "state change" code to detect the press and/or release of the button. Example here.

Then, consider using three variables: current_angle, offset_angle, reported_angle, where reported = current-offset, and offset is adjusted during the press and release operation.

For the most accurate tilt angles, you should make and average several measurements, after calibrating the accelerometer according to this tutorial.

3

@jremington - thank you. This makes a lot of sense. I understand exactly what you're explaining. I will try it out.

4

You should use variable names more meaningful than 'x' and 'y' especially when they are angles

//Calcuate degrees of angle
  double x_Buff = float(mySensor.readAccelerometer(X_AXIS));
  double y_Buff = float(mySensor.readAccelerometer(Y_AXIS));
  double z_Buff = float(mySensor.readAccelerometer(Z_AXIS));


  static float angleOffset = 0;


  float angle = atan2(- x_Buff , sqrt(y_Buff * y_Buff + z_Buff * z_Buff)) * 57.3;


  if (digitalRead(buttonApin) == LOW)
  {
    angleOffset = angle;
  }


  angle -= angleOffset;


  Serial.print("      Pitch: ");
  Serial.print(angle);
  Serial.print(" degrees (offset =");
  Serial.print(angleOffset);
  Serial.println(")");

Note the use of 'static' to create a local variable that retains its value across invocations of the function. Regular local variables pick up whatever garbage is in the stack space they are assigned when the function starts.

5

@johnwasser, thanks for the help. Your code worked perfectly and I was able to achieve the result I was looking for. Now I just need to figure out how to convert pitch to degrees. I understand that involves quaternions so I have some learning to do.

6

The output of this line is in degrees (the conversion factor is 180/PI, approximately 57.3).

 float angle = atan2(- x_Buff , sqrt(y_Buff * y_Buff + z_Buff * z_Buff)) * 57.3;
7

@jremington, you're right. I didn't explain my issue correctly. I am working in 0 to 90 and 0 - -90 degrees. I need to covert to 360 degrees.

8

bneuf:
I am working in 0 to 90 and 0 - -90 degrees. I need to covert to 360 degrees.

I think you can extend the measurement to 360 degrees by checking if z_Buff is negative:

  float angle = atan2(- x_Buff , sqrt(y_Buff * y_Buff + z_Buff * z_Buff)) * 57.3;
  // Range is -90 to 90
  angle += 90;  // Change range from -90..90 to 0..180

  // If upside-down, change 0..180 to 360..180 to get 0..360 angle
  if (z_Buff < 0)
     angle = 360 - angle;


 if (digitalRead(buttonApin) == LOW)
  {
    angleOffset = angle;
  }

  angle = (angle - angleOffset) % 360;

The '%' is the 'modulo' operator: The remainder after division. That way 390 degrees wraps around to 30 degrees.