Hackster is hosting Hackster Holidays, Ep. 7: Livestream & Giveaway Drawing. Watch previous episodes or stream live on Friday!Stream Hackster Holidays, Ep. 7 on Friday!
oztechlab
Published

Near-Perfect Gyroscope

This code enables you to make an accelerometer and gyroscope give the best results.

BeginnerWork in progress19,818

Things used in this project

Hardware components

SparkFun Triple Axis Accelerometer and Gyro Breakout - MPU-6050
SparkFun Triple Axis Accelerometer and Gyro Breakout - MPU-6050
×1
Jumper wires (generic)
Jumper wires (generic)
×1
Breadboard (generic)
Breadboard (generic)
If you just want to solder it together, you don't need this, but we suggest using pin headers for the gyroscope and wires.
×1
Arduino UNO
Arduino UNO
Nearly any Arduino will work for this, though the Uno is the standard Arduino.
×1

Hand tools and fabrication machines

Soldering iron (generic)
Soldering iron (generic)

Story

Read more

Schematics

Gyroscope wiring

This is a diagram of the wiring for this project. all 3 modes are filled in, so if you want you can use the breadboard, circuit or schematic view to wire your version!

Code

WIP stablized gyroscope

Arduino
Simply set up your gyroscope, plug the code in, and watch the fun on the serial monitor or serial plotter. Your numbers will be in degrees, but this is still WIP and may not work completely or at all.
/**
        _
   _  _ \_\  _
  |_|  /_\_\ | |_
          \_\
version 1.0.0 . this work is free to use by anyone, but please credit it if you are using it.
**/


#include<Wire.h>
#include<Serial.h>
/**
 * the #include 's are to tell the program to import the two libraries instead of writing them all out.
 * wire lets the gyroscope and the arduino communicate over the ic^2 connection.
 */
#define gyro_address 0x68 
/* change this number to the ic^2 adress of your gyroscope (in Hexidecimal, so find that column and ad a 0x if there isn't one), 
 *  though if you don't have one already, an Invenesense MPU-6050 will make the code work without edits.
*/
void setup() {
  int16_t accel_x, accel_y, accel_z, temp, gyro_x, gyro_y, gyro_z; 
  /**the int16_t defines 16 bits per variable. 15 can be used, but computers prefer 8, 16, 32, etc. change the number
   * to change the bits. these are left blank because the first thing to happen in the loop is them being filled with
   * the gyroscope's value. the math used has to be altered though, so some gyroscopes may use more bits. at this point,
   * you really need the documentation for the gyroscope.
   */
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true); //set this to false, and it doesn't end. simple enough, right?
  
  Wire.beginTransmission(gyro_address);
  Wire.write(0x6a);
  Wire.requestFrom(gyro_adress, 1, true);
  int usercontrol = Wire.read();
  Wire.endTransmission(true);
  Serial.begin(9600); 
  /**initiate 'Serial' library. most arduinos use 9600 baud (bits per second) but some use different amounts. 
   * again, check the documentation.*/
   double x_rotation = 0, y_rotation = 0, z_rotation = 0; 
   //these double variables have double the bits as others, so they can be longer and therefor more specific.
}

void loop() {
  // put your main code here, to run repeatedly:

}

Code version 1.1.0

Arduino
This is an updated version of the code, with a few improvements but no filter or math yet..
/**
        _
   _  _ \_\  _
  |_|  /_\_\ | |_
          \_\
version 1.1.0 . this work is free to use by anyone, but please credit it if you are using it.
**/


#include<Wire.h>
/**
 * the #include 's are to tell the program to import the two libraries instead of writing them all out.
 * wire lets the gyroscope and the arduino communicate over the ic^2 connection.
 */
#define gyro_address 0x68 
/* change this number to the ic^2 adress of your gyroscope (in Hexidecimal, so find that column and add a 0x if there isn't one), 
 *  though if you don't have one already, an Invenesense MPU-6050 will make the code work without edits.
*/
int16_t accel_x, accel_y, accel_z, temp, gyro_x, gyro_y, gyro_z; 
/**the int16_t defines 16 bits per variable. 15 can be used, but computers prefer 8, 16, 32, etc. change the number
   * to change the bits. these are left blank because the first thing to happen in the loop is them being filled with
   * the gyroscope's value. the math used has to be altered though, so some gyroscopes may use more bits. at this point,
   * you really need the documentation for the gyroscope.
   */
  
void setup() {
  Wire.begin();
  Wire.beginTransmission(gyro_address);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true); //set this to false, and it doesn't end. simple enough, right?
  
  Wire.beginTransmission(gyro_address);
  Wire.write(0x6a);
  Wire.requestFrom(gyro_address, 1, true);
  int usercontrol = Wire.read();
  Wire.endTransmission(true);
  Serial.begin(9600); 
  /**initiate 'Serial' library. most arduinos use 9600 baud (bits per second) but some use different amounts. 
   * again, check the documentation.*/
   double x_rotation = 0, y_rotation = 0, z_rotation = 0; 
   //these double variables have double the bits as others, so they can be longer and therefor more specific.
}

void loop() {
  Wire.beginTransmission(gyro_address);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H).this number may change depending on how the gyroscope transferrs data.
  Wire.endTransmission(false);
  Wire.requestFrom(gyro_address, 14, true); // request a total of 14 registers (again, it may be different for your gyroscope)
  accel_x = Wire.read() << 8 | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  accel_y = Wire.read() << 8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  accel_z = Wire.read() << 8 | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  temp = Wire.read() << 8 | Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  gyro_x = Wire.read() << 8 | Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  gyro_y = Wire.read() << 8 | Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  gyro_z = Wire.read() << 8 | Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
  /*
   * these 7 variables look familiar? we defined them as nothing in setup(). these store the raw values of the gyroscope.
   * once again, all these numbers may change. if you can, refer to a program someone else made with the same gyro to
   * save a couple headaches.
   */
   Serial.print(" | Accelerometer X:");
   Serial.print(accel_x);
   Serial.print(" | Accelerometer Y:");
   Serial.print(accel_y);
   Serial.print(" | Accelerometer Z:");
   Serial.print(accel_z);
   Serial.print(" | Temperature:");
   Serial.print(temp / 340.00 + 36.53);
   Serial.print(" | Gyro X:");
   Serial.print(gyro_x);
   Serial.print(" | Gyro Y:");
   Serial.print(gyro_y);
   Serial.print(" | Gyro Z:");
   Serial.println(gyro_z);
   delay(100);//this is important to make sure the board and arduino both aren't overloading. slower-operating gyros may need a bigger number.
}

Code version 1.2.1

Arduino
The code now adds data to the gyroscope and accelerometer saved data arrays. This will allow the Kalman filter to make its predictions based on past data. very important! There are also some other small fixes. the filter will probably be added soon, but when is unknown. We're doing our best to implement the math.
/**
        _
   _  _ \_\  _
  |_|  /_\_\ | |_
          \_\
version 1.2.1 (filter still in progress, but data gets saved) . this work is free to use by anyone, but please credit it if you are using it.
**/


#include<Wire.h>
/**
 * the #include 's are to tell the program to import the two libraries instead of writing them all out.
 * wire lets the gyroscope and the arduino communicate over the ic^2 connection.
 */
#define gyro_address 0x68 
/* change this number to the ic^2 adress of your gyroscope (in Hexidecimal, so find that column and add a 0x if there isn't one), 
 *  though if you don't have one already, an Invenesense MPU-6050 will make the code work without edits.
*/
int16_t accel_x, accel_y, accel_z, temp, gyro_x, gyro_y, gyro_z; 
/**the int16_t defines 16 bits per variable. 15 can be used, but computers prefer 8, 16, 32, etc. change the number
   * to change the bits. these are left blank because the first thing to happen in the loop is them being filled with
   * the gyroscope's value. the math used has to be altered though, so some gyroscopes may use more bits. at this point,
   * you really need the documentation for the gyroscope.
   */
array gyro_data = []; //this stores the past data from the sensor, giving the kalman filter some of its neccasary data.
array accel_data = []; //this stores the past data from the sensor, giving the kalman filter some of its neccasary data.
array gyro_current = []; //this feeds into the main variable, but it's here just to better orginize the X, Y. & Z values.
array accel_current = []; //this feeds into the main variable, but it's here just to better orginize the X, Y. & Z values.

void setup() {
  Wire.begin();
  Wire.beginTransmission(gyro_address);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true); //set this to false, and it doesn't end. simple enough, right?
  
  Wire.beginTransmission(gyro_address);
  Wire.write(0x6a);
  Wire.requestFrom(gyro_address, 1, true);
  int usercontrol = Wire.read();
  Wire.endTransmission(true);
  Serial.begin(9600); 
  /**initiate 'Serial' library. most arduinos use 9600 baud (bits per second) but some use different amounts. 
   * again, check the documentation.*/
   double x_rotation = 0, y_rotation = 0, z_rotation = 0; 
   //these double variables have double the bits as others, so they can be longer and therefor more specific.
}

void loop() {
  Wire.beginTransmission(gyro_address);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H).this number may change depending on how the gyroscope transferrs data.
  Wire.endTransmission(false);
  Wire.requestFrom(gyro_address, 14, true); // request a total of 14 registers (again, it may be different for your gyroscope)
  accel_x = Wire.read() << 8 | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  accel_y = Wire.read() << 8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  accel_z = Wire.read() << 8 | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  temp = Wire.read() << 8 | Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  gyro_x = Wire.read() << 8 | Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  gyro_y = Wire.read() << 8 | Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  gyro_z = Wire.read() << 8 | Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
  /*
   * these 7 variables look familiar? we defined them as nothing in setup(). these store the raw values of the gyroscope.
   * once again, all these numbers may change. if you can, refer to a program someone else made with the same gyro to
   * save a couple headaches.
   */
   accel_current = [accel_x,accel_y,accel_z];
   gyro_current = [gyro_x,gyro_y,gyro_z];
   Serial.print(" | Accelerometer X:");
   Serial.print(accel_x);
   Serial.print(" | Accelerometer Y:");
   Serial.print(accel_y);
   Serial.print(" | Accelerometer Z:");
   Serial.print(accel_z);
   Serial.print(" | Temperature:");
   Serial.print(temp / 340.00 + 36.53);
   Serial.print(" | Gyro X:");
   Serial.print(gyro_x);
   Serial.print(" | Gyro Y:");
   Serial.print(gyro_y);
   Serial.print(" | Gyro Z:");
   Serial.println(gyro_z);
   gyro_data[gyro_data.length()] = gyro_current;
   accel_data[accel_data.length()] = accel_current;
   /**
    * these two top statements add the current data from the gyro and accel to the data arrays, using a neat trick: every array counts 
    * starting at 0, but the .length() function starts at one. this allows us to create new data in the arrays constantly! be warned:
    * using a constantly adding array can be dangerous, especially with large amounts of data.
    */
   delay(100);//this is important to make sure the board and arduino both aren't overloading. slower-operating gyros may need a bigger number.
}

Code version 1.3.0

Arduino
Hurrah! now that we've found time, we've got a major update for you! the arrays didn't work, so they're gone, but now there is a complimentary filter, lots of thanks to Pieter-Jan of Pieter-jan.com for this math! next update should clean it up a little, it's getting long! NOTE: we've found out that the code is missing one piece, the atan2 conversion for the accelerometer data. do not use this code yet!
/**
        _
   _  _ \_\  _
  |_|  /_\_\ | |_
          \_\
version 1.3.0 (filter 1 out of 2 done, one more still in progress, no more data gets saved) . this work is free to use by anyone, but please credit it if you are using it.
**/


#include<Wire.h>
/**
 * the #include 's are to tell the program to import the two libraries instead of writing them all out.
 * wire lets the gyroscope and the arduino communicate over the ic^2 connection.
 */
#define gyro_address 0x68 
/* change this number to the ic^2 adress of your gyroscope (in Hexidecimal, so find that column and add a 0x if there isn't one), 
 *  though if you don't have one already, an Invenesense MPU-6050 will make the code work without edits.
*/
#define ACCELEROMETER_SENSITIVITY 8192.0
#define GYROSCOPE_SENSITIVITY 65.536
/* these are used to account for the lack of perfectness in the gyroscope, they can help a little
*/
int dataRate = 100;

int16_t accelX, accelY, accelZ, temp, gyroX, gyroY, gyroZ,angleX,angleY,angleZ; 
/**the int16_t defines 16 bits per variable. 15 can be used, but computers prefer 8, 16, 32, etc. change the number
   * to change the bits. these are left blank because the first thing to happen in the loop is them being filled with
   * the gyroscope's value. the math used has to be altered though, so some gyroscopes may use more bits. at this point,
   * you really need the documentation for the gyroscope.
   */


void setup() {
  Wire.begin();
  Wire.beginTransmission(gyro_address);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true); //set this to false, and it doesn't end. simple enough, right?
  
  Wire.beginTransmission(gyro_address);
  Wire.write(0x6a);
  Wire.requestFrom(gyro_address, 1, true);
  int usercontrol = Wire.read();
  Wire.endTransmission(true);
  Serial.begin(9600);
    /**initiate 'Serial' library. most arduinos use 9600 baud (bits per second) but some use different amounts. 
   * again, check the documentation.*/
  Serial.println("libraries loaded ");
  Serial.println();
  Serial.println("+--------------------+");
  Serial.println("|         _          |");
  Serial.println("|   _  _  | |  _     |");
  Serial.println("|  |_|  /_ | | | |_  |");
  Serial.println("|           |_|      |");
  Serial.println("|thank you for using |");
  Serial.println("|our code. mention us|");
  Serial.println("|if you use it!      |");
  Serial.println("+--------------------+");
  delay(2500); 
  //this is our message. please don't copy our code without mention. there's a lot of work here, and we wanted to share it with you as long as you respect our work.

   double x_rotation = 0, y_rotation = 0, z_rotation = 0; 
   //these double variables have double the bits as others, so they can be longer and therefor more specific.
}

void loop() {
  Wire.beginTransmission(gyro_address);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H).this number may change depending on how the gyroscope transferrs data.
  Wire.endTransmission(false);
  Wire.requestFrom(gyro_address, 14, true); // request a total of 14 registers (again, it may be different for your gyroscope)
  accelX = Wire.read() << 8 | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  accelY = Wire.read() << 8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  accelZ = Wire.read() << 8 | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  temp = Wire.read() << 8 | Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  gyroX = Wire.read() << 8 | Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  gyroY = Wire.read() << 8 | Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  gyroZ = Wire.read() << 8 | Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
  /*
   * these 7 variables look familiar? we defined them as nothing in setup(). these store the raw values of the gyroscope.
   * once again, all these numbers may change. if you can, refer to a program someone else made with the same gyro to
   * save a couple headaches.
   */
   //heavy math time:
   angleX = 0.98* (angleX + gyroX * dataRate) + 0.02 * accelX; 
   angleY = 0.98* (angleY + gyroY * dataRate) + 0.02 * accelY;
   angleZ = 0.98* (angleZ + gyroZ * dataRate) + 0.02 * accelZ;
   
   Serial.print(" | Accelerometer X:");
   Serial.print(accelX);
   Serial.print(" | Accelerometer Y:");
   Serial.print(accelY);
   Serial.print(" | Accelerometer Z:");
   Serial.print(accelZ);
   Serial.print(" | Temperature:");
   Serial.print(temp / 340.00 + 36.53);
   Serial.print(" | Gyro X:");
   Serial.print(gyroX);
   Serial.print(" | Gyro Y:");
   Serial.print(gyroY);
   Serial.print(" | Gyro Z:");
   Serial.println(gyroZ);
   //there's our first data sets.

      Serial.print(" | AngleX: ");
   Serial.print(angleX);
   Serial.print(" | Angle Y: ");
   Serial.print(angleY);
   Serial.print(" | Angle Z: ");
   Serial.println(angleZ);
   //and here's the second, with the filter!
   /**
    * these two top statements add the current data from the gyro and accel to the data arrays, using a neat trick: every array counts 
    * starting at 0, but the .length() function starts at one. this allows us to create new data in the arrays constantly! be warned:
    * using a constantly adding array can be dangerous, especially with large amounts of data.
    */
   delay(dataRate);//this is important to make sure the board and arduino both aren't overloading. slower-operating gyros may need a bigger number.
}

Code version 1.3.1

Arduino
Basically version 1.3.0, but we did some cleaning up, making this thing easier to follow. once the code is done, we can post a version that has no comments, for those of you who understand the code, and only want the program to copy. also, as a note: the program uses 22,940 bytes, which is close to the old uno's limit. a stripped-down version can be added later for those with this chip. NOTE: we've found out that the code is missing one piece, the atan2 conversion for the accelerometer data. do not use this code yet!
/**
        _
   _  _ \_\  _
  |_|  /_\_\ | |_
          \_\
version 1.3.1 (filter 1 out of 2 done, one more still in progress, cleaned up the text a little) . this work is free to use by anyone, but please credit it if you are using it.
**/
#include<Wire.h>
/**
 * the #include 's are to tell the program to import the two libraries instead of writing them all out. wire lets the gyroscope and the arduino communicate over the ic^2 connection.
 */
#define gyro_address 0x68 
/* change this number to the ic^2 adress of your gyroscope (in Hexidecimal, so find that column and add a 0x if there isn't one), though if you don't have one already, an Invenesense MPU-6050 will make the code work without edits.
*/
#define ACCELEROMETER_SENSITIVITY 8192.0
#define GYROSCOPE_SENSITIVITY 65.536
/* these are used to account for the lack of perfectness in the gyroscope, they can help a little CURRENTLY NOT IN USE
*/
int dataRate = 100;
int16_t accelX, accelY, accelZ, temp, gyroX, gyroY, gyroZ,angleX,angleY,angleZ; 
/**the int16_t defines 16 bits per variable. 15 can be used, but computers prefer 8, 16, 32, etc. change the number to change the bits. these are left blank because the first thing to happen in the loop is them being filled with
   * the gyroscope's value. the math used has to be altered though, so some gyroscopes may use more bits. at this point,you really need the documentation for the gyroscope.
   */

void setup() {
  Wire.begin();
  Wire.beginTransmission(gyro_address);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true); //set this to false, and it doesn't end. simple enough, right?
  
  Wire.beginTransmission(gyro_address);
  Wire.write(0x6a);
  Wire.requestFrom(gyro_address, 1, true);
  int usercontrol = Wire.read();
  Wire.endTransmission(true);
  Serial.begin(9600);
    /**initiate 'Serial' library. most arduinos use 9600 baud (bits per second) but some use different amounts. 
   * again, check the documentation.*/
  Serial.println("libraries loaded ");
  Serial.println();
  Serial.println("+--------------------+");Serial.println("|         _          |");Serial.println("|   _  _  | |  _     |");Serial.println("|  |_|  /_ | | | |_  |");Serial.println("|           |_|      |");Serial.println("|thank you for using |");Serial.println("|our code. mention us|");Serial.println("|if you use it!      |");Serial.println("+--------------------+");
  delay(2500); 
  //this is our message. please don't copy our code without mention. there's a lot of work here, and we wanted to share it with you as long as you respect our work.

   double x_rotation = 0, y_rotation = 0, z_rotation = 0; 
   //these double variables have double the bits as others, so they can be longer and therefor more specific.
}

void loop() {
  Wire.beginTransmission(gyro_address);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H).this number may change depending on how the gyroscope transferrs data.
  Wire.endTransmission(false);
  Wire.requestFrom(gyro_address, 14, true); // request a total of 14 registers (again, it may be different for your gyroscope)
  accelX = Wire.read() << 8 | Wire.read(); accelY = Wire.read() << 8 | Wire.read(); accelZ = Wire.read() << 8 | Wire.read(); //accelerometer data shift and variable update
  temp = Wire.read() << 8 | Wire.read(); //temperature data shift and variable update 
  gyroX = Wire.read() << 8 | Wire.read(); gyroY = Wire.read() << 8 | Wire.read(); gyroZ = Wire.read() << 8 | Wire.read(); //gyroscope data shift and variable update 
  /*
   * these 7 variables look familiar? we defined them as nothing in setup(). these store the raw values of the gyroscope. once again, all these numbers may change depending on 
   * gyroscope type. if you can, refer to a program someone else made with the same gyro to save a couple headaches. */
   //heavy math time:
   angleX = 0.98* (angleX + gyroX * dataRate) + 0.02 * accelX; 
   angleY = 0.98* (angleY + gyroY * dataRate) + 0.02 * accelY;
   angleZ = 0.98* (angleZ + gyroZ * dataRate) + 0.02 * accelZ;
   
   //Serial.print(" | Accelerometer X:");Serial.print(accelX);Serial.print(" | Accelerometer Y:");Serial.print(accelY);Serial.print(" | Accelerometer Z:");Serial.print(accelZ);Serial.print(" | Temperature:");Serial.print(temp / 340.00 + 36.53);Serial.print(" | Gyro X:");Serial.print(gyroX);Serial.print(" | Gyro Y:");Serial.print(gyroY);Serial.print(" | Gyro Z:");Serial.println(gyroZ);
   //there's our first data set, now retired. progress!.

      Serial.print(" | AngleX: ");
   Serial.print(angleX);
   Serial.print(" | Angle Y: ");
   Serial.print(angleY);
   Serial.print(" | Angle Z: ");
   Serial.println(angleZ);
   //and here's the second, with the filter!
   
   delay(dataRate);//this is important to make sure the board and arduino both aren't overloading. slower-operating gyros may need a bigger number.
}

code version 1.3.2

Arduino
hurrah! the math for the complimentary filter is done, meaning that there is one filter left, the kalman filter, and then the final conversion math can be completed.
/**
        _
   _  _ \_\  _
  |_|  /_\_\ | |_
          \_\
version 1.3.1 (filter 1 out of 2 done, one more still in progress, cleaned up the text a little) . this work is free to use by anyone, but please credit it if you are using it.
**/
#include<Wire.h>
#include<math.h>
/**
 * the #include 's are to tell the program to import the two libraries instead of writing them all out. wire lets the gyroscope and the arduino communicate over the ic^2 connection.
 */
#define gyro_address 0x68 
/* change this number to the ic^2 adress of your gyroscope (in Hexidecimal, so find that column and add a 0x if there isn't one), though if you don't have one already, an Invenesense MPU-6050 will make the code work without edits.
*/
#define ACCELEROMETER_SENSITIVITY 8192.0
#define GYROSCOPE_SENSITIVITY 65.536
/* these are used to account for the lack of perfectness in the gyroscope, they can help a little CURRENTLY NOT IN USE
*/
int dataRate = 100;
int16_t accelX, accelY, accelZ, temp, gyroX, gyroY, gyroZ,angleX,angleY,angleZ; 
/**the int16_t defines 16 bits per variable. 15 can be used, but computers prefer 8, 16, 32, etc. change the number to change the bits. these are left blank because the first thing to happen in the loop is them being filled with
   * the gyroscope's value. the math used has to be altered though, so some gyroscopes may use more bits. at this point,you really need the documentation for the gyroscope.
   */

void setup() {
  Wire.begin();
  Wire.beginTransmission(gyro_address);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true); //set this to false, and it doesn't end. simple enough, right?
  
  Wire.beginTransmission(gyro_address);
  Wire.write(0x6a);
  Wire.requestFrom(gyro_address, 1, true);
  int usercontrol = Wire.read();
  Wire.endTransmission(true);
  Serial.begin(9600);
    /**initiate 'Serial' library. most arduinos use 9600 baud (bits per second) but some use different amounts. 
   * again, check the documentation.*/
  Serial.println("libraries loaded ");
  //this is our message. please don't copy our code without mention. there's a lot of work here, and we wanted to share it with you as long as you respect our work.

   double x_rotation = 0, y_rotation = 0, z_rotation = 0; 
   //these double variables have double the bits as others, so they can be longer and therefor more specific.
}

void loop() {
  Wire.beginTransmission(gyro_address);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H).this number may change depending on how the gyroscope transferrs data.
  Wire.endTransmission(false);
  Wire.requestFrom(gyro_address, 14, true); // request a total of 14 registers (again, it may be different for your gyroscope)
  accelX = Wire.read() << 8 | Wire.read(); accelY = Wire.read() << 8 | Wire.read(); accelZ = Wire.read() << 8 | Wire.read(); //accelerometer data shift and variable update
  temp = Wire.read() << 8 | Wire.read(); //temperature data shift and variable update 
  gyroX = Wire.read() << 8 | Wire.read(); gyroY = Wire.read() << 8 | Wire.read(); gyroZ = Wire.read() << 8 | Wire.read(); //gyroscope data shift and variable update 
  /*
   * these 7 variables look familiar? we defined them as nothing in setup(). these store the raw values of the gyroscope. once again, all these numbers may change depending on 
   * gyroscope type. if you can, refer to a program someone else made with the same gyro to save a couple headaches. */
   //heavy math time:
   accelX = atan2(accelY, accelZ) * 57.2957795131;
   accelY = atan2(accelX, accelZ) * 57.2957795131;
   //accelZ = atan2(accelX, accelY): //due to math problems, the z axis isn't avaible as yet. however, two axis (plural) should be fine
   angleX = 0.98* (angleX + gyroX * dataRate) + 0.02 * accelX; 
   angleY = 0.98* (angleY + gyroY * dataRate) + 0.02 * accelY;
   //angleZ = 0.98* (angleZ + gyroZ * dataRate) + 0.02 * accelZ;
   
   //Serial.print(" | Accelerometer X:");Serial.print(accelX);Serial.print(" | Accelerometer Y:");Serial.print(accelY);Serial.print(" | Accelerometer Z:");Serial.print(accelZ);Serial.print(" | Temperature:");Serial.print(temp / 340.00 + 36.53);Serial.print(" | Gyro X:");Serial.print(gyroX);Serial.print(" | Gyro Y:");Serial.print(gyroY);Serial.print(" | Gyro Z:");Serial.println(gyroZ);
   //there's our first data set, now retired. progress!.
    Serial.print(" | AngleX: ");
   Serial.print(angleX);
   Serial.print(" | Angle Y: ");
   Serial.print(angleY);
   Serial.print(" | Angle Z: ");
   Serial.println(angleZ);
   //and here's the second, with the filter!
   
   delay(dataRate);//this is important to make sure the board and arduino both aren't overloading. slower-operating gyros may need a bigger number.
}

code version 1.4.2

Arduino
complete revamp of the whole system. its now running an optimized version of the original code with an awesome complimentary filter! the data is still jumpy, however, and updates are sure to come soon. sorry it's been so long!
#include<Wire.h>
#define MPU_addr 0x68//0x means HEX (because I don't really understand these things yet)
/**
        _
   _  _ \_\  _
  |_|  /_\_\ | |_
          \_\
  Version 1.4.2 (2/ now 4 filters done)
**/
//much of the original code was done by JonChi of the arduino community!
//vars for stuff
double AccelX, AccelY, AccelZ, GyroX, GyroY, GyroZ, AngleX, AngleY, AngleZ;
int tOne = 0, tTwo = 0;
int16_t AccelX_out, AccelY_out, AccelZ_out, Tmp, GyroX_out, GyroY_out, GyroZ_out;
double xR = 0, yR = 0, zR = 0, OldXr = 0, xA = 0, yA = 0, zA = 0;
double DegX, DegSX;
double dFudgeGyroX = 0;

//this loop just gets everything up and going
void setup() {

  Serial.begin(9600);
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);

  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6A);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 1, true);
  Wire.endTransmission(true);

}

void loop() {
  //communication with sensor
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 14, true); // request a total of 14 registers
  AccelX_out = Wire.read() << 8 | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L) 
  AccelY_out = Wire.read() << 8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AccelZ_out = Wire.read() << 8 | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmp = Wire.read() << 8 | Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  GyroX_out = Wire.read() << 8 | Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyroY_out = Wire.read() << 8 | Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyroZ_out = Wire.read() << 8 | Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
  int debugPin = digitalRead(13);
  if (debugPin == 0) {
    delay(1000);
    xR = 0;
    yR = 0;
    zR = 0;
    Serial.println("Variables reset!");
  }

  //the stablization numbers
  AccelX = atan2(AccelY_out, AccelZ_out) * 57.2957795131;
  AccelY = atan2(AccelX_out, AccelZ_out) * 57.2957795131;
  //AccelZ = atan2(AccelX, AccelY): //due to math problems, the z axis isn't avaible as yet. however, two axis (plural) should be fine
  //TAccelX = SAccelX * ((double(tTwo - tOne)) / 1000.0);

  if (GyroX_out >= 0) {
    GyroX = (double(GyroX_out)) * 250 / 32767.0;
  } else if (GyroX_out < 0) {
    GyroX = (double(GyroX_out)) * 250 / 32768.0;
  }
  
  if (GyroY_out >= 0) {
    GyroY = (double(GyroY_out)) * 250 / 32767.0;
  } else if (GyroX_out < 0) {
    GyroY = (double(GyroY_out)) * 250 / 32768.0;
  }
  //GyroX = nGyroX;
  tTwo = millis();
  //SGyroX = GyroX * ((double(tTwo - tOne)) / 1000.0);
  GyroX = GyroX * ((double(tTwo - tOne)) / 1000.0);
  yR += GyroY;
  xR += GyroX;
  yR = (0.50 * yR) + (0.50 * AccelX);
  xR = (0.50 * xR) + (0.50 * AccelX); 
  //if (nAccelX + nAccelY + nAccelZ < 32767) {
    AngleX = (0.97 * ((AngleX + xR) / 2) + 0.03 * (AccelX / 2));
    AngleY = (0.97 * (AngleY + GyroY) + 0.03 * (AccelY / 2));
    AngleZ = (0.97 * (AngleZ + GyroZ) + 0.03 * ( 1/*compass*/));
  //}
  /*
    if (AngleX >= 0) {
    xR = (double(AngleX)) * 250 / 32767.0;
    } else if (AngleX < 0) {
    xR = (double(AngleX)) * 250 / 32768.0;
    }
    //AngleX = 0;
    //AngleX = xR * ((double(tTwo - tOne)) / 1000.0);
  */
  //Serial.println(tTwo - tOne);
  tOne = tTwo;

  //GyroY = GyroY * 250 / 32767.5;
  //GyroZ = GyroZ * 250 / 32767.5;
  Serial.print(AngleX);
  //Serial.print(" ");
  //Serial.print(AngleY);
  //Serial.print(GyroX_out);
  //Serial.print(" ");
  //Serial.print(AccelX);
  //Serial.print(" ");
  //Serial.print(nGyroX);
  //Serial.print(" ");
  //Serial.println(AccelZ);
  //Serial.print(SGyroX);
  Serial.println();

  OldXr = xR;
  delay(10);
}

adaptive filter apha

Arduino
here's a more active version of the complementary filter, making sure that the
/* 
 *  this code is a PROTOTYPE version of the adaptive filter for the MPU-6050 gyroscope unit. 
 *  this is very akin to a complimentary filter, however it uses the gyroscope data to more 
 *  accurately fluctuate between control of the gyroscope, accelerometer, and later compass.
 *  please use, change or edit this code to your own will. HOWEVER: this code has taken a lot
 *  of work and you MUST reference the creater of this code. 
 *  
 *   Oscar Zingle and Flynn Meredith-Black, 2017
 *  
 *  OZTL. this code is released on HACKSTER.IO . please submit changes to hackster to support
 *  the open-sourced community!
 *  
 *  version 0.0.2 (very very alpha stage, not working perfectly)
 */
#include<Wire.h>
#define MPU_addr 0x68
#define delay_value 0.01 //how long the program is delayed each time. please change this value, 
//DO NOT change the delay manually. it will break everything

//variables
int16_t AccelX_out, AccelY_out, AccelZ_out, Tmp, GyroX_out, GyroY_out, GyroZ_out;
double AccelX, AccelY, AccelZ, GyroX, GyroY, GyroZ, AngleX, AngleY, AngleZ, xR, yR, zR;
void setup() {
  Wire.begin();
  Serial.begin(9600);//change depending on the data rate of your arduino.
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);

  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6A);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 1, true);
  Wire.endTransmission(true);
}

void loop() {
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 14, true); // request a total of 14 registers
  AccelX_out = Wire.read() << 8 | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L) 
  AccelY_out = Wire.read() << 8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AccelZ_out = Wire.read() << 8 | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmp = Wire.read() << 8 | Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  GyroX_out = Wire.read() << 8 | Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyroY_out = Wire.read() << 8 | Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyroZ_out = Wire.read() << 8 | Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
  AccelX = atan2(AccelY_out, AccelZ_out) * 57.2957795131;
  AccelY = atan2(AccelX_out, AccelZ_out) * 57.2957795131;
  //Serial.println(AccelX);
  if (GyroX_out >= 0) {
    GyroX = ((double(GyroX_out)) * 250 / 32767.0) * delay_value * 10;
  } else if (GyroX_out < 0) {
    GyroX = ((double(GyroX_out)) * 250 / 32768.0) * delay_value * 10;
  }
  
  if (GyroY_out >= 0) {
    GyroY = ((double(GyroY_out)) * 250 / 32767.0) * delay_value * 10;
  } else if (GyroX_out < 0) {
    GyroY = ((double(GyroY_out)) * 250 / 32768.0) * delay_value * 10;
  }
  xR += GyroX;
  yR += GyroY;
  zR += GyroZ;
  delay(delay_value * 1000);
  if (GyroX > 10) {
    AngleX = (0.97 * ((AngleX + xR) / 2) + 0.03 * (AccelX));
    Serial.print("option 1 ");
  } else if (GyroX <= 10 && GyroX > 5) {
    AngleX = (0.90 * ((AngleX + xR) / 2) + 0.10 * (AccelX));
    Serial.print("option 2 ");
  } else if (GyroX <= 5 && GyroX > 2.5) {
    AngleX = (0.80 * ((AngleX + xR) / 2) + 0.20 * (AccelX));
    Serial.print("option 2 ");
  } else {
    AngleX = (0.03 * ((AngleX + xR) / 2) + 0.97 * (AccelX));
    xR = AccelX;
    Serial.print("option 3 ");
  }
  Serial.println(AngleX);
}
/*This product is meant for educational purposes only. Any resemblance to real persons, living or dead is
purely coincidental. Void where prohibited. Batteries not included.
*/

adaptive filter version 0.8.2

Arduino
the adaptive filter works! yay! it's not fluid, but values are set out. yay!
/* 
 *  this code is a PROTOTYPE version of the adaptive filter for the MPU-6050 gyroscope unit. 
 *  this is very akin to a complimentary filter, however it uses the gyroscope data to more 
 *  accurately fluctuate between control of the gyroscope, accelerometer, and later compass.
 *  please use, change or edit this code to your own will. HOWEVER: this code has taken a lot
 *  of work and you MUST reference the creater of this code. 
 *  
 *   Oscar Zingle and Flynn Meredith-Black, 2017
 *  
 *  OZTL. this code is released on HACKSTER.IO . please submit changes to hackster to support
 *  the open-sourced community!
 *  
 *  version 0.0.2 (very very alpha stage, not working perfectly)
 */
#include<Wire.h>
#define MPU_addr 0x68
#define delay_value 0.01 //how long the program is delayed each time. please change this value, 
//DO NOT change the delay manually. it will break everything

//variables
int16_t AccelX_out, AccelY_out, AccelZ_out, Tmp, GyroX_out, GyroY_out, GyroZ_out;
double AccelX, AccelY, AccelZ, GyroX, GyroY, GyroZ, AngleX, AngleY, AngleZ, xR, yR, zR;
void setup() {
  Wire.begin();
  Serial.begin(9600);//change depending on the data rate of your arduino.
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);

  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6A);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 1, true);
  Wire.endTransmission(true);
}

void loop() {
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 14, true); // request a total of 14 registers
  AccelX_out = Wire.read() << 8 | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L) 
  AccelY_out = Wire.read() << 8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AccelZ_out = Wire.read() << 8 | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmp = Wire.read() << 8 | Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  GyroX_out = Wire.read() << 8 | Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyroY_out = Wire.read() << 8 | Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyroZ_out = Wire.read() << 8 | Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
  AccelX = atan2(AccelY_out, AccelZ_out) * 57.2957795131;
  AccelY = atan2(AccelX_out, AccelZ_out) * 57.2957795131;
  //Serial.println(AccelX);
  if (GyroX_out >= 0) {
    GyroX = ((double(GyroX_out)) * 250 / 32767.0) * delay_value * 10;
  } else if (GyroX_out < 0) {
    GyroX = ((double(GyroX_out)) * 250 / 32768.0) * delay_value * 10;
  }
  
  if (GyroY_out >= 0) {
    GyroY = ((double(GyroY_out)) * 250 / 32767.0) * delay_value * 10;
  } else if (GyroX_out < 0) {
    GyroY = ((double(GyroY_out)) * 250 / 32768.0) * delay_value * 10;
  }
  xR += GyroX;
  yR += GyroY;
  zR += GyroZ;
  delay(delay_value * 1000);
  
  if (AccelX > 10) {
    AngleX = (0.95 * ((AngleX + xR) / 2) + 0.05 * (AccelX));
    //Serial.print("option 1 ");
  } else if (AccelX <= 10 && AccelX > 5) {
    AngleX = (0.65 * ((AngleX + xR) / 2) + 0.35 * (AccelX));
    //Serial.print("option 2 ");
  } else if (AccelX <= 5 && AccelX > 2.5) {
    AngleX = (0.80 * ((AngleX + xR) / 2) + 0.20 * (AccelX));
    //Serial.print("option 2 ");
  } else {
    AngleX = (0.00 * ((AngleX + xR) / 2) + 1.00 * (AccelX));
    xR = AccelX;
    //Serial.print("option 3 ");
  }
  Serial.print(AngleX);
  Serial.print(" ");
  Serial.print(GyroX);
  Serial.print(" ");
  Serial.print(AccelX);
  Serial.println();
}
/*This product is meant for educational purposes only. Any resemblance to real persons, living or dead is
purely coincidental. Void where prohibited. Batteries not included.
*/

Adaptive filter version 1.1.2

Arduino
Yay! its awful.



--changelog--
-NEW! lots of random data to sort through!
-FIXED! the data is relatively smooth
-that's all
/* 
 *  this code is a PROTOTYPE version of the adaptive filter for the MPU-6050 gyroscope unit. 
 *  this is very akin to a complimentary filter, however it uses the gyroscope data to more 
 *  accurately fluctuate between control of the gyroscope, accelerometer, and later compass.
 *  please use, change or edit this code to your own will. HOWEVER: this code has taken a lot
 *  of work and you MUST reference the creater of this code. 
 *  
 *   Oscar Zingle and Flynn Meredith-Black, 2017
 *  
 *  OZTL. this code is released on HACKSTER.IO . please submit changes to hackster to support
 *  the open-sourced community!
 *  
 *  version 1.1.2 (very very alpha stage, not working perfectly)
 */
#include<Wire.h>
#define MPU_addr 0x68
#define delay_value 0.01 //how long the program is delayed each time. please change this value, 
//DO NOT change the delay manually. it will break everything

//variables
int Accel_old;
int16_t AccelX_out, AccelY_out, AccelZ_out, Tmp, GyroX_out, GyroY_out, GyroZ_out;
double AccelX, AccelY, AccelZ, GyroX, GyroY, GyroZ, AngleX, AngleY, AngleZ, xR, yR, zR;
void setup() {
  Wire.begin();
  Serial.begin(9600);//change depending on the data rate of your arduino.
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);

  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6A);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 1, true);
  Wire.endTransmission(true);
}

void loop() {
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 14, true); // request a total of 14 registers
  AccelX_out = Wire.read() << 8 | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L) 
  AccelY_out = Wire.read() << 8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AccelZ_out = Wire.read() << 8 | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmp = Wire.read() << 8 | Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  GyroX_out = Wire.read() << 8 | Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyroY_out = Wire.read() << 8 | Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyroZ_out = Wire.read() << 8 | Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
  AccelX = atan2(AccelY_out, AccelZ_out) * 57.2957795131;
  AccelY = atan2(AccelX_out, AccelZ_out) * 57.2957795131;
  AccelZ = atan2(AccelX_out, AccelY_out) * 57.2957795131;
  //Serial.println(AccelX);
  if (GyroX_out >= 0) {
    GyroX = ((double(GyroX_out)) * 250 / 32767.0) * delay_value * 10;
  } else if (GyroX_out < 0) {
    GyroX = ((double(GyroX_out)) * 250 / 32768.0) * delay_value * 10;
  }
  
  if (GyroY_out >= 0) {
    GyroY = ((double(GyroY_out)) * 250 / 32767.0) * delay_value * 10;
  } else if (GyroX_out < 0) {
    GyroY = ((double(GyroY_out)) * 250 / 32768.0) * delay_value * 10;
  }
  xR += GyroX;
  yR += GyroY;
  zR += GyroZ;
  delay(delay_value * 1000);
  if (((AccelX_out + AccelY_out + AccelZ_out) - Accel_old) < 100 and ((AccelX_out + AccelY_out + AccelZ_out) - Accel_old) > -100) { 
    AngleX = (0.10 * ((AngleX + xR) / 2) + 0.90 * (AccelX));
    xR = AccelX;
  } else {
    
    AngleX = ( 0.90 * ((AngleX + xR) / 2) + 0.10 * (AccelX));
  }
  //AngleX = ( (((AccelX_out + AccelY_out + AccelZ_out) - 16383.75) - Accel_old) * ((AngleX + xR) / 2) + (1 -(((AccelX_out + AccelY_out + AccelZ_out) - 16383.75)) * (AccelX))) ;
  Serial.print(AngleX);
  Serial.print(" ");
  Serial.print(AccelX);
  Serial.print(" ");
  Serial.print(AccelY);
  Serial.print(" ");
  Serial.print(Tmp/340.00+36.53);
  Serial.print(" ");
  Serial.print(GyroX);
  Serial.print(" ");
  Serial.print(GyroY);
  Serial.println();
  Accel_old = (AccelX_out + AccelY_out + AccelZ_out);
}
/*This product is meant for educational purposes only. Any resemblance to real persons, living or dead is
purely coincidental. Void where prohibited. Batteries not included.
*/

Credits

oztechlab
0 projects • 8 followers
Thanks to Arduino team, Unknown, and Pieter-Jan Van.

Comments

Related channels and tags
  • Arduino
  • SparkFun
  • TDK Corporation