#include <Wire.h>
float RateRoll,RatePitch,RateYaw;

float RateCalibrationRoll,RateCalibrationPitch,RateCalibrationYaw; // calibration values variables
int RateCalibrationNumber;

void gyro_signals(void){
Wire.beginTransmission(0x68); // i2c address or mpu
Wire.write(0x1A); //REGISTER 26 CONFIGURATION 
Wire.write(0X05); //switch on LOW PASS FILTER WITH CUTOFF OF 10Hz
Wire.endTransmission();

Wire.beginTransmission(0x68); // i2c address or mpu
Wire.write(0x1B); //set sensitivity scale factor
Wire.write(0X8); //switch on LOW PASS FILTER WITH CUTOFF OF 10Hz
Wire.endTransmission();

Wire.beginTransmission(0x68); // i2c address or mpu
Wire.write(0x43); //ACCESS REG STORING GYRO MEASUREMENTS INDICATES FIRST REG TO BE USED
Wire.endTransmission();

Wire.requestFrom(0x68,6); //request 6 bytes from mpu
int16_t GyroX = Wire.read()<<8 | Wire.read(); // read gyro measurements around x
int16_t GyroY = Wire.read()<<8 | Wire.read(); // Y
int16_t GyroZ = Wire.read()<<8 | Wire.read(); //Z

RateRoll = (float)GyroX/65.5; // convert FROM LSB to degress per second   from selected lsb in reg 0x1B
RatePitch = (float)GyroY/65.5;  
RateYaw = (float)GyroZ/65.5;

}
void setup() {
  // put your setup code here, to run once:
Serial.begin(57600);
pinMode(13, OUTPUT);
digitalWrite(13, HIGH);

Wire.setClock(400000);  //set i2c clock speed
Wire.begin();
delay(250); //time to start

Wire.beginTransmission(0x68); //ACTIVATE MPU6050 AND CONTINUE IN POWER MODE
Wire.write(0x6B);  //POWER MANAGEMENT REGISTER 
Wire.write(0x00);  //   EVEN TEMP SWITCH OFF
Wire.endTransmission();
//hold breadboad stationary first 2 seconds for calibration.
for(RateCalibrationNumber=0;RateCalibrationNumber<2000;RateCalibrationNumber++)
      {gyro_signals();
        RateCalibrationRoll+= RateRoll;//perfoming calibration measurements
        RateCalibrationPitch+= RatePitch;   //add all measured values to calibration variables 
        RateCalibrationYaw+= RateYaw;        
        delay(1);                           //take 2000 gyro measurements 1ms after the other (total time taken is 2seconds)
          }
RateCalibrationRoll/=2000;
RateCalibrationPitch/=2000;   // calculate calibration values devide sum of all the variables by 2000 
RateCalibrationYaw/=2000;
}

void loop() {
  // put your main code here, to run repeatedly:
gyro_signals(); // call defined function 
RateRoll -= RateCalibrationRoll;     //correct measured values
RatePitch -= RateCalibrationPitch;
RateYaw -= RateCalibrationYaw;
 Serial.print("Roll[`/s]= "); 
 Serial.print(RateRoll);
 Serial.print("Pitch[`/s]= ");
 Serial.print(RatePitch);
 Serial.print("Yaw[`/s]= ");
 Serial.print(RateYaw);
 Serial.println(" ");
 delay(2000);//50ms
}