MPU 6050 Beschleunigungssensor - Programm zu langsam

[Atvilar]

Hallo,

Ich bin dabei einen Datalogger zu bauen, der aktuelle Zeit, Temperatur(en), Ausrichtung gegenüber Startpunkt und Beschleunigungen auf eine SD Karte speichern soll.
Als Prozessor verwende ich einen Arduino Uno, Der Gyro ist ein IMU6050.
Das Demoprogramm funktioniert super.

Nach einer Implementierung in mein Programm stellte ich fest, dass das Programm nur sehr langsame Winkeländerungen richtig wahrnimmt.
Mein Gedanke ist, dass mein Programm so langsam ist, dass die IMU verhältnismäßig kurz angesprochen wird. Während der anderen Zeit gehen dann Lageänderungen verloren.
Da die Kiste aber dazu gedacht ist, Stöße und Würfe zu loggen, ist das ein Problem.

Ich habe versucht ein Programm eines Balancer - Roboters zu verfolgen, aber ohne Erfolg.

hier mein Code:

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// PRE SETUP

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// LIBRARIES

//////////////////////////////////////////////////////////////////////////////////////////////////// EXTERNAL TEMP SENSOR

#include "Wire.h"
#include "OneWire.h"
#include "DallasTemperature.h"

//////////////////////////////////////////////////////////////////////////////////////////////////// GYROSCOPE

#include <Wire.h>
#include <MPU6050.h>

//////////////////////////////////////////////////////////////////////////////////////////////////// SD CARD READER

#include <SPI.h>
#include <SD.h>

//////////////////////////////////////////////////////////////////////////////////////////////////// Clock

#include <virtuabotixRTC.h> //Library used

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// DEFINE I0

//////////////////////////////////////////////////////////////////////////////////////////////////// EXTERNAL TEMP SENSOR

#define ONE_WIRE_BUS_1 A0
OneWire ourWire1(ONE_WIRE_BUS_1);
DallasTemperature T_ext(&ourWire1);

//////////////////////////////////////////////////////////////////////////////////////////////////// GYROSCOPE

MPU6050 mpu;

//////////////////////////////////////////////////////////////////////////////////////////////////// SD CARD READER

File myFile;

//////////////////////////////////////////////////////////////////////////////////////////////////// Clock

virtuabotixRTC myRTC(6, 7, 8); //If you change the wiring change the pins here also

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// DEFINE VARIABLES

//////////////////////////////////////////////////////////////////////////////////////////////////// EXTERNAL TEMP SENSOR

float T_ext_RawValue =0;

float T_ext_RawHigh = 98.4;          // Rohwert gemessen bei 100°C
float T_ext_RawLow = -0.4;           // Rohwert gemessen bei 0°C
float T_ext_ReferenceHigh = 98.3;    // Sollwert bei 100°C
float T_ext_ReferenceLow = 0;        // Sollwert bei 0°C

//////////////////////////////////////////////////////////////////////////////////////////////////// INTERNAL TEMP SENSOR (GYRO)

float T_int_RawValue =0;

float T_int_RawHigh = 100;          // Rohwert gemessen bei 100°C                                 Soll: 28,4 ..... Ist: 31,45
float T_int_RawLow = 0;           // Rohwert gemessen bei 0°C
float T_int_ReferenceHigh = 100;    // Sollwert bei 100°C
float T_int_ReferenceLow = 0;        // Sollwert bei 0°C

//////////////////////////////////////////////////////////////////////////////////////////////////// POSITION (GYRO)

// Timers
unsigned long timer = 0;
float timeStep = 0.01;

// Pitch, Roll and Yaw values
float pitch = 0;    // y
float roll = 0;     // x
float yaw = 0;      // z







/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// PRE CALCULATION

//////////////////////////////////////////////////////////////////////////////////////////////////// EXTERNAL TEMP SENSOR

float T_ext_RawRange = T_ext_RawHigh - T_ext_RawLow;
float T_ext_ReferenceRange = T_ext_ReferenceHigh - T_ext_ReferenceLow;

//////////////////////////////////////////////////////////////////////////////////////////////////// INTERNAL TEMP SENSOR (GYRO)

float T_int_RawRange = T_int_RawHigh - T_int_RawLow;
float T_int_ReferenceRange = T_int_ReferenceHigh - T_int_ReferenceLow;





/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// SETUP

void setup() {

//////////////////////////////////////////////////////////////////////////////////////////////////// SERIAL PORT

  delay(1000);
  Serial.begin(9600);
  Serial.println("Serial port etablished");

//////////////////////////////////////////////////////////////////////////////////////////////////// EXTERNAL TEMP SENSOR

  T_ext.begin();
  Serial.println("External temperature sensor set up");

//////////////////////////////////////////////////////////////////////////////////////////////////// GYROSCOPE

  Serial.println("Initialize MPU6050");

  while(!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G))
  {
    Serial.println("Could not find a valid MPU6050 sensor, check wiring!");
    delay(500);
  }

  // Calibrate gyroscope. The calibration must be at rest.
  // If you don't want calibrate, comment this line.
  mpu.calibrateGyro();

  // Set threshold sensivty. Default 3.
  // If you don't want use threshold, comment this line or set 0.
  mpu.setThreshold(3);

//////////////////////////////////////////////////////////////////////////////////////////////////// SD CARD READER

Serial.print("Initializing SD card...");
 
  if (!SD.begin(4)) {
    Serial.println("initialization failed!");
    while (1);
  }
  Serial.println("initialization done.");
 
  // open the file. note that only one file can be open at a time,
  // so you have to close this one before opening another.
  myFile = SD.open("Log.txt", FILE_WRITE);
  // close the file:
  myFile.close();

//////////////////////////////////////////////////////////////////////////////////////////////////// PRINT HEADER TO SERIAL PORT

  Serial.println("  Date   /   Time   /  T EXT   /   T INT   /  Rot X  /  Rot Y  /  Rot Z  /  Acc X  /  Acc Y");

}

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// LOOP

void loop() {

//////////////////////////////////////////////////////////////////////////////////////////////////// EXTERNAL TEMP SENSOR

  T_ext.requestTemperatures();
  float T_ext_RawValue = T_ext.getTempCByIndex(0);

  float T_ext_CorrectedValue = (((T_ext_RawValue - T_ext_RawLow) * T_ext_ReferenceRange) / T_ext_RawRange) + T_ext_ReferenceLow;

//////////////////////////////////////////////////////////////////////////////////////////////////// INTERNAL TEMP SENSOR (GYRO)

  float T_int_RawValue = mpu.readTemperature();
  float T_int_CorrectedValue = (((T_int_RawValue - T_int_RawLow) * T_int_ReferenceRange) / T_int_RawRange) + T_int_ReferenceLow;

//////////////////////////////////////////////////////////////////////////////////////////////////// POSITION (GYRO)

  timer = millis();

  // Read normalized values
  Vector norm = mpu.readNormalizeGyro();

  // Calculate Pitch, Roll and Yaw
  pitch = pitch + norm.YAxis * timeStep;
  roll = roll + norm.XAxis * timeStep;
  yaw = yaw + norm.ZAxis * timeStep;

  // Wait to full timeStep period
  delay((timeStep*1000) - (millis() - timer));

//////////////////////////////////////////////////////////////////////////////////////////////////// ACCELERATION (GYRO)

  // Read normalized values 
  Vector normAccel = mpu.readNormalizeAccel();

  // Calculate Pitch & Roll
  int acc_pitch = -(atan2(normAccel.XAxis, sqrt(normAccel.YAxis*normAccel.YAxis + normAccel.ZAxis*normAccel.ZAxis))*180.0)/M_PI;
  int acc_roll = (atan2(normAccel.YAxis, normAccel.ZAxis)*180.0)/M_PI;

//////////////////////////////////////////////////////////////////////////////////////////////////// Clock

  myRTC.updateTime();
  
//////////////////////////////////////////////////////////////////////////////////////////////////// PRINT TO SERIAL PORT

  Serial.print(myRTC.dayofmonth);
  Serial.print(".");
  Serial.print(myRTC.month);
  Serial.print(".");
  Serial.print(myRTC.year);
  Serial.print(" / ");
  Serial.print(myRTC.hours);
  Serial.print(":");
  Serial.print(myRTC.minutes);
  Serial.print(":");
  Serial.print(myRTC.seconds);
  Serial.print(" / ");
  Serial.print(T_ext_CorrectedValue, 1);
  Serial.print("°C  /  ");
  Serial.print(T_int_CorrectedValue);
  Serial.print("°C  /  ");
  Serial.print(roll);
  Serial.print("°  /  ");
  Serial.print(pitch);
  Serial.print("°  /  ");
  Serial.print(yaw);
  Serial.print("°  /    ");
  Serial.print(acc_roll);
  Serial.print("g  /    ");
  Serial.print(acc_pitch);
  Serial.print("g  /    ");

  
  
  Serial.println(" ");

//////////////////////////////////////////////////////////////////////////////////////////////////// PRINT TO SD CARD

myFile = SD.open("Log.txt", FILE_WRITE);
 
  // if the file opened okay, write to it:
  if (myFile) {
    myFile.print(myRTC.dayofmonth);
    myFile.print(".");
    myFile.print(myRTC.month);
    myFile.print(".");
    myFile.print(myRTC.year);
    myFile.print(" ");
    myFile.print(myRTC.hours);
    myFile.print(":");
    myFile.print(myRTC.minutes);
    myFile.print(":");
    myFile.print(myRTC.seconds);
    myFile.print(" ");
    myFile.print(T_ext_CorrectedValue, 1);
    myFile.print(" ");
    myFile.print(T_int_CorrectedValue);
    myFile.print(" ");
    myFile.print(roll);
    myFile.print(" ");
    myFile.print(pitch);
    myFile.print(" ");
    myFile.print(yaw);
    myFile.print(" ");
    myFile.print(acc_roll);
    myFile.print(" ");
    myFile.print(acc_pitch);
    myFile.print(" ");
    

    myFile.println(" ");
    // close the file:
    myFile.close();
  } else {
    // if the file didn't open, print an error:
    Serial.println("error opening Log.txt");
  }



}

Die übertriebenen Comments sind für mich nur eine Lesehilfe, die kommen beim fertigen Code natürlich raus.
Habt ihr eine Idee, wie ich der IMU belastbare Werte entlocken kann?

Vielen Dank,
Atvilar