#include <FS.h>
#include <WiFi.h>
#include <SPIFFS.h>
#include <esp_now.h>
#include <LiquidCrystal_I2C.h>

#include "MPU6050.h"
#include "SparkFunMPU9250-DMP.h"

#define FORMAT_SPIFFS_IF_FAILED true

int lcdColumns = 20;
int lcdRows = 4;

MPU9250_DMP imu;
MPU6050 imu6050;

LiquidCrystal_I2C lcd(0x27, lcdColumns, lcdRows);

int side = 0; // 0 = DM, 1 = DP, 2 = TM, 3 = TP
float measuring_angle = 10.0; // 10 ou 20°
int diameter_value = 15;
bool mm = true;

float foreign_toe;
bool aceso = true;
bool laser = true;

// Preencher com endereço da placa que é o par da placa deste código
uint8_t broadcastAddress[] = {0x24, 0x62, 0xAB, 0xDD, 0x96, 0x14};

const int BOTAO1 = 12;
const int BOTAO2 = 14;
const int BOTAO3 = 26;
const int BOTAO4 = 25;
const int BUZZER = 15;
const int LASER = 16;
const int PINPOT = 34;

unsigned long tempo = 0, tempo1 = 0;
unsigned long afk = millis();
unsigned long atimer;
unsigned long tempo_nivel_bateria = 0;
int potValue = 0;

bool bt1_down = false;
bool bt2_down = false;
bool bt3_down = false;
bool bt4_down = false;

enum {
  SPLASH,
  MENU,
  SUBMENU,
  QUICKCONFIGS,
  CASTER,
  TOE,
  TOGGLE_SIDE,
  AXLE,
  CH_ANGLE,
  CALIBRATE,
  CH_DIAMETER,
  RESET_CAL,
  KPIBACK,
  TOGGLE_LASER
};

int old_modo = SPLASH;
int modo = SPLASH;
bool render = true;
int volts = 0;

// Novas definições para as rotinas do MPU6050
unsigned long ultimaEuler = 0;
const long intervaloEuler = 1000; // 1 segundo
unsigned long ultimaYaw = 0; // Tempo da última atualização de yaw

// Variáveis para armazenar as taxas de rotação do giroscópio
float gyroXRate, gyroYRate, gyroZRate;
float yaw = 0.0; // Valor inicial de yaw;

void setup() {
  Serial.begin(115200);
  delay(100);

  // Inicializa o LCD e cria os caracteres
  lcd.init();
  lcd.backlight();
  lcd_create_chars();

  if (!SPIFFS.begin(FORMAT_SPIFFS_IF_FAILED)) {
    Serial.println("Falha ao montar SPIFFS");
    return;
  }

  pinMode(BOTAO1, INPUT_PULLUP);
  pinMode(BOTAO2, INPUT_PULLUP);
  pinMode(BOTAO3, INPUT_PULLUP);
  pinMode(BOTAO4, INPUT_PULLUP);
  pinMode(PINPOT, INPUT);
  pinMode(BUZZER, OUTPUT);
  pinMode(LASER, OUTPUT);

  digitalWrite(BUZZER, LOW);
  digitalWrite(LASER, HIGH);

  Serial.print("MAC: ");
  Serial.println(WiFi.macAddress());

  WiFi.mode(WIFI_STA);

  while (esp_now_init() != ESP_OK) {
    Serial.println("Erro ao inicializar ESP-NOW, tentando novamente");
  }

  esp_now_peer_info_t peerInfo;
  memcpy(peerInfo.peer_addr, broadcastAddress, 6);
  peerInfo.channel = 0;
  peerInfo.encrypt = false;

  while (esp_now_add_peer(&peerInfo) != ESP_OK) {
    Serial.println("Falha ao adicionar par, tentando novamente");
  }

  esp_now_register_recv_cb(OnDataRecv);

  readFile(SPIFFS);

  if (imu.begin() != INV_SUCCESS) {
    Serial.println("Incapaz de comunicar com MPU-9250");
    Serial.println("Verifique as conexões e tente novamente.");
    Serial.println();
    delay(5000);
  }

  imu6050.initialize();

  if (!imu6050.testConnection()) {
    Serial.println("Conexão com MPU6050 falhou");
    return;
  }

  imu.dmpBegin(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_GYRO_CAL, 10);
  ultimaYaw = millis(); // Inicializa o tempo da última atualização de yaw
}

void loop() {
  update_nivel_bateria();
  update_sensors();
  screen_mode();
  euler();
}

void update_nivel_bateria() {
  potValue = analogRead(PINPOT); // Lê o valor analógico do pino do potenciômetro
  volts = (potValue * 100.0) / 4095.0; // Converte o valor lido para porcentagem (0 a 100%)

  // Atualiza e imprime o nível da bateria a cada 10 segundos
  if (millis() - tempo_nivel_bateria >= 10000) { // Verifica se passaram 10 segundos
    tempo_nivel_bateria = millis(); // Atualiza o tempo para a próxima verificação

    // Imprime o valor bruto lido do potenciômetro
    Serial.print("Valor Bruto do Potenciômetro: ");
    Serial.println(potValue);

    // Imprime o nível estimado da bateria em porcentagem
    Serial.print("Nível estimado da bateria: ");
    Serial.print(volts);
    Serial.println("%");
  }
}

// Adicionado rotinas para utilização do MPU6050

void euler() {
  unsigned long tempoAtual = millis();

  // Verifica se passou 1 segundo desde a última atualização
  if (tempoAtual - ultimaEuler >= intervaloEuler) {
    ultimaEuler = tempoAtual; // Atualiza o tempo da última leitura

    float anguloRoll, anguloPitch;
    getAngulosEuler(&anguloRoll, &anguloPitch); // Obtém anguloRoll e anguloPitch

    // Offsets para calibração
    float offsetRoll = 0.0;
    float offsetPitch = 0.0;

    // Aplica offsets
    float rollCorrigido = anguloRoll - offsetRoll;
    float pitchCorrigido = anguloPitch - offsetPitch;

    // Imprime valores corrigidos
    Serial.print("Inclinação lateral: ");
    Serial.print(rollCorrigido);
    Serial.println(" °");
    Serial.print("Inclinação frente/atrás: ");
    Serial.print(pitchCorrigido);
    Serial.println(" °");
    Serial.print("Direção Atual (Yaw): ");
    Serial.print(yaw);
    Serial.println(" °");
  }
}

void getAngulosEuler(float* anguloRoll, float* anguloPitch) {
  int16_t ax, ay, az, gx, gy, gz;
  imu6050.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); // Lê dados do sensor

  // Atualiza as taxas de rotação
  gyroXRate = gx / 131.0;
  gyroYRate = gy / 131.0;
  gyroZRate = gz / 131.0;

  // Calculando o delta de tempo desde a última atualização
  unsigned long agora = millis();
  float deltaTime = (agora - ultimaYaw) / 1000.0f; // Tempo em segundos
  ultimaYaw = agora; // Atualiza o tempo para a próxima iteração

  // Atualiza o yaw com base na taxa de rotação em torno do eixo Z
  yaw += gyroZRate * deltaTime;

  // Correção para manter o yaw dentro de -180 a 180 graus
  if (yaw > 180) yaw -= 360;
  else if (yaw < -180) yaw += 360;

  // Converte valores para a escala de gravidade da Terra
  float axEscalado = (float)ax / 16384.0;
  float ayEscalado = (float)ay / 16384.0;
  float azEscalado = (float)az / 16384.0;

  // Calcula anguloRoll e anguloPitch
  *anguloRoll = atan2(ayEscalado, azEscalado) * RAD_TO_DEG;
  *anguloPitch = atan2(-axEscalado, sqrt(ayEscalado * ayEscalado + azEscalado * azEscalado)) * RAD_TO_DEG;
}
$abcdeabcde151015202530354045505560fghijfghij
Loading
esp32-devkit-c-v4