// ----------------------------------------------------------------------
/* Código para Arduino Uno
   Medição de distancia utilizando sensor ultrassônico HC-SR04
   Por: Renan Ribeiro Machado e Prof. Jaime A. M. Sanchez, EESC/USP, 2024. */

#include <Servo.h>

// ----------------------------------------------------------------------
// Variáveis globais
// Criando um objeto para o servo motor
Servo meuServo;

// Definindo os pinos
const int pinoServo = 9;    // saída do sinal de PWM para o SG90
const int pinoPot = A0;     // Leitura do potenciômetro
const int pinoServoR = A1;  // Leitura do sinal de acionamento do servo
const int trigPin = 7;      // TRIGGER do ultrassom
const int echoPin = 6;      // ECHO do ultrassom

// ----------------------------------------------------------------------
// Configuração GPIO, periféricos, etc.
void setup() {
  // Associando o servo ao pino de controle
  meuServo.attach( pinoServo );

  // Inicializando a comunicação serial para monitoramento e plotagem
  Serial.begin( 9600 );

  // Configurando os pinos do sensor ultrassônico
  pinMode( trigPin, OUTPUT );
  pinMode( echoPin, INPUT );
}

// ----------------------------------------------------------------------
// Loop principal:  
void loop() {
  
  // Leitura do sensor ultrassônico
  long duracao_ultrassom, duracao_PWM;
  int distancia;

  // Garantindo que o TRIG esteja baixo
  digitalWrite( trigPin, LOW );
  delayMicroseconds( 2 );

  // Enviando pulso TRIG de 10µs
  digitalWrite( trigPin, HIGH );
  delayMicroseconds( 10 );
  digitalWrite( trigPin, LOW );

  // Calculando o tempo de duração do ECHO
  duracao_ultrassom = pulseIn( echoPin, HIGH );

  // Convertendo a duração para distância (em centímetros)
  distancia = duracao_ultrassom * 0.034 / 2;

  // Controle do servo pelo potenciômetro
  int valorPot = analogRead( pinoPot );
  int angulo = map( valorPot, 0, 1023, -45, 225 );

  // Se a distância for menor que 25 cm, esterçar pra direita ou pra esquerda
  // de acordo com a posição atual da haste:
  if ( distancia < 25 ){
    if ( angulo > 90 ){
      angulo = 180;
    }
    else{
      if( angulo < 90 ){
      angulo = 0;
      }
    }
  }

  // Movendo o servo para o ângulo correspondente
  meuServo.write( angulo );

   // Calculando o tempo de duração do pulso de PWM:
  duracao_PWM = pulseIn( pinoServoR, HIGH );
  int anguloServo = map( duracao_PWM, 520, 2370, 0, 180 );

  // Enviando os valores para o Serial Plotter
  Serial.print( "Potenciometro: " );
  // Serial.print( angulo );
  Serial.print( valorPot );
  Serial.print( "\tServo: " );
  //Serial.println( anguloServo );
  Serial.println( duracao_PWM );

  // Delay para suavizar as leituras e atualização do gráfico
  delay( 50 );
}