Near-Perfect Gyroscope

/**
        _
   _  _ \_\  _
  |_|  /_\_\ | |_
          \_\
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.
}

/**
        _
   _  _ \_\  _
  |_|  /_\_\ | |_
          \_\
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.
}

/**
        _
   _  _ \_\  _
  |_|  /_\_\ | |_
          \_\
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.
}

/**
        _
   _  _ \_\  _
  |_|  /_\_\ | |_
          \_\
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.
}

/**
        _
   _  _ \_\  _
  |_|  /_\_\ | |_
          \_\
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.
}

#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);
}

/* 
 *  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.
*/

/* 
 *  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.
*/

/* 
 *  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.
*/