Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- /*RECUERDO QUE NO MODIFIQUEN LOS CALCULOS NI LA DECODIFICACION DEL ACELEROMETRO PUES SU RESULTADO PUEDE FALLAR*/
- //PUEDEN AÑADIR LAS LIBRERIAS QUE NECESITEN SIEMPRE Y CUANDO NO BORREN LA LIBRERIA WIRE
- #include <Wire.h>
- /////////////////////DECLARACION DE VARIABLES////////////////////////////
- //Direccion I2C de la IMU
- #define MPU 0x68 //0x68 es el numero del acelerometro
- //Ratios de conversion
- #define A_R 16384.0
- #define G_R 131.0
- //Conversion de radianes a grados 180/PI
- #define RAD_A_DEG = 57.295779
- //MPU-6050 da los valores en enteros de 16 bits
- //Valores sin refinar
- int16_t AcX, AcY, AcZ, GyX, GyY, GyZ;
- //Angulos
- float Acc[2];
- float Gy[2];
- float Angle[2];
- float AngleOffset[2];
- ///////////////////DECLARACION VARIABLES DE CODIGOS///////////////////////
- // LO DEJARÉ EN BLANCO PARA QUE PUEDAN MODIFICARLO A SU GUSTO (O NECESIDAD)
- ////////////////////////////////////////////////////////////////////////
- void setup()
- {///////////////////////////////DECLARACION DE TODO LO DEL ACELEROMETRO////////////////////////////////
- Wire.begin();
- Wire.beginTransmission(MPU);
- Wire.write(0x6B);
- Wire.write(0);
- Wire.endTransmission(true);
- Serial.begin(9600);
- // AngleOffset[0]=-81;
- // AngleOffset[1]=2;
- Serial.print("AngleOffset X: "); Serial.print(AngleOffset[0]); Serial.print("\n");
- Serial.print("AngleOffset Y: "); Serial.print(AngleOffset[1]); Serial.print("\n------------\n");
- ///////////////////////////////DECLARACION DE PINES DE CODIGOS//////////////////////////////////////
- // LO DEJARÉ EN BLANCO PARA QUE PUEDAN MODIFICARLO A SU GUSTO (O NECESIDAD)
- }
- void loop()
- {
- ///////////////////////////CALCULOS Y FUNCIONAMIENTO DEL ACELEROMETRO//////////////////////////////
- //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,true); //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,4,true); //A diferencia del Acelerometro, solo se piden 4 registros
- GyX=Wire.read()<<8|Wire.read();
- GyY=Wire.read()<<8|Wire.read();
- //Calculo del angulo del Giroscopio
- Gy[0] = GyX/G_R;
- Gy[1] = GyY/G_R;
- //Aplicar el Filtro Complementario
- Angle[0] = 0.98 *(Angle[0]+Gy[0]*0.010) + 0.02*Acc[0];
- Angle[1] = 0.98 *(Angle[1]+Gy[1]*0.010) + 0.02*Acc[1];
- //Mostrar los valores por consola
- Serial.print("Angle X: "); Serial.print(Angle[0]); Serial.print("\n");
- Serial.print("Angle Y: "); Serial.print(Angle[1]); Serial.print("\n------------\n");
- //Angle[0] = Angle[0] + AngleOffset[0];
- //Angle[1] = Angle[1] + AngleOffset[1];
- // Serial.print("AngleCalibrated X: "); Serial.print(Angle[0]+AngleOffset[0]); Serial.print("\n");
- // Serial.print("AngleCalibrated Y: "); Serial.print(Angle[1]+AngleOffset[1]); Serial.print("\n------------\n");
- delay(10); //Nuestra dt sera, pues, 0.010, que es el intervalo de tiempo en cada medida
- ////////////////////////////////////////////////////CONDICIONES/////////////////////////////////////////////////
- // LO DEJARÉ EN BLANCO PARA QUE PUEDAN MODIFICARLO A SU GUSTO (O NECESIDAD)
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement