// Programa : CHAVETEIRA COM DISPLAY LCD 20X4

#include <Keypad.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

// Inicializa o display no endereco 0x3F ----------------------
// LiquidCrystal_I2C lcd(0x3F, 2, 1, 0, 4, 5, 6, 7, 3, POSITIVE);
LiquidCrystal_I2C lcd(0x27, 20, 4);


// Variaveis --------------------------------------------------
float posAtual_X = 000.00, posAtual_Y = 000.00,
      posFinal_X = 000.00, posFinal_Y = 000.00,
      multiplicador = 0.01,
      avanco = 0.02;

int encoderX = 0,
    encoderY = 0;  // encoders virtuais para contar pulsos do motor

byte recuo = 30;  // numero de pulsos para recuar a ferramenta

byte ultima_direcao_X = 0,  // Armazena ultima direcao do eixo X; 1 = X+ / 2 = X-
  ultima_direcao_Y = 0,     // Armazena ultima direcao do eixo Y; 1 = Y+ / 2 = Y-
  folga_X = 14,             // Armazena valor de backlash no fuso X
  folga_Y = 16;             // Armazena valor de backlash no fuso Y

byte saida = 0,
     dirChaveta = 0,    // 0 = y unilateral; 1 = y bilateral; 2 = x unilateral; 3 = x bilateral
  primeiraPassada = 1,  // usado para controlar 1 passe da chaveta
  modoAutomatico = 0,   // modo Automatico 0 = OFF e 1 = ON
  ultimoPasse = 0,
     FerrAfastada = 0;

volatile byte estadoFerramenta = 2;  // guarda estado da ferramenta 0 = recuada e 1 = avancada

char botao;

const byte Linhas = 4;
const byte Colunas = 4;

int tempoPulso = 1000,  // Tempo em micro segundos do passo do motor na velocidade maxima
  passadaAtual = 0,     // Armazena o numero da passada atual
  qtdDePassadas = 0;    // Armazena a quantidade total de passadas (posicao final / avanço)

const int DIR_MOTOR_Y = 7;     // Pino Saida direcao motor Y
const int PULSO_MOTOR_Y = 8;   // Pino Saida para pulsos motor Y
const int DIR_MOTOR_X = 11;    // Pino Saida direcao motor X
const int PULSO_MOTOR_X = 12;  // Pino Saida para pulsos motor X
const int HORARIO_X = LOW;
const int ANTI_HORARIO_X = HIGH;
const int HORARIO_Y = LOW;
const int ANTI_HORARIO_Y = HIGH;
bool alargaChaveta = false;  // Controle para alargar ou nao a chaveta


byte Pins_Linhas[] = { 38, 40, 42, 44 };   // Pinos Arduino para as linhas.
byte Pins_Colunas[] = { 46, 48, 50, 52 };  // Pinos Arduino para as colunas.

char Teclas[Linhas][Colunas] = {
  { '1', '2', '3', 'A' },
  { '4', '5', '6', 'B' },
  { '7', '8', '9', 'C' },
  { '*', '0', '#', 'D' }
};

Keypad Teclado1 = Keypad(makeKeymap(Teclas), Pins_Linhas, Pins_Colunas, Linhas, Colunas);

//----------------FIM DECLARACOES DE VARIAVEIS-------------------------------------------------------------

//---------------------------------------------------
//------- ROTINA P/ USINAR EM Y ---------------------
//---------------------------------------------------
void usinaY() {
  primeiraPassada = 1;
  ultimoPasse = 0;
  passadaAtual = 0;
  FerrAfastada = 0;
  estadoFerramenta = 2;  // setado em 1 para garantir que a ferramenta nao vai avancar no meio da peca pela 1 vez

  float avancoUsinagem = avanco;  // guarda avanco sem alargar

  if (alargaChaveta)  // aumenta avanco no momento de alargar
  {
    avanco = 0.1;
  }

  qtdDePassadas = (posFinal_Y / avanco);

  bool saida = true;  // controle do loop usinagem

  while (ultimoPasse == 0) {
    //while (passadaAtual < qtdDePassadas)
    while (saida) {
      if (primeiraPassada == 0)  //avanca se a ferr. estiver recuada
      {
        if (digitalRead(3) == 0 && FerrAfastada == 1) {
          //noInterrupts();
          passadaAtual++;

          if (passadaAtual > qtdDePassadas) {
            float diferenca = posFinal_Y - (qtdDePassadas * avanco);
            motor('y', diferenca * -1);
            saida = false;
          } else {
            motor('y', avanco * -1);
            FerrAfastada = 0;
            estadoFerramenta = 2;
            delay(150);
            //interrupts();
          }
        }

        if (FerrAfastada == 0 && digitalRead(2) == 0)  // RECUA se a ferr. estiver avancada
        {
          noInterrupts();
          FerrAfastada = 1;
          primeiraPassada = 0;
          delay(150);
          interrupts();
        }
      }

      if (digitalRead(2) == 0) {
        ultimoPasse = 1;
      }
    }
  }
  avanco = avancoUsinagem;  // retorna para avanco normal de usinagem
}
//----------------------------------------------------------
//---------- FIM ROTINA USINA EM Y -------------------------
//----------------------------------------------------------

//---------------------------------------------------
//------- ROTINA P/ USINAR EM X ---------------------
//---------------------------------------------------
void usinaX() {
  primeiraPassada = 1;
  ultimoPasse = 0;
  passadaAtual = 0;
  qtdDePassadas = (posFinal_X / avanco);
  FerrAfastada = 0;
  estadoFerramenta = 2;  // setado em 1 para garantir que a ferramenta nao vai avancar no meio da peca pela 1 vez
  while (ultimoPasse == 0) {
    while (passadaAtual < qtdDePassadas) {
      if (primeiraPassada == 0)  // avanca se a ferr. estiver recuada
      {
        if (digitalRead(3) == 0 && FerrAfastada == 1) {
          //noInterrupts();
          passadaAtual++;
          motor('x', avanco * -1);
          FerrAfastada = 0;
          estadoFerramenta = 2;
          delay(150);
          //interrupts();
        }
      }

      if (digitalRead(2) == 0 && FerrAfastada == 0) {
        noInterrupts();
        FerrAfastada = 1;
        primeiraPassada = 0;
        delay(150);
        interrupts();
      }
    }

    if (digitalRead(2) == 0)  // Libera o retorno se a ferr. estiver avancada
    {
      ultimoPasse = 1;
    }
  }
}

//----------------------------------------------------------
//-------- ROTINA AUTOMATICO -------------------------------
//----------------------------------------------------------
void Automatico() {
  switch (dirChaveta) {
    case 0:                    // direcao Y - unilateral
      usinaY();                // Chama a rotina para usinar em Y
      motor('y', posFinal_Y);  // Retorna para Y = 0

      //---------- Verifica se precisa alargar -------
      if (posFinal_X > posAtual_X) {
        motor('x', posFinal_X * -1);  // Desloca para usinar largura
        alargaChaveta = true;
        usinaY();  // Usina
        alargaChaveta = false;
        motor('y', posFinal_Y);  // Retorna para Y = 0
        motor('x', posFinal_X);  // Retorna para X = 0
      }
      break;

    case 1:                    // direcao Y - bilateral
      usinaY();                // Chama a rotina para usinar em Y
      motor('y', posFinal_Y);  // Retorna para Y = 0

      //---------- Verifica se precisa alargar -------
      if (posFinal_X > posAtual_X) {
        float metadeLargura = posFinal_X / 2.0;
        motor('x', metadeLargura);  // Alarga metade
        alargaChaveta = true;
        usinaY();  // Usina
        alargaChaveta = false;
        motor('y', posFinal_Y);       // Retorna para Y=0
        motor('x', posFinal_X * -1);  // Usina outra metade
        alargaChaveta = true;
        usinaY();  // Usina
        alargaChaveta = false;
        motor('y', posFinal_Y);     // Retorna para Y = 0
        motor('x', metadeLargura);  // Retorna para X = 0
      }
      break;

    case 2:                    // direcao X - unilateral
      usinaX();                // Chama a rotina para usinar em X
      motor('x', posFinal_X);  // Retorna para X = 0

      //---------- Verifica se precisa alargar -------
      if (posFinal_Y > posAtual_Y) {
        motor('y', posFinal_Y * -1);  // Desloca para usinar largura
        alargaChaveta = true;
        usinaX();  // Repete o processo de usinagem em X
        alargaChaveta = false;
        motor('x', posFinal_X);  // Retorna para X = 0
        motor('y', posFinal_Y);  // Retorna para Y = 0
      }
      break;

    case 3:                    // direcao X - bilateral
      usinaX();                // Chama a rotina para usinar em X
      motor('x', posFinal_X);  // Retorna para X = 0

      //---------- Verifica se precisa alargar -------
      if (posFinal_Y > posAtual_Y) {
        float metadeLargura = posFinal_Y / 2.0;
        motor('y', metadeLargura);  // Alarga metade
        alargaChaveta = true;
        usinaX();  // Usina
        alargaChaveta = false;
        motor('x', posFinal_X);       // Retorna para X = 0
        motor('y', posFinal_Y * -1);  // Usina outra metade
        alargaChaveta = true;
        usinaX();  // Usina
        alargaChaveta = false;
        motor('x', posFinal_X);     // Retorna para X = 0
        motor('y', metadeLargura);  // Retorna para Y = 0
      }
      break;
  }
}
//----------------------------------------------------
//-------FIM VOID AUTOMATICO--------------------------
//----------------------------------------------------

void telaPrincipal() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("X:");
  lcd.print(posAtual_X);
  lcd.setCursor(11, 0);
  lcd.print("Y:");
  lcd.print(posAtual_Y);
  lcd.setCursor(6, 1);
  lcd.print("<<MENU>>");
  lcd.setCursor(0, 2);
  lcd.print("[A]Jog      [C]Auto.");
  lcd.setCursor(0, 3);
  lcd.print("[B]P. Chaveta");
}
//------------------------------------------------
void telaJog() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("X:");
  lcd.print(posAtual_X);
  lcd.setCursor(11, 0);
  lcd.print("Y:");
  lcd.print(posAtual_Y);
  lcd.setCursor(0, 1);
  lcd.print("<<JOG>> mult.= ");
  lcd.print(multiplicador);
  lcd.setCursor(0, 2);
  lcd.print("[2]Y+ [4]X- [5]Zerar");
  lcd.setCursor(0, 3);
  lcd.print("[8]Y- [6]X+ [*]Mult.");
  saida = 0;
  while (saida < 1) {
    botao = Teclado1.getKey();
    if (botao != 0) {
      switch (botao) {
        case '2':  //------ Direcao Y+ -------
          motor('y', multiplicador);
          break;

        case '8':  //------ Direcao Y- -------
          motor('y', multiplicador * -1);
          break;

        case '5':  //------ Zera Cooredenadas --------
          posAtual_X = 000.00;
          posAtual_Y = 000.00;
          atualizaCoordenadas(posAtual_X, posAtual_Y);
          break;

        case '6':  //------ Direcao X+ -------
          motor('x', multiplicador);
          break;

        case '4':  //------ Direcao X- -------
          motor('x', multiplicador * -1);
          break;

        case '*':  //------ Multiplicador X1, X10, X100 -----
          multiplicador *= 10.0;
          if (multiplicador > 10.00) {
            multiplicador = 0.01;
          }
          lcd.setCursor(0, 1);
          lcd.print("<<JOG>> mult.= ");
          lcd.print(multiplicador, 2);
          lcd.setCursor(19, 1);
          lcd.print(" ");
          break;

        case '#':  //------ Volta para Tela principal -----
          saida = 1;
          break;

        default:
          break;
      }
    }
  }
  telaPrincipal();
}
//------------------------------------------------
void paramChaveta() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("[A]P.final X: ");
  lcd.print(posFinal_X);
  lcd.setCursor(0, 1);
  lcd.print("[B]P.final Y: ");
  lcd.print(posFinal_Y);
  lcd.setCursor(0, 2);
  lcd.print("[C]Dir. Chaveta:   ");
  lcd.setCursor(0, 3);
  lcd.print("[D]Avanco: ");
  lcd.print(avanco);

  switch (dirChaveta) {
    case 0:
      lcd.setCursor(0, 2);
      lcd.print("[C]Dir.Y > Unilat.");
      break;
    case 1:
      lcd.setCursor(0, 2);
      lcd.print("[C]Dir.Y > Bilat. ");
      break;
    case 2:
      lcd.setCursor(0, 2);
      lcd.print("[C]Dir.X > Unilat.");
      break;
    case 3:
      lcd.setCursor(0, 2);
      lcd.print("[C]Dir.X > Bilat. ");
      break;
  }

  saida = 0;

  String str = "";

  while (saida < 1) {
    botao = Teclado1.getKey();
    if (botao != 0) {
      switch (botao) {

        case '1':
          {
            str += "1";
            lcd.print("1");
          }
          break;

        case '2':
          {
            str += "2";
            lcd.print("2");
          }
          break;

        case '3':
          {
            str += "3";
            lcd.print("3");
          }
          break;

        case '4':
          {
            str += "4";
            lcd.print("4");
          }
          break;

        case '5':
          {
            str += "5";
            lcd.print("5");
          }
          break;

        case '6':
          {
            str += "6";
            lcd.print("6");
          }
          break;

        case '7':
          {
            str += "7";
            lcd.print("7");
          }
          break;

        case '8':
          {
            str += "8";
            lcd.print("8");
          }
          break;

        case '9':
          {
            str += "9";
            lcd.print("9");
          }
          break;

        case '0':
          {
            str += "0";
            lcd.print("0");
          }
          break;

        case '#':
          {  //------ Saida para tela inicial -----
            saida = 1;
          }
          break;

        case '*':
          {
            str += ".";
            lcd.print(".");
          }
          break;

        case 'A':
          {  //------ Insere valor Posicao Final em X ------
            lcd.setCursor(14, 0);
            lcd.print("      ");
            lcd.setCursor(14, 0);
            lcd.blink();
            if (str != "") {
              lcd.setCursor(14, 0);
              lcd.print("     ");
              lcd.setCursor(14, 0);
              delay(200);
              posFinal_X = str.toFloat();
              lcd.print(posFinal_X);
              lcd.noBlink();
              str = "";
            }
          }
          break;

        case 'B':
          {  //------ Insere valor Posicao Final em Y ------
            lcd.setCursor(14, 1);
            lcd.print("      ");
            lcd.setCursor(14, 1);
            lcd.blink();
            if (str != "") {
              lcd.setCursor(14, 1);
              lcd.print("     ");
              lcd.setCursor(14, 1);
              delay(200);
              posFinal_Y = str.toFloat();
              lcd.print(posFinal_Y);
              lcd.noBlink();
              str = "";
            }
          }
          break;

        case 'C':
          {  //------ Direcao da chaveta e modo de usinagem ------
            dirChaveta++;
            if (dirChaveta > 3)
              dirChaveta = 0;
            switch (dirChaveta) {
              case 0:
                lcd.setCursor(0, 2);
                lcd.print("[C]Dir.Y > Unilat.");
                break;
              case 1:
                lcd.setCursor(0, 2);
                lcd.print("[C]Dir.Y > Bilat. ");
                break;
              case 2:
                lcd.setCursor(0, 2);
                lcd.print("[C]Dir.X > Unilat.");
                break;
              case 3:
                lcd.setCursor(0, 2);
                lcd.print("[C]Dir.X > Bilat. ");
                break;
            }
          }
          break;

        case 'D':
          {  //------ Insere avanco da ferramenta ------
            lcd.setCursor(11, 3);
            lcd.print("      ");
            lcd.setCursor(11, 3);
            lcd.blink();
            if (str != "") {
              lcd.setCursor(11, 3);
              lcd.print("     ");
              lcd.setCursor(11, 3);
              delay(1000);
              avanco = str.toFloat();
              lcd.print(avanco);
              lcd.noBlink();
              str = "";
            }
          }
          break;

        default:

          break;
      }
    }
  }
  lcd.noBlink();
  telaPrincipal();
}
//----------------------------------------------
void telaAuto() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("X:");
  lcd.print(posAtual_X);
  lcd.setCursor(11, 0);
  lcd.print("Y:");
  lcd.print(posAtual_Y);
  lcd.setCursor(0, 1);
  lcd.print("Pos. final X: ");
  lcd.print(posFinal_X, 2);

  lcd.setCursor(0, 2);
  lcd.print("Pos. final Y: ");
  lcd.print(posFinal_Y, 2);
  lcd.setCursor(0, 3);
  lcd.print(" << [A] Iniciar! >>");
  modoAutomatico = 0;
  saida = 0;
  while (saida < 1) {
    botao = Teclado1.getKey();
    if (botao != 0) {
      switch (botao) {
        case '#':  //----- Volta para Tela principal -----
          saida = 1;
          modoAutomatico = 0;
          break;

        case 'A':
          lcd.setCursor(0, 3);
          lcd.print("Avanco:            ");
          lcd.setCursor(8, 3);
          lcd.print(avanco);
          modoAutomatico = 1;
          saida = 1;
          break;
        /*
              case 'B':
                modoAutomatico = 0;
                lcd.setCursor(0, 3);
                lcd.print("Parado! >[A]Continua");
                break;
      */
        default:

          break;
      }
    }
  }
  if (modoAutomatico == 1) {
    Automatico();
  }

  if (modoAutomatico == 1) {
    telaAuto();
  }

  telaPrincipal();
}
//---------------------------------------------------------
void ferramentaAvancada() {
  estadoFerramenta = 1;  // guarda estado da ferramenta 1 = avancada
}
//----------------------------------------------------------
void ferramentaRecuada() {
  estadoFerramenta = 0;  // guarda estado da ferramenta 0 = recuada
}
//-------------------------------------------------------------------
//------------- ROTINA PARA ACIONAR MOTOR X -------------------------
//-------------------------------------------------------------------
// TESTE DE FUNCAO ACIONA MOTOR GENERICA
void motor(char eixo, float distancia) {
  //noInterrupts();
  int pulsos = abs(distancia / 0.01);
  encoderX = 0;
  int Rampa = pulsos / 4;  // Aproximadamente 30% de rampa
  if (Rampa > 250)
    Rampa = 250;
  int tempo = 2700;  // Tempo da rampa em micro segundos
  int fator = ((tempo - tempoPulso) / Rampa) + 1;
  int inicio_Desacel = pulsos - Rampa;  // Define quando desacelerar
  int pin_motor = -1;

  // LOGICA PARA DEFINIR O MOTOR E A DIRECAO
  switch (eixo) {
    case 'x':
      pin_motor = PULSO_MOTOR_X;
      if (distancia > 0) {
        digitalWrite(DIR_MOTOR_X, HORARIO_X);
        delay(5);
        if (ultima_direcao_X == 2)  // x-
          pulsos += folga_X;
        ultima_direcao_X = 1;  // x+
        posAtual_X += distancia;
      } else {
        digitalWrite(DIR_MOTOR_X, ANTI_HORARIO_X);
        delay(5);
        if (ultima_direcao_X == 1)
          pulsos += folga_X;
        ultima_direcao_X = 2;  // x-
        posAtual_X += distancia;
      }
      break;

    case 'y':
      pin_motor = PULSO_MOTOR_Y;
      if (distancia > 0) {
        digitalWrite(DIR_MOTOR_Y, HORARIO_Y);
        delay(5);
        if (ultima_direcao_Y == 2)
          pulsos += folga_Y;
        ultima_direcao_Y = 1;  // y+
        posAtual_Y += distancia;
      } else {
        digitalWrite(DIR_MOTOR_Y, ANTI_HORARIO_Y);
        delay(5);
        if (ultima_direcao_Y == 1)
          pulsos += folga_Y;
        ultima_direcao_Y = 2;  // y-
        posAtual_Y += distancia;
      }
      break;

    default:
      break;
  }

  for (int i = 1; i <= pulsos; i++) {
    digitalWrite(pin_motor, HIGH);
    delayMicroseconds(tempo);
    digitalWrite(pin_motor, LOW);
    delayMicroseconds(tempo);
    encoderX++;

    if (pulsos >= 10) {
      if (encoderX <= Rampa) {
        tempo -= fator;
        if (tempo < tempoPulso) {  // corrige o tempo se for menor que tempoPulso
          tempo = tempoPulso;
        }
      }

      if (encoderX >= inicio_Desacel) {
        tempo += fator;
        if (tempo > 2700) {  // corrige o tempo se for maior que tempoPulso
          tempo = 2700;
        }
      }
    }
  }  // end for
     //interrupts();
  // Atualiza coordenadas no display
  atualizaCoordenadas(posAtual_X, posAtual_Y);
}

void atualizaCoordenadas(float coordX, float coordY) {
  // Atualiza display com nova coordenada
  lcd.setCursor(0, 0);
  lcd.print("X:       ");
  lcd.setCursor(2, 0);
  lcd.print(posAtual_X);
  lcd.setCursor(11, 0);
  lcd.print("Y:       ");
  lcd.setCursor(13, 0);
  lcd.print(posAtual_Y);
}


//-----------------------------------------------------------------------
//------ INICIALIZACAO DO PROGRAMA---------------------------------------
//-----------------------------------------------------------------------
void setup() {
  Serial.begin(9600);
  pinMode(8, OUTPUT);   // Pino como pulsos do motor eixo Y
  pinMode(7, OUTPUT);   // Pino define direcao motor Y
  pinMode(12, OUTPUT);  // Pino como pulsos do motor eixo X
  pinMode(11, OUTPUT);  // Pino define direcao motor X
  pinMode(2, INPUT);    // Pino para interrupcao Sensor Ferramenta Recuada
  pinMode(3, INPUT);    // Pino para interrupcao Sensor Ferramenta Avancada
  pinMode(4, OUTPUT);   // Led ferramenta Recuada
  pinMode(5, OUTPUT);   // Led ferramenta avancada
  // attachInterrupt(1, ferramentaRecuada, LOW);
  // attachInterrupt(0, ferramentaAvancada, LOW);
  lcd.init();
  lcd.backlight();
  telaPrincipal();
}

void loop() {
  botao = Teclado1.getKey();
  if (botao != 0) {
    switch (botao) {
      case 'A':
        telaJog();
        break;
      case 'B':
        paramChaveta();
        break;
      case 'C':
        telaAuto();
        break;

      default:
        // if nothing else matches, do the default
        // default is optional
        break;
    }
  }
}