#include <Wire.h>  //Bibliotecas necessárias.
#include <MPU6050.h>
#include <SPI.h>  
#include <SD.h>

const int CS = 4;
File dataFile;            // Objeto responsável por escrever/Ler do cartão SD
bool cartaoOk = true;

MPU6050 mpu1(0x68); //Declaração do módulo.
MPU6050 mpu2(0x69);

int16_t ax, ay, az, gx, gy, gz; 
int16_t ax2, ay2, az2, gx2, gy2, gz2;//Declaração das variáveis. A int16_t é uma 
                                //variável int sem sinal de 16 bits. 

//Variáveis com 'a' são eixos do acelerômetro e 'g' são do giroscópio.
void setup() {
  Serial.begin(9600);  //Inicializa a tela Serial.

  pinMode(CS, OUTPUT);
  delay(100);
  Serial.print("Inicializando cartao SD...");
  if (!SD.begin(CS)) 
  {
    Serial.println("Falha na Inicializacao!");
    return;
  }
  Serial.println("Inicializacao terminada");

  Wire.begin();  //Inicializa a comunicação I2C.
  mpu1.initialize(); //Inicializa o módulo.
  mpu1.setFullScaleAccelRange('MPU6050_ACCEL_FS_2'); // mudança na escala de medição
  mpu2.initialize();
  mpu2.setFullScaleAccelRange('MPU6050_ACCEL_FS_2'); // escala de -4G a +4G
  //Serial.print(mpu1.getFullScaleAccelRange());
  //Serial.print(mpu2.getFullScaleAccelRange());
  if(!mpu1.testConnection()){ 
    while (1); 
    }
  if(!mpu1.testConnection()){ 
    while (1); 
  }
}

void loop() {
  mpu1.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //Função que faz a leitura 
  mpu2.getMotion6(&ax2, &ay2, &az2, &gx2, &gy2, &gz2);
  String leitura = "";
                                //dos eixos do acelerômetro e do giroscópio.
  if (cartaoOk){
      dataFile = SD.open("datalog.csv", FILE_WRITE); 
      Serial.println("Cartão SD Inicializado para escrita :D ");
  }

  leitura = String(millis()) + ";" + converter_a(ax) + ";" + converter_a(ay) + ";" + converter_a(az) + ";" + converter_g(gx) + ";" + converter_g(gy) + ";" + converter_g(gz) + ";" + converter_a(ax2) + ";" + converter_a(ay2) + ";" + converter_a(az2) + ";" + converter_g(gx2) + ";" + converter_g(gy2) + ";" + converter_g(gz2); 

  if (dataFile) {   
    Serial.println(leitura);    // Mostramos no monitor a linha que será escrita
    dataFile.println(leitura);  // Escrevemos no arquivos e pulamos uma linha
    dataFile.close();           // Fechamos o arquivo
  }
  delay(500); //Para poder ler os dados na tela.
}

String converter_a(int valor_digital){
  float valor;
  valor = float(valor_digital)*0.000244140625;
  return String(valor);
}

String converter_g(int valor_digital){
  float valor;
  valor = float(valor_digital)*0.00763358778625954198473282442748;
  return String(valor);
}