/*
  Projeto didático para uso exclusivo em aulas de robótica.
  Criado por Claudio Roberto da Silva.
  © 2024 - Todos os direitos reservados. Proibida a reprodução parcial ou total sem a autorização expressa do autor.
  
  Projeto: Controle de Braço Robótico com Joysticks e Display OLED
  
  Descrição:
  Este projeto permite o controle de um braço robótico composto por quatro micro servos (base, garra, braço esquerdo e braço direito) usando dois joysticks analógicos. 
  Além do controle, o projeto apresenta uma interface de exibição com um display OLED, onde o usuário pode alternar entre quatro telas que mostram as informações de cada servo (ângulo) 
  e o valor correspondente dos potenciômetros dos joysticks. A alternância de telas é controlada por um dos botões presentes nos joysticks, enquanto o outro botão retorna os servos suavemente 
  à posição inicial de 90 graus.

  Funções dos Botões:
  - Botão A: Retorna os quatro servos à posição inicial de 90 graus de forma suave.
  - Botão B: Alterna entre as quatro telas no display OLED, cada tela mostrando o ângulo de um servo e o valor do potenciômetro correspondente.

  Controles dos Joysticks:
  - Joystick A: 
      - Eixo Horizontal (X): Controla o giro da base.
      - Eixo Vertical (Y): Controla o movimento do braço esquerdo (elevação).
  - Joystick B: 
      - Eixo Horizontal (X): Controla a abertura/fechamento da garra.
      - Eixo Vertical (Y): Controla o movimento do braço direito (avanço/recuo).
      
  Implementações adicionais:
  - Zona neutra configurada para suavizar os movimentos dos servos.
  - Filtro de Kalman aplicado no controle do servo da base para suavizar o comportamento da base, reduzindo tremores.
  - Interface gráfica remodelada para o display OLED, com retângulos demarcando o ângulo do servo e o valor do potenciômetro.
*/

#include <Servo.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

// Definição do display OLED
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET    -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// Definição dos servos
Servo baseServo, garraServo, esquerdaServo, direitaServo;

// Pinos dos joysticks e pushbuttons
const int joystickXA = A0; // Eixo horizontal do Joystick A (base)
const int joystickYA = A1; // Eixo vertical do Joystick A (eleva garra)
const int joystickXB = A2; // Eixo horizontal do Joystick B (garra)
const int joystickYB = A3; // Eixo vertical do Joystick B (avança garra)

const int buttonA = 3;  // Botão do Joystick A
const int buttonB = 2;  // Botão do Joystick B

// Pinos dos servos
const int servoBasePin = 9;
const int servoGarraPin = 12;
const int servoEsquerdaPin = 11;
const int servoDireitaPin = 10;

// Zona neutra para os joysticks
const int ZONA_NEUTRA_MIN = 450;
const int ZONA_NEUTRA_MAX = 550;

// Variáveis para armazenar os ângulos dos servos
int anguloBase = 90, anguloGarra = 90, anguloEsquerda = 90, anguloDireita = 90;

// Variáveis do filtro de Kalman (somente para o servo da base)
float base_estimated = 90.0;
float base_estimation_error = 1.0;
float base_process_noise = 0.1;
float base_measurement_noise = 1.0;

// Variáveis para o display
int telaAtual = 0; // Controla a tela que está sendo exibida

// Função para inicializar o display
void inicializarDisplay() {
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("Falha ao inicializar o display OLED"));
    for (;;);
  }
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
}

// Função para inicializar os servos
void inicializarServos() {
  baseServo.attach(servoBasePin);
  garraServo.attach(servoGarraPin);
  esquerdaServo.attach(servoEsquerdaPin);
  direitaServo.attach(servoDireitaPin);

  baseServo.write(anguloBase);
  garraServo.write(anguloGarra);
  esquerdaServo.write(anguloEsquerda);
  direitaServo.write(anguloDireita);
}

// Função de filtro de Kalman para o servo da base
float filtroKalman(float leitura_base) {
  base_estimated = base_estimated;
  base_estimation_error += base_process_noise;

  float kalman_gain = base_estimation_error / (base_estimation_error + base_measurement_noise);
  base_estimated = base_estimated + kalman_gain * (leitura_base - base_estimated);
  base_estimation_error = (1 - kalman_gain) * base_estimation_error;

  return base_estimated;
}

// Função para ler os valores dos joysticks e controlar os servos
void controlarServos() {
  int leituraXA = analogRead(joystickXA);
  int leituraYA = analogRead(joystickYA);
  int leituraXB = analogRead(joystickXB);
  int leituraYB = analogRead(joystickYB);

  if (leituraXA < ZONA_NEUTRA_MIN) {
    anguloBase = max(0, anguloBase - 1);
  } else if (leituraXA > ZONA_NEUTRA_MAX) {
    anguloBase = min(180, anguloBase + 1);
  }
  
  float leitura_base_filtrada = filtroKalman(anguloBase);
  baseServo.write(leitura_base_filtrada);

  if (leituraYA < ZONA_NEUTRA_MIN) {
    anguloEsquerda = max(0, anguloEsquerda - 1);
  } else if (leituraYA > ZONA_NEUTRA_MAX) {
    anguloEsquerda = min(180, anguloEsquerda + 1);
  }
  esquerdaServo.write(anguloEsquerda);

  if (leituraXB < ZONA_NEUTRA_MIN) {
    anguloGarra = max(0, anguloGarra - 1);
  } else if (leituraXB > ZONA_NEUTRA_MAX) {
    anguloGarra = min(180, anguloGarra + 1);
  }
  garraServo.write(anguloGarra);

  if (leituraYB < ZONA_NEUTRA_MIN) {
    anguloDireita = max(0, anguloDireita - 1);
  } else if (leituraYB > ZONA_NEUTRA_MAX) {
    anguloDireita = min(180, anguloDireita + 1);
  }
  direitaServo.write(anguloDireita);
}

// Função para retornar os servos suavemente à posição inicial
void retornarPosicaoInicial() {
  while (anguloBase != 90 || anguloGarra != 90 || anguloEsquerda != 90 || anguloDireita != 90) {
    if (anguloBase > 90) anguloBase--;
    else if (anguloBase < 90) anguloBase++;
    baseServo.write(anguloBase);

    if (anguloGarra > 90) anguloGarra--;
    else if (anguloGarra < 90) anguloGarra++;
    garraServo.write(anguloGarra);

    if (anguloEsquerda > 90) anguloEsquerda--;
    else if (anguloEsquerda < 90) anguloEsquerda++;
    esquerdaServo.write(anguloEsquerda);

    if (anguloDireita > 90) anguloDireita--;
    else if (anguloDireita < 90) anguloDireita++;
    direitaServo.write(anguloDireita);

    delay(20);  // Controla a suavidade do movimento
  }
}

// Função para alternar entre as telas do display
void alternarTela() {
  telaAtual = (telaAtual + 1) % 4;
}

// Função para exibir as diferentes telas no display OLED com layout customizado
void exibirTela() {
  display.clearDisplay();

  // Desenha o título no topo da tela
  display.drawRect(0, 0, SCREEN_WIDTH, 16, SSD1306_WHITE);
  display.setCursor(30, 4);
  
  switch (telaAtual) {
    case 0:
      display.print(F("Servo Base"));
      break;
      
    case 1:
      display.print(F("Servo Garra"));
      break;

    case 2:
      display.print(F("Servo Esquerda"));
      break;

    case 3:
      display.print(F("Servo Direita"));
      break;
  }

  // Desenha as duas áreas lado a lado para o servo e o potenciômetro
  display.drawRect(0, 20, 60, 40, SSD1306_WHITE); // Área do servo
  display.drawRect(64, 20, 60, 40, SSD1306_WHITE); // Área do potenciômetro

  // Exibe os valores dentro dos retângulos
  display.setCursor(5, 30);
  display.print(F("SERVO:"));
  
  display.setCursor(5, 40);
  switch (telaAtual) {
    case 0:
      display.print(anguloBase);
      break;
      
    case 1:
      display.print(anguloGarra);
      break;

    case 2:
      display.print(anguloEsquerda);
      break;

    case 3:
      display.print(anguloDireita);
      break;
  }

  display.setCursor(70, 30);
  display.print(F("POT:"));
  display.setCursor(70, 40);
  
  // Exibe os valores dos potenciômetros
  switch (telaAtual) {
    case 0:
      display.print(analogRead(joystickXA));
      break;
      
    case 1:
      display.print(analogRead(joystickXB));
      break;

    case 2:
      display.print(analogRead(joystickYA));
      break;

    case 3:
      display.print(analogRead(joystickYB));
      break;
  }

  display.display();
}

// Setup do Arduino
void setup() {
  Serial.begin(9600);
  inicializarDisplay();
  inicializarServos();

  pinMode(buttonA, INPUT_PULLUP);
  pinMode(buttonB, INPUT_PULLUP);

  exibirTela();
}

// Loop principal
void loop() {
  controlarServos();

  if (digitalRead(buttonA) == LOW) {
    retornarPosicaoInicial();
  }

  if (digitalRead(buttonB) == LOW) {
    alternarTela();
    delay(200); // Debounce
  }

  exibirTela();
}