#include<Wire.h>  //Hinzufügen der Wire.h Library zum kommunizieren mit dem MPU-6050

#include  <MCP342X.h>
MCP342X myADC;
int outputPin = 6;

double GyX, GyXkorr, GyXkorralt=0, K=0, Winkel, t, deltat, n ;
float Ud;

void Calibration(){
  Serial.println("Welle in Stillstand bringen");
  delay(5000);
  Serial.println("Kalibrierung in 2 Sekunden");
  delay(2000);
  Serial.println("Start der Kalibrierung");

  //Sammeln von 1000 Messdaten zum ermitteln einer Durchschnitts Messabweichung
  for(int i=0; i<1000; i++) {
    recordGyroData();
    K += GyX/16.4;
    delay(2);
  }
  K=K/1000;

  Serial.println("Kalibrierung abgeschlossen");
  Serial.print("K = ");
  Serial.println(K);
  delay(2000);
  Winkel=0;
  t=millis();
}

void recordGyroData() {
  Wire.beginTransmission(0b1101000); //Start der Kommunikation mit dem Sensor
  Wire.write(0x43); //Ansprechen des Startregisters für Gyrodaten
  Wire.endTransmission();
  Wire.requestFrom(0b1101000,2); //Anfordern der Gyro Register 43 und 44
  GyX = Wire.read()<<8|Wire.read(); //Speichern der ersten beiden Bytes auf GyX

}

void correctGyroData() {  //erstellen einer eigenen Methode zum Korrigieren, da zum Kalibrieren unkorrigierte Gyrodaten genutzt werden
  GyXkorralt=GyXkorr;
  GyXkorr = GyX / 16.4 - K;    //Korrigieren der Gyro Werte um den Korrekturwert K und teilen durch den Divisor 16.4 welcher durch den Sensor vorgegeben wird
}


void setup(){
  Wire.begin();
  Wire.beginTransmission(0x68); //Start der Kommunikation mit dem Sensor
  Wire.write(0x6B);  // PWR_MGMT_1 Register
  Wire.write(0);     // Aufwecken des Sensors
  Wire.endTransmission(true);
  Serial.begin(9600); //Starten des Serial Monitors
  Wire.beginTransmission(0x68); //I2C address of the MPU  testen ob 0x68 geht   im Notfall das hier als Adresse nehmen 0b1101000
  Wire.write(0x1B); //Ansprechen des Gyo Konfigurations Registers
  Wire.write(0x18); //Setzen des Gyro Sensors auf seine volle Reichweite von 2000°/s
  Wire.endTransmission(true);
  pinMode(A6, INPUT);
  pinMode(A7, INPUT);
  analogReference(INTERNAL);



  TWBR = 12;  // 400 kHz (maximum)

  Serial.begin(9600); // Open serial connection to send info to the host
  while (!Serial) {}  // wait for Serial comms to become ready
  Serial.println("Starting up");
  Serial.println("Testing device connection...");
    Serial.println(myADC.testConnection() ? "MCP342X connection successful" : "MCP342X connection failed");

  myADC.configure( MCP342X_MODE_CONTINUOUS |
                   MCP342X_CHANNEL_1 |
                   MCP342X_SIZE_16BIT |
                   MCP342X_GAIN_1X
                 );
delay(2000);
 // Serial.println(myADC.getConfigRegShdw(), HEX);

  Calibration();
}


void loop(){  //laufende Dauerschleife des Arduinos
  recordGyroData();
  correctGyroData();
  deltat=micros()-t;
  //Integrieren des neuen Winkelwertes
  Winkel += deltat*(GyXkorr+GyXkorralt)/(1000000*2);
  n=GyXkorr/6;    //nmax=331.45 1/min
  t=micros();

//  Drehzahlschutz
  if (abs(n)<330){
   Serial.print(Winkel);
   Serial.print(",");
   Serial.print(n);
   Serial.print(",");


  }
  else {
    Serial.println();
    Serial.println();
    Serial.print("Fehler: Drehzahl > 330 1/min");
    Serial.println();
    Serial.println();
    delay(2000);
    exit(0);
  }

static int16_t  result;

  myADC.startConversion();
  myADC.getResult(&result);

  Serial.println(result, DEC);



}