MPU 6050 Beschleunigungssensor - Programm zu langsam
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:
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
