84 lines
2.2 KiB
C++
84 lines
2.2 KiB
C++
#include <Arduino.h>
|
|
#include <Wire.h>
|
|
|
|
//Direccion I2C de la IMU
|
|
#define MPU 0x68
|
|
|
|
//Ratios de conversion
|
|
#define A_R 16384.0 // 32768/2
|
|
#define G_R 131.0 // 32768/250
|
|
|
|
//Conversion de radianes a grados 180/PI
|
|
#define RAD_A_DEG = 57.295779
|
|
|
|
//MPU-6050 da los valores en enteros de 16 bits
|
|
//Valores RAW
|
|
int16_t AcX, AcY, AcZ, GyX, GyY, GyZ;
|
|
|
|
//Angulos
|
|
float Acc[2];
|
|
float Gy[3];
|
|
float Angle[3];
|
|
|
|
String valores;
|
|
|
|
long tiempo_prev;
|
|
float dt;
|
|
|
|
void setup()
|
|
{
|
|
Wire.begin(D2,D1); // D2(GPIO4)=SDA / D1(GPIO5)=SCL
|
|
Wire.beginTransmission(MPU);
|
|
Wire.write(0x6B);
|
|
Wire.write(0);
|
|
Wire.endTransmission(true);
|
|
Serial.begin(115200);
|
|
}
|
|
|
|
void loop()
|
|
{
|
|
//Leer los valores del Acelerometro de la IMU
|
|
Wire.beginTransmission(MPU);
|
|
Wire.write(0x3B); //Pedir el registro 0x3B - corresponde al AcX
|
|
Wire.endTransmission(false);
|
|
Wire.requestFrom(MPU,6); //A partir del 0x3B, se piden 6 registros
|
|
AcX=Wire.read()<<8|Wire.read(); //Cada valor ocupa 2 registros
|
|
AcY=Wire.read()<<8|Wire.read();
|
|
AcZ=Wire.read()<<8|Wire.read();
|
|
|
|
//A partir de los valores del acelerometro, se calculan los angulos Y, X
|
|
//respectivamente, con la formula de la tangente.
|
|
Acc[1] = atan(-1*(AcX/A_R)/sqrt(pow((AcY/A_R),2) + pow((AcZ/A_R),2)))*RAD_TO_DEG;
|
|
Acc[0] = atan((AcY/A_R)/sqrt(pow((AcX/A_R),2) + pow((AcZ/A_R),2)))*RAD_TO_DEG;
|
|
|
|
//Leer los valores del Giroscopio
|
|
Wire.beginTransmission(MPU);
|
|
Wire.write(0x43);
|
|
Wire.endTransmission(false);
|
|
Wire.requestFrom(MPU,6); //A partir del 0x43, se piden 6 registros
|
|
GyX=Wire.read()<<8|Wire.read(); //Cada valor ocupa 2 registros
|
|
GyY=Wire.read()<<8|Wire.read();
|
|
GyZ=Wire.read()<<8|Wire.read();
|
|
|
|
//Calculo del angulo del Giroscopio
|
|
Gy[0] = GyX/G_R;
|
|
Gy[1] = GyY/G_R;
|
|
Gy[2] = GyZ/G_R;
|
|
|
|
dt = (millis() - tiempo_prev) / 1000.0;
|
|
tiempo_prev = millis();
|
|
|
|
//Aplicar el Filtro Complementario
|
|
Angle[0] = 0.98 *(Angle[0]+Gy[0]*dt) + 0.02*Acc[0];
|
|
Angle[1] = 0.98 *(Angle[1]+Gy[1]*dt) + 0.02*Acc[1];
|
|
|
|
//Integración respecto del tiempo paras calcular el YAW
|
|
Angle[2] = Angle[2]+Gy[2]*dt;
|
|
|
|
//Mostrar los valores por consola
|
|
valores = "90, " +String(Angle[0]) + "," + String(Angle[1]) + "," + String(Angle[2]) + ", -90";
|
|
Serial.println(valores);
|
|
|
|
delay(10);
|
|
}
|