#include <Wire.h>
#include <Adafruit_SSD1306.h>
//direcciones MPU
#define MPU_ADDR 0x68
#define PWR_MGMT_1 0x6B
#define WHO_AM_I 0x75
#define ACCEL_XOUT_H 0x3B
#define SMPLRT_DIV 0x19
#define CONFIG 0x1A
#define GYRO_CONFIG 0x1B
#define ACCEL_CONFIG 0x1C
//variables MPU
float aRes = 2.0/32768.0;
float gRes = 250.0/32768.0;
float accelBias[3], gyroBias[3];
int16_t aRaw[3], gRaw[3];
float aSc[3], gSc[3];
long tiempo_prev, dt;
float a_angX, a_angY;
float g_angX, g_angY, g_angX_prev, g_angY_prev;
//Display settings
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
#define OLED_RESET -1 // Reset pin # (or -1 if sharing reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
void writeByte(uint8_t address, uint8_t subAddress, uint8_t data){
Wire.beginTransmission(address);
Wire.write(subAddress);
Wire.write(data);
Wire.endTransmission(true);
}
uint8_t readByte(uint8_t address, uint8_t subAddress){
Wire.beginTransmission(address);
Wire.write(subAddress);
Wire.endTransmission(false);
Wire.requestFrom(address, 1, true);
return Wire.read();
}
void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest){
Wire.beginTransmission(address);
Wire.write(subAddress);
Wire.endTransmission(false);
uint8_t i = 0;
Wire.requestFrom(address, count);
while(Wire.available()){
dest[i++] = Wire.read();
}
}
void initMPU(){
/*Inicia el sensor,
selecciona como reloj interno un PLL
con referencia del eje x del girsocopio*/
writeByte(MPU_ADDR,PWR_MGMT_1,0x01);
/*Ajusta la velocidad de muestreo del MPU a 200Hz*/
writeByte(MPU_ADDR,SMPLRT_DIV,0x04);
/*Ajusta el filtro pasa bajos del giroscopio
y del acelerómetro a 42Hz y 44Hz, respectivamente*/
writeByte(MPU_ADDR,CONFIG,0x03);
/*Ajusta el rango de medición del giroscopio a +/- 250°/s*/
writeByte(MPU_ADDR,GYRO_CONFIG,0x00);
/*Ajusta el rango de sensor del acelerómetro a +/- 2g*/
writeByte(MPU_ADDR,ACCEL_CONFIG,0x00);
}
void calibrateMPU(float* accel_bias, float* gyro_bias){
uint8_t data[14];
int nSamples = 40;
for(int i=0; i<nSamples; i++){
int16_t accel_temp[3] = {0,0,0}, gyro_temp[3] = {0,0,0};
readBytes(MPU_ADDR, ACCEL_XOUT_H, 14, data);
// Form signed 16-bit integer for each sample
accel_temp[0] = data[0] << 8 | data[1];
accel_temp[1] = data[2] << 8 | data[3];
accel_temp[2] = data[4] << 8 | data[5];
gyro_temp[0] = data[8] << 8 | data[9];
gyro_temp[1] = data[10] << 8 | data[11];
gyro_temp[2] = data[12] << 8 | data[13];
//Suma de las mediciones
accel_bias[0] += accel_temp[0];
accel_bias[1] += accel_temp[1];
accel_bias[2] += accel_temp[2];
gyro_bias[0] += gyro_temp[0];
gyro_bias[1] += gyro_temp[1];
gyro_bias[2] += gyro_temp[2];
}
//Promedio de las mediciones
accel_bias[0] /= nSamples;
accel_bias[1] /= nSamples;
accel_bias[2] /= nSamples;
gyro_bias[0] /= nSamples;
gyro_bias[1] /= nSamples;
gyro_bias[2] /= nSamples;
if(accel_bias[2]>0L){accel_bias[2] -= (int32_t)16384;}
else{accel_bias[2] += (int32_t)16384;}
}
uint8_t readWhoAmI(){
uint8_t id = readByte(MPU_ADDR, WHO_AM_I);
return id;
}
void readData() {
uint8_t data[14];
readBytes(MPU_ADDR, ACCEL_XOUT_H, 14, data);
//0x3B (ACCEL_XOUT_H)|0x3C (ACCEL_XOUT_L)
aRaw[0] = data[0] << 8 | data[1];
//0x3D (ACCEL_YOUT_H)|0x3E (ACCEL_YOUT_L)
aRaw[1] = data[2] << 8 | data[3];
//0x3F (ACCEL_ZOUT_H)|0x40 (ACCEL_ZOUT_L)
aRaw[2] = data[4] << 8 | data[5];
//0x43 (GYRO_XOUT_H)|0x44 (GYRO_XOUT_L)
gRaw[0] = data[8] << 8 | data[9];
//0x45 (GYRO_YOUT_H)|0x46 (GYRO_YOUT_L)
gRaw[1] = data[10] << 8 | data[11];
//0x47 (GYRO_ZOUT_H)|0x48 (GYRO_ZOUT_L)
gRaw[2] = data[12] << 8 | data[13];
}
void displayWhoAmI(uint8_t id){
display.clearDisplay();
display.setTextColor(WHITE);
//display.startscrollright(0x00, 0x0F);
display.setTextSize(1);
display.setCursor(0,0);
display.print("i am 0x");
display.println(id, HEX);
switch(id){
case 0x68:
display.print("MPU 6000/6050/9150");
break;
case 0x70:
display.print("MPU 6500");
break;
case 0x71:
display.print("MPU 9250");
break;
case 0x73:
display.print("MPU 9255");
break;
}
display.display();
}
void displayData(int16_t Acc[], int16_t Gyr[]){
display.clearDisplay();
display.setCursor(0,0);
display.println("Acelerometro ");
display.print(Acc[0]);
display.print(", ");
display.print(Acc[1]);
display.print(", ");
display.println(Acc[2]);
display.println("Giroscopio ");
display.print(Gyr[0]);
display.print(", ");
display.print(Gyr[1]);
display.print(", ");
display.println(Gyr[2]);
display.display();
}
void displayData(float Acc[], float Gyr[]){
display.clearDisplay();
display.setCursor(0,0);
display.println("Acelerometro ");
display.print(Acc[0]);
display.print(", ");
display.print(Acc[1]);
display.print(", ");
display.println(Acc[2]);
display.println("Giroscopio ");
display.print(Gyr[0]);
display.print(", ");
display.print(Gyr[1]);
display.print(", ");
display.println(Gyr[2]);
display.display();
}
void displayData(float Acc[], float Gyr[], float angX, float angY){
display.clearDisplay();
display.setCursor(0,0);
display.println("Acelerometro ");
display.print(Acc[0]);
display.print(", ");
display.print(Acc[1]);
display.print(", ");
display.println(Acc[2]);
display.println("Giroscopio ");
display.print(Gyr[0]);
display.print(", ");
display.print(Gyr[1]);
display.print(", ");
display.println(Gyr[2]);
display.println("Rotacion en x: ");
display.println(angX);
display.println("Rotacion en y: ");
display.println(angY);
display.display();
}
void setup() {
Serial.begin(115200);
/*Setup I2C*/
Wire.begin();
/*Initialize sensor*/
initMPU();
/*Initialize Display*/
display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
/*Identify Sensor*/
uint8_t id = readWhoAmI();
displayWhoAmI(id);
delay(2000);
calibrateMPU(accelBias, gyroBias);
}
void loop() {
dt = millis()-tiempo_prev;
if(dt >= 5)
{
readData();
aSc[0] = (aRaw[0] - accelBias[0])*aRes;
aSc[1] = (aRaw[1] - accelBias[1])*aRes;
aSc[2] = (aRaw[2] - accelBias[2])*aRes;
gSc[0] = (gRaw[0] - gyroBias[0])*gRes;
gSc[1] = (gRaw[1] - gyroBias[1])*gRes;
gSc[2] = (gRaw[2] - gyroBias[2])*gRes;
a_angX = atan(aSc[0]/sqrt(pow(aSc[1],2) + pow(aSc[2],2)))*(180.0/3.1416);
a_angY = atan(aSc[1]/sqrt(pow(aSc[0],2) + pow(aSc[2],2)))*(180.0/3.1416);
g_angX = 0.98*(gSc[0]*dt/1000 + g_angX_prev) + 0.02*a_angX;
g_angY = 0.98*(gSc[1]*dt/1000 + g_angY_prev) + 0.02*a_angY;
g_angX_prev = g_angX;
g_angY_prev = g_angY;
displayData(aSc,gSc,g_angX,g_angY);
tiempo_prev = millis();
}
}