Advertisement
Guest User

Untitled

a guest
Jan 30th, 2021
114
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include<Wire.h>  //Hinzufügen der Wire.h Library zum kommunizieren mit dem MPU-6050
  2.  
  3. #include  <MCP342X.h>
  4. MCP342X myADC;
  5. int outputPin = 6;
  6.  
  7. double GyX, GyXkorr, GyXkorralt=0, K=0, Winkel, t, deltat, n ;
  8. float Ud;
  9.  
  10. void Calibration(){
  11.   Serial.println("Welle in Stillstand bringen");
  12.   delay(5000);
  13.   Serial.println("Kalibrierung in 2 Sekunden");
  14.   delay(2000);
  15.   Serial.println("Start der Kalibrierung");
  16.  
  17.   //Sammeln von 1000 Messdaten zum ermitteln einer Durchschnitts Messabweichung
  18.   for(int i=0; i<1000; i++) {
  19.     recordGyroData();    
  20.     K += GyX/16.4;
  21.     delay(2);
  22.   }
  23.   K=K/1000;
  24.  
  25.   Serial.println("Kalibrierung abgeschlossen");
  26.   Serial.print("K = ");
  27.   Serial.println(K);
  28.   delay(2000);
  29.   Winkel=0;
  30.   t=millis();
  31. }
  32.  
  33. void recordGyroData() {
  34.   Wire.beginTransmission(0b1101000); //Start der Kommunikation mit dem Sensor
  35.   Wire.write(0x43); //Ansprechen des Startregisters für Gyrodaten
  36.   Wire.endTransmission();
  37.   Wire.requestFrom(0b1101000,2); //Anfordern der Gyro Register 43 und 44
  38.   GyX = Wire.read()<<8|Wire.read(); //Speichern der ersten beiden Bytes auf GyX
  39.  
  40. }
  41.  
  42. void correctGyroData() {  //erstellen einer eigenen Methode zum Korrigieren, da zum Kalibrieren unkorrigierte Gyrodaten genutzt werden
  43.   GyXkorralt=GyXkorr;
  44.   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
  45. }
  46.  
  47.  
  48. void setup(){
  49.   Wire.begin();
  50.   Wire.beginTransmission(0x68); //Start der Kommunikation mit dem Sensor
  51.   Wire.write(0x6B);  // PWR_MGMT_1 Register
  52.   Wire.write(0);     // Aufwecken des Sensors
  53.   Wire.endTransmission(true);
  54.   Serial.begin(9600); //Starten des Serial Monitors
  55.   Wire.beginTransmission(0x68); //I2C address of the MPU  testen ob 0x68 geht   im Notfall das hier als Adresse nehmen 0b1101000
  56.   Wire.write(0x1B); //Ansprechen des Gyo Konfigurations Registers
  57.   Wire.write(0x18); //Setzen des Gyro Sensors auf seine volle Reichweite von 2000°/s
  58.   Wire.endTransmission(true);  
  59.   pinMode(A6, INPUT);
  60.   pinMode(A7, INPUT);
  61.   analogReference(INTERNAL);
  62.  
  63.  
  64.  
  65.   TWBR = 12;  // 400 kHz (maximum)
  66.  
  67.   Serial.begin(9600); // Open serial connection to send info to the host
  68.   while (!Serial) {}  // wait for Serial comms to become ready
  69.   Serial.println("Starting up");
  70.   Serial.println("Testing device connection...");
  71.     Serial.println(myADC.testConnection() ? "MCP342X connection successful" : "MCP342X connection failed");
  72.    
  73.   myADC.configure( MCP342X_MODE_CONTINUOUS |
  74.                    MCP342X_CHANNEL_1 |
  75.                    MCP342X_SIZE_16BIT |
  76.                    MCP342X_GAIN_1X
  77.                  );
  78. delay(2000);
  79.  // Serial.println(myADC.getConfigRegShdw(), HEX);
  80.  
  81.   Calibration();
  82. }
  83.  
  84.  
  85. void loop(){  //laufende Dauerschleife des Arduinos
  86.   recordGyroData();
  87.   correctGyroData();
  88.   deltat=micros()-t;
  89.   //Integrieren des neuen Winkelwertes
  90.   Winkel += deltat*(GyXkorr+GyXkorralt)/(1000000*2);
  91.   n=GyXkorr/6;    //nmax=331.45 1/min
  92.   t=micros();
  93.  
  94. //  Drehzahlschutz
  95.   if (abs(n)<330){
  96.    Serial.print(Winkel);
  97.    Serial.print(",");
  98.    Serial.print(n);
  99.    Serial.print(",");
  100.  
  101.  
  102.   }
  103.   else {
  104.     Serial.println();
  105.     Serial.println();
  106.     Serial.print("Fehler: Drehzahl > 330 1/min");
  107.     Serial.println();
  108.     Serial.println();
  109.     delay(2000);
  110.     exit(0);  
  111.   }
  112.  
  113. static int16_t  result;
  114.  
  115.   myADC.startConversion();
  116.   myADC.getResult(&result);
  117.  
  118.   Serial.println(result, DEC);
  119.  
  120.  
  121.  
  122. }
Advertisement
RAW Paste Data Copied
Advertisement