Hoi Reto,
Ich habe bei der Formel "nur" eine Variable für den Winkel eingesetzt, wie in meinem Formelbeispiel. Ich füge mal unten den Code an dem ich gearbeitet habe an. Hoffe es hilft Dir weiter! Achja, ich denke der Begriff/Variabel Abtastfrequenz hab ich mal da mir dieser Ausdruck nicht gepasst hat durch Abtastrate ersetzt. Im Skript ist dt die Variabel für diese Abtastrate und ich verwende den Wert 0,02, was bedeutet, das die Werte circa 50 mal pro Sekunde (50 Hz) abgerufen werden. Ausserdem nimmt mein Skript erst vor der Ausgabe den Vergleich zwischen alten und neuen Winkelwerten vor. Denke mir aber man kann es auch wie in Deiner Formel/Gleichung lösen.
Hoffe ich hab nichts vergessen...
MfG
Patric Hausammann
Code:
//MPU-6050 mit Komplementärfilter und drei Servos
//Erstellt mit hilfe der Servo-, Wire-, I2Cdev-, und der MPU-6050-Library durch Patric Hausammann release vom 10.07.2014. Veröffentlicht unter GNU-License.
#include <Servo.h> // Alle Librarys aufrufen
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#define dt 0.01 //Abtastrate für Komplementärfilter
MPU6050 mpu;
int16_t ax, ay, az; // In der Form erhalten wir die Accelerometer-Werte
int16_t gx, gy, gz; //In der Form erhalten wir die Gyro-Daten
int correctureX; //Meine Variable, welche für den Winkel beim Komplementärfilter steht(X-Achse).
int correctureY; //Meine Variable, welche für den Winkel beim Komplementärfilter steht(Y-Achse).
int correctureZ; //Meine Variable, welche für den Winkel beim Komplementärfilter steht(Z-Achse).
Servo myservo; //Servo-Objekt erzeugen
Servo myservo1;
Servo myservo2;
int val; //Korrigierter resp. gemapter Wert des Winkels
int prevVal; //Letzter bzw. vorheriger korrigierter resp. gemapter Wert des Winkels
int val1;
int prevVal1;
int val2;
int prevVal2;
void setup()
{
Wire.begin();
Serial.begin(115200);
Serial.println("Initialize MPU");
mpu.initialize();
Serial.println(mpu.testConnection() ? "Connected" : "Connection failed");
myservo.attach(A1, 1000, 2000); // Hier wird das Servo-Objekt auf A1 gelegt, die Signale,
myservo1.attach(A2, 1000, 2000); //die wir erzeugen sind Servosteuersignale, deren Werte von 1000 bis 2000 reichen,
myservo2.attach(A3, 1000, 2000); //Mittelstellung des Servos liegt bei 1500
}
void loop()
{
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); // Hier lesen wir die Werte der MPU-6050 ein.
correctureX = 0.98*(correctureX+gx*dt)+0.02*(ax); //Dies ist die Formel die ich zur Korrektur einsetze (Komplementärfilter: Winkel=0.98*(Winkel+GyroDaten*Abtastrate)+0.02*(AccelerometerDaten), wobei ich diesen Vorgang auf allen drei Achsen wiederhole.
correctureY = 0.98*(correctureY+gy*dt)+0.02*(ay);
correctureZ = 0.98*(correctureZ+gz*dt)+0.02*(az);
val = map(correctureX, -32768, 32767, 0, 180); //Hier passe ich die erhaltenen Werte (360 Grad) auf die möglichen Werte der Servoausgabe (180 Grad)an(auch jede Achse einzeln).
val1 = map(correctureY, -32768, 32767, 0, 180);
val2 = map(correctureZ, -32768, 32767, 0, 180);
if (val != prevVal) //Vergleich des Aktuellen mit dem vorherigen Wert
{
myservo.write(val); //Gibt den korrigierten Wert an den Servo aus
prevVal = val;}
if(val1 !=prevVal1)
{
myservo1.write(val1);
prevVal1 = val1;}
if(val2 !=prevVal2){
myservo2.write(val2);
prevVal2 = val2;
Serial.println("val"); //Beschriftung der serielle Ausgabe unseres korrigierten Wertes
Serial.println(val); //Serielle Ausgabe unseres korrigierten Wertes
Serial.println("val1");
Serial.println(val1);
Serial.println("val2");
Serial.println(val2);
}
}
