#include <Wire.h>
#include <Ultrasonic.h>
// Inicialização dos sensores ultrassônicos - FRENTE
const int trigPin1 = 13;
const int echoPin1 = 12;
Ultrasonic ultrasonic1(trigPin1, echoPin1);
// ATRÁS
const int trigPin2 = 5;
const int echoPin2 = 4;
Ultrasonic ultrasonic2(trigPin2, echoPin2);
// ESQUERDA
const int trigPin3 = 19;
const int echoPin3 = 18;
Ultrasonic ultrasonic3(trigPin3, echoPin3);
// DIREITA
const int trigPin4 = 17;
const int echoPin4 = 16;
Ultrasonic ultrasonic4(trigPin4, echoPin4);
// Parâmetros de controle
const float distanciaMin = 10.0; // em cm, distância mínima segura
const float distanciaMax = 100.0; // em cm, distância máxima que afeta o controle
const int pwmMin = 50; // PWM mínimo (duty cycle mínimo)
const int pwmMax = 255; // PWM máximo (duty cycle máximo)
// Função para calcular o duty cycle exponencialmente
int calcularPWM(float distancia) {
if (distancia <= distanciaMin) {
return pwmMin;
}
if (distancia >= distanciaMax) {
return pwmMax;
}
// Normaliza distância para [0, 1]
float x = (distancia - distanciaMin) / (distanciaMax - distanciaMin);
// Mapeia exponencialmente
float pwm = pwmMin + (pwmMax - pwmMin) * pow(x, 2.5); // expoente ajustável
return (int)pwm;
}
void setup() {
Serial.begin(115200);
pinMode(trigPin1, OUTPUT);
pinMode(echoPin1, INPUT);
pinMode(trigPin2, OUTPUT);
pinMode(echoPin2, INPUT);
pinMode(trigPin3, OUTPUT);
pinMode(echoPin3, INPUT);
pinMode(trigPin4, OUTPUT);
pinMode(echoPin4, INPUT);
pinMode(trigPin4, OUTPUT);
}
void loop() {
delay(200); // Tempo entre as leituras
// Leitura das distâncias
long distancia1 = ultrasonic1.read(CM);
long distancia2 = ultrasonic2.read(CM);
long distancia3 = ultrasonic3.read(CM);
long distancia4 = ultrasonic4.read(CM);
// Exibição no monitor serial
Serial.println("Distâncias (cm):");
Serial.print("Frente: ");
Serial.print(distancia1);
Serial.print(" | Trás: ");
Serial.print(distancia2);
Serial.print(" | Esquerda: ");
Serial.print(distancia3);
Serial.print(" | Direita: ");
Serial.println(distancia4);
int pwm = calcularPWM(distancia4);
Serial.print(" cm | PWM: ");
Serial.println(pwm);
Serial.println("-----------------------------");
}