//Desenvolvido por: Eng. Allan Henrique Zanella Hank
#include <LiquidCrystal.h> //Biblioteca do display 16X2
#include <Servo.h>
//----------------Config. display e servomotores-------------------------------
  LiquidCrystal tela(13,12,11,10,8,7);//RS,E,D4,D5,D6,D7
  Servo ombro;
  Servo cotovelo;
  Servo base;
  Servo pulso;
//------------------Pinos joystick e potenciômetro-----------------------------
  const int Jx = A1;
  const int Jy = A0;
  const int Base = A2;
//-------------------Variáveis-------------------------------------------------
  int angulo_base;
  const float L1 = 10; //10cm
  const float L2 = 10; //10cm
  float x = 8; // Posição inicial de X
  float y = 15; //Posição inicial de Y
  float t1_graus = 0; // Ângulos
  float t2_graus = 0; // Ângulos
  int passo_angulo = 90;
  float pos_final = 0;
  int pino_mais = 4;
  int pino_menos = 2;
  int contador_ciclos = 0;
  int contador_ciclos2 = 0;
//-----------------------      configurações-----------------------------------
void setup(){
  tela.begin(16,2); // inicia display
  tela.setCursor(1,0);  //linha 1, coluna 0
  tela.print("braco robotico");

  ombro.attach(6); // Pino PWM
  cotovelo.attach(5); // Pino PWM
  base.attach(3); // Pino PWM
  pulso.attach(9); // Pino PWM

  pinMode(pino_mais, INPUT_PULLUP); // Entrada
  pinMode(pino_menos, INPUT_PULLUP); // Entrada

  ombro.write(90); // Ângulo inicial
  cotovelo.write(90); // Ângulo inicial
  pulso.write(90); // Ângulo inicial
  base.write(90); // Ângulo inicial

  delay(1000);
  
  tela.clear(); // Limpa a tela
}
//---------------------------------Repetições----------------------------------
void loop(){
  if(contador_ciclos >= 4){ // Garante tempo suficiente para atualização do joystick
      joystick(x , y); // Leitura do controle
  }
//-----------------------------------------------------------------------------
  cinematica_inversa(x , y , t1_graus , t2_graus); // Calculo dos angulos
//-----------------------------------------------------------------------------
  Angulo_base(); //Calcula rotação da base
//-----------------------------------------------------------------------------
    float ombro_pos = constrain(t1_graus , 15 , 165); // Limita o ângulo do servo motor do ombro
    float cotovelo_pos = constrain(180 + t2_graus , 0 , 180); // Limita o ângulo do servo motor do cotovelo
//-------------------------------------------------------------------------------
  if(contador_ciclos>=4){ // Garante tempo suficiente para atualizações dos botões
    if(digitalRead(pino_mais) == LOW){
    passo_angulo += 1;
  }
  else if(digitalRead(pino_menos) == LOW){
    passo_angulo -= 1;
  }
    contador_ciclos = 0;
  }
  if(contador_ciclos2>=10){
    Tela();
    contador_ciclos2 = 0;
    } // Garante atualização da tela a cada 200ms
//-----------------------------------------------------------------------------
    angulo_pulso(t1_graus , t2_graus , pos_final); // Calcula o ângulo do pulso
//-----------------------------------------------------------------------------
  ombro.write(ombro_pos); // Servo se movimenta
  //escreverSuave(ombro , ombro_pos); // Utilizar caso movimento não esteja suave
  cotovelo.write(cotovelo_pos); // Servo se movimenta
  //escreverSuave(cotovelo , cotovelo_pos); // Utilizar caso movimento não esteja suave
  base.write(angulo_base); // Servo se movimenta
  //escreverSuave(base , angulo_base); // Utilizar caso movimento não esteja suave
  pulso.write(pos_final); // Servo se movimenta
  //escreverSuave(pulso , pos_final); // Utilizar caso movimento não esteja suave
 //-----------------------------------------------------------------------------
  contador_ciclos++; // Soma 1 ao contador
  contador_ciclos2++; // Soma 1 ao contador  20 * 10 = 200 milisegundos (5Hz)
  delay(20); // 20 * 4 = 80 milisegundos por ciclo (12,5Hz)
}



//-----------------------função de leitura do joystick------------------------
void joystick(float &X, float &Y){ // Cria duas variáveis de posição que alteram o valor das variáveis globais
  float passo = 0.1; // Incremento na posição
  int val_x = analogRead(Jx); // Armazena a leitura do pino
  int val_y = analogRead(Jy); // Armazena a leitura do pino
  float r_max = L1 + L2 - 0.1; // 29,9cm, garante o raio máximo
  float r_min = abs(L1 - L2)+ 0.1; // |L1-L2|, modulo da diferença dos braços
  float y_max = sqrt((r_max * r_max)-(X * X)); // Limita Y em 0 e máximo
  float y_min = 0; // Não ultrabassa a base do robô

  if(abs(X) < r_min){ // Se a distância X for menor que o raio mínimo
    y_min = sqrt((r_min * r_min)-(X * X)); // Garante que o braço alcance
  }
  if(val_x > 900) {X -= passo;} // Decrementa a posição X
  else if(val_x < 100) {X += passo;} // Incrementa a posição X
  if(val_y > 900) {Y += passo;} // Incrementa a posição Y
  else if(val_y < 100) {Y -= passo;} // Decrementa a posição Y

   X = constrain(X , 0 , r_max); // Limita X em  e 29,9
   Y = constrain(Y, y_min , y_max); // Limita Y em 0 e máximo
}



//-----------------------Cinemática inversa-------------------------------------
void cinematica_inversa(float x, float y, float &theta1, float &theta2){
  float cos_theta2 = ((x * x) + (y * y) - (L1 * L1) - (L2 * L2)) / (2 * L1 * L2);
    cos_theta2 = constrain(cos_theta2, -1 , 1); // Garante que cosseno de theta2 fique entre -1 e 1
  float sen_theta2 = -sqrt(1 - cos_theta2 * cos_theta2); // Cotovelo para cima
    theta2 = atan2(sen_theta2 , cos_theta2); // Encontra os ângulos
    theta1 = atan2(y,x) - atan2(L2 * sen_theta2, L1 + L2 * cos_theta2);

    theta2 = theta2 * 180 / PI; // Converte radianos para graus
    theta1 = theta1 * 180 / PI; // Converte radianos para graus
}



//-------------------------Atualiza a tela---------------------------------------
void Tela(){
  tela.setCursor(0,0);
  tela.print("X: ");
  tela.setCursor(3,0);
  tela.print(x);
  tela.setCursor(7,0);
  tela.print("|");
  tela.setCursor(8,0);
  tela.print("Y: ");
  tela.setCursor(11,0);
  tela.print(y);
  tela.setCursor(0,1);
  tela.print("Efetuador: ");
  tela.setCursor(11,1);
  tela.print(passo_angulo);
  tela.print("  ");
}



//--------------------------Rotação da base--------------------------------------
void Angulo_base(){
  angulo_base = analogRead(Base);
  angulo_base = map(angulo_base , 0 , 1023 , 0 , 180);
}



//---------------------------angulo do pulso(efetuador)-------------------------
void angulo_pulso(float t1, float t2 , float &Pos_final){ // Recebe os ângulos theta1 e 2
  passo_angulo = constrain(passo_angulo , 0 , 180);
  Pos_final = constrain(passo_angulo - (t1 + t2) , 0 , 180);
}



//----------------------------Movimento suave (se necessário)------------------
/*void escreverSuave(Servo &meuServo, int anguloAlvo) {
  int posAtual = meuServo.read(); // Lê onde o servo está agora
  if (posAtual < anguloAlvo) {
    posAtual++; // Anda +1 grau em direção ao alvo
  } else if (posAtual > anguloAlvo) {
    posAtual--; // Anda -1 grau em direção ao alvo
  }
  meuServo.write(posAtual); // Escreve o passo suave
}*/