// Projeto: Controle de Robô com ESC, DRV8833 e Bluetooth
// Plataforma: Arduino Nano
// Autor: ChatGPT (OpenAI) + Usuário

//#include <SoftwareSerial.h>

//#include <ServoTimer2.h>
#include <Servo.h>
#include "neotimer.h"
#include <TimerFreeTone.h>

// Pinos de comunicação Bluetooth
#define Rx 2
#define Tx 3

// Controle de motores (DRV8833)
#define In1 5
#define In2 6
#define In3 9
#define In4 10

// Buzina e ESC
#define Buzina 11
#define ESC A0

// Sensor de vibração
#define WSensor A1

// Bluetooth e ESC
//SoftwareSerial Bt(Rx, Tx);
//ServoTimer2 Esc;
Servo Esc;
// Temporizadores
Neotimer Delay0 = Neotimer(100);    // Ciclos de aumento da serra
Neotimer Delay1 = Neotimer(3000);   // Tempo mínimo desligado após sensor
Neotimer Delay2 = Neotimer(300);    // Debounce de comandos

// Variáveis de controle
String Text = "";       //cria textos a partir dos caracteres recebidos por Crct
char Crct = "";         //guarda os caracteres
int Nivel[] = {95, 95}; //X e Y
int Pw = 1000;          //valor de
int Max_Pw = 2000;      //valor maximo que define a velocidade
int Pw_Gain = 10;       //valor de ganho de velocidade
bool Ats = false;       //inicializador da Serra
bool DY = false;
bool DX = false;
bool cmm = false;

// Setup inicial
void setup() {
  //inicia a comunicação dentre dispositivos
  Serial.begin(115200);
 // Bt.begin(115200);

  //definição de portas
  pinMode(Buzina, OUTPUT);
  pinMode(WSensor, INPUT);

  pinMode(In1, OUTPUT);
  pinMode(In2, OUTPUT);
  pinMode(In3, OUTPUT);
  pinMode(In4, OUTPUT);

  Esc.attach(ESC);
  armarESC();

  //Serial.println("Sistema iniciado");
}

// Loop principal
void loop() {
  Bluetooth();
  Mc();
  Serra(); //menu iniciar serra de corte
  if (Ats == true) { //verifica estado da serra
    if (DX == true){
    ligarSerra();
    }
  } else {
    //if (DY == true){
    desligarSerra(); //desligar serra de corte
    //}
  }
}

// Armar ESC no início
void armarESC() {
  Esc.writeMicroseconds(2000); //valor maximo para calibrar o Esc
  delay(2500);
  Esc.writeMicroseconds(1000); //valor minimo para concluir a calibração e manter o motor armado
  delay(2000);
}

// Liga a serra
void ligarSerra() {
  Pw = 1000;
  DY = true; //valida a função DY para iniciar o aumento de rotações do motor
  DX = false;
  Ats = true;
//  Serial.println("Serra ligada.");
}

// Desliga a serra
void desligarSerra() {
  Pw = 1000;
  DY = false;
  DX = true;
  Ats = false;
  Esc.writeMicroseconds(Pw);
//  Serial.println("Serra Desligada.");
}

// Controle da serra com sensor de segurança
void Serra() {
  if (Delay1.repeat(1)) { //valida a função DY para iniciar o aumento de rotações do motor
    DY = true;
  }

  if (Pw <= Max_Pw && DY == true) { //aumenta as rotações do motor da serra
    if (Delay0.repeat()) {
      Pw += Pw_Gain;
      if (Pw >= Max_Pw) {
        Pw = Max_Pw;
        DY = false; //invalida DY para a parar o aumento de rotações do motor
      }
    }
  }

  Esc.writeMicroseconds(Pw); //envia todos os valores para o esc

//  if (digitalRead(WSensor) == HIGH && Pw >= 1500) {
//    Serial.println("Sensor de vibração ativo! Desligando serra...");
  //  Delay1.repeatReset();
  //  DY = false;
  //  Pw = 1000;
  //  Esc.writeMicroseconds(Pw);
  }
//}

// Comunicação Bluetooth
void Bluetooth() {
  if (Serial.available()) {
    Crct = Serial.read();

    if (Crct >= 32 && Crct <= 126) {
      Text += Crct;

      if (Text == "A") {
        if (cmm == true) {
          Ats = !Ats;
          cmm = false;
          Delay2.repeatReset();
        }
        Text = "";
      } else if (Text == "W") {
        if (cmm == true) {
       // Serial.println("Buzina acionada");
        // NewTone(Buzina, 391);
        TimerFreeTone(Buzina, 1567, 500);
        //motor direito
        digitalWrite(In2, LOW);
        digitalWrite(In1, LOW);
        //motor esquerdo
        digitalWrite(In4, LOW);
        digitalWrite(In3, LOW);
        Text = "";
        cmm = false;
        Delay2.repeatReset();
      }
      } else {
       if(Delay2.repeat(1)){
        cmm = true;
        }
      }
      if (Text.endsWith("X") || Text.endsWith("Y")) {
        char eixo = Text.charAt(Text.length() - 1);
        int val = Text.substring(0, Text.length() - 1).toInt();
        if (val >= 0 && val <= 180) {
          if (eixo == 'X') {
            Nivel[0] = val;
        //    Serial.print("X: "); Serial.println(val);
          } else {
            Nivel[1] = val;
         //   Serial.print("Y: "); Serial.println(val);
          }
        }
        Text = "";
      }

      if (Text.length() > 6) Text = "";
    }
  }
}

// Controle de direção com sinais digitais (sem PWM)
void Mc() {
  /* Função de movimento dos motores
     contorle por potenciometro de 0 a 180
     Nivel[] recebe os valores e guarda em (X, Y).
     X define a direção e o quanto o robô conseguirar virar para tal direção
     Y Define a velocidade e se o robô deverá seguir para frente ou para Tras
     110 - 180X define a Esquerda
        80 - 0X define a Direita
     110 - 180Y faz ir para Frente
        80 - 0Y faz ir para Tras
     90 - 100X/Y centro - nada acontece
  */


  if (Nivel[1] >= 150 && Nivel[0] >= 150) { //frente + esquerda
    //motor direito
    digitalWrite(In1, LOW); //define o quanto a velocidade aumenta para ir para frente
    digitalWrite(In2, LOW);
    //motor esquerdo

    digitalWrite(In3, HIGH);
    digitalWrite(In4, LOW);

  } else if (Nivel[1] >= 150 && Nivel[0] <= 60) { //frente + direita
    //motor direito
    digitalWrite(In1, HIGH);
    digitalWrite(In2, LOW);
    //motor esquerdo
    digitalWrite(In3, LOW); //define o quanto a velocidade aumenta para ir para frente
    digitalWrite(In4, LOW);
  } else if (Nivel[1] <= 60 && Nivel[0] >= 150) { //ré + esquerda
    //motor direito
    digitalWrite(In2, LOW); 
    digitalWrite(In1, HIGH);
    //motor esquerdo

    digitalWrite(In4, LOW);
    digitalWrite(In3, LOW); //define o quanto a velocidade aumenta para ir para tras

  } else if (Nivel[1] <= 60 && Nivel[0] <= 60) { //ré + direita arrumar
    //motor direito

    digitalWrite(In2, LOW);
    digitalWrite(In1, HIGH); //define o quanto a velocidade aumenta para ir para tras
    //motor esquerdo
    digitalWrite(In4, LOW);
    digitalWrite(In3, LOW);
  } else if (Nivel[1] >= 150) { //ré
    //motor direito
    digitalWrite(In1, HIGH);
    digitalWrite(In2, LOW);
    //motor esquerdo
    digitalWrite(In3, HIGH);
    digitalWrite(In4, LOW);
  } else if (Nivel[1] <= 60) { //frente
    //motor direito
    digitalWrite(In1, LOW);
    digitalWrite(In2, HIGH);
    //motor esquerdo
    digitalWrite(In3, LOW);
    digitalWrite(In4, HIGH);
  } else if (Nivel[0] >= 150) { //esquerda

    //motor direito
    digitalWrite(In1, LOW);
    digitalWrite(In2, HIGH);
    //motor esquerdo
    digitalWrite(In3, HIGH);
    digitalWrite(In4, LOW);
  } else if (Nivel[0] <= 60) { //direita

    //motor direito
    digitalWrite(In1, HIGH);
    digitalWrite(In2, LOW);
    //motor esquerdo
    digitalWrite(In3, LOW);
    digitalWrite(In4, HIGH);
  } else {
    //motor direito
    digitalWrite(In2, LOW);
    digitalWrite(In1, LOW);
    //motor esquerdo
    digitalWrite(In4, LOW);
    digitalWrite(In3, LOW);
  }

}