/*
* Experimento 2: Controle de Robô com FreeRTOS no ESP32
* Sistema multitarefa com sensores ultrassônicos e controle PWM de motores.
* A velocidade dos motores varia exponencialmente com a distância dos obstáculos.
*/
// --- Bibliotecas ---
#include <Arduino.h>
#include "freertos/FreeRTOS.h"
#include "freertos/semphr.h"
// === Pinos dos Sensores Ultrassônicos ===
#define TRIG_FRONT 23
#define ECHO_FRONT 22
#define TRIG_RIGHT 18
#define ECHO_RIGHT 5
#define TRIG_BACK 32
#define ECHO_BACK 33
#define TRIG_LEFT 27
#define ECHO_LEFT 14
// === Motores controlados via canais de PWM ===
#define MOTOR1_PIN 25
#define MOTOR2_PIN 26
#define MOTOR3_PIN 12
#define MOTOR4_PIN 13
#define MOTOR1_CH 0
#define MOTOR2_CH 1
#define MOTOR3_CH 2
#define MOTOR4_CH 3
const int PWM_FREQ = 5000;
const int PWM_RESOLUTION = 8;
const int MAX_DUTY = 255;
// Variáveis compartilhadas
volatile float distFront, distRight, distBack, distLeft;
SemaphoreHandle_t mutexDist;
// Estrutura para sensor
struct SensorConfig {
int trig;
int echo;
volatile float* distPtr;
};
// Protótipos
void TaskSensor(void* param);
void TaskMotorControl(void* param);
float lerDistancia(int trig, int echo);
int calcularDutyCycle(float distancia);
void setup() {
Serial.begin(115200);
Serial.println("Sistema Iniciado");
// PWM: configuração dos canais
ledcSetup(MOTOR1_CH, PWM_FREQ, PWM_RESOLUTION); ledcAttachPin(MOTOR1_PIN, MOTOR1_CH);
ledcSetup(MOTOR2_CH, PWM_FREQ, PWM_RESOLUTION); ledcAttachPin(MOTOR2_PIN, MOTOR2_CH);
ledcSetup(MOTOR3_CH, PWM_FREQ, PWM_RESOLUTION); ledcAttachPin(MOTOR3_PIN, MOTOR3_CH);
ledcSetup(MOTOR4_CH, PWM_FREQ, PWM_RESOLUTION); ledcAttachPin(MOTOR4_PIN, MOTOR4_CH);
// Cria mutex
mutexDist = xSemaphoreCreateMutex();
// Configuração dos sensores
static SensorConfig cfgFront = {TRIG_FRONT, ECHO_FRONT, &distFront};
static SensorConfig cfgRight = {TRIG_RIGHT, ECHO_RIGHT, &distRight};
static SensorConfig cfgBack = {TRIG_BACK, ECHO_BACK, &distBack};
static SensorConfig cfgLeft = {TRIG_LEFT, ECHO_LEFT, &distLeft};
// Criação das tarefas
xTaskCreatePinnedToCore(TaskSensor, "Sensor Front", 2048, &cfgFront, 1, NULL, 1);
xTaskCreatePinnedToCore(TaskSensor, "Sensor Right", 2048, &cfgRight, 1, NULL, 1);
xTaskCreatePinnedToCore(TaskSensor, "Sensor Back", 2048, &cfgBack, 1, NULL, 1);
xTaskCreatePinnedToCore(TaskSensor, "Sensor Left", 2048, &cfgLeft, 1, NULL, 1);
xTaskCreatePinnedToCore(TaskMotorControl, "Motor Control", 2048, NULL, 2, NULL, 1);
}
void loop() {
vTaskDelete(NULL); // Controle total entregue ao FreeRTOS
}
// Função de leitura ultrassônica
float lerDistancia(int trig, int echo) {
digitalWrite(trig, LOW); delayMicroseconds(2);
digitalWrite(trig, HIGH); delayMicroseconds(10);
digitalWrite(trig, LOW);
long dur = pulseIn(echo, HIGH, 30000);
return (dur == 0) ? 500.0 : (dur * 0.0343) / 2.0;
}
// Tarefa genérica de leitura de sensor
void TaskSensor(void* param) {
SensorConfig* cfg = (SensorConfig*)param;
pinMode(cfg->trig, OUTPUT);
pinMode(cfg->echo, INPUT);
while (true) {
float d = lerDistancia(cfg->trig, cfg->echo);
if (xSemaphoreTake(mutexDist, pdMS_TO_TICKS(10)) == pdTRUE) {
*(cfg->distPtr) = d;
xSemaphoreGive(mutexDist);
}
vTaskDelay(pdMS_TO_TICKS(75));
}
}
// Calcula duty cycle com base na distância (controle exponencial)
int calcularDutyCycle(float distancia) {
const float distMin = 10.0, distMax = 60.0;
distancia = constrain(distancia, 0, distMax);
if (distancia < distMin) return 0;
float fator = (distancia - distMin) / (distMax - distMin);
float base = 10.0;
float resultado = (pow(base, fator) - 1) / (base - 1);
return int(resultado * MAX_DUTY);
}
// Tarefa de controle dos motores
void TaskMotorControl(void* param) {
float dF, dB;
while (true) {
if (xSemaphoreTake(mutexDist, pdMS_TO_TICKS(10)) == pdTRUE) {
dF = distFront;
dB = distBack;
xSemaphoreGive(mutexDist);
}
int pwmF = calcularDutyCycle(dF);
int pwmB = calcularDutyCycle(dB);
ledcWrite(MOTOR1_CH, pwmF);
ledcWrite(MOTOR2_CH, pwmF);
ledcWrite(MOTOR3_CH, pwmB);
ledcWrite(MOTOR4_CH, pwmB);
Serial.printf("Frente: %.1f cm → %d | Trás: %.1f cm → %d\n", dF, pwmF, dB, pwmB);
vTaskDelay(pdMS_TO_TICKS(50));
}
}