/*
main.c
Odradek Simulator on ESP32 using ESP-IDF & FreeRTOS
---------------------------------------------------
- Reads two HC-SR04 ultrasonic sensors (front and bottom)
- Samples an analog noise sensor (microphone) on ADC1_CH6
- Drives a buzzer via PWM to emulate “lup-dup” heartbeat sounds (lower‐pitched “lup”)
- Controls two servos:
* claw‐servo: locks or opens the claw
* radar‐servo: sweeps when no EV, locks onto direction when EV detected
- Controls a stepper motor for chase indication
- Updates a NeoPixel LED ring via RMT with blinking white/blue/yellow/red
- Manages a 4-state Finite-State Machine (FSM):
* EV_FAR
* EV_CLOSE
* EV_NEAR_CAUTIOUS
* EV_NEAR_ALERT
- Uses mutexes and queues for safe data sharing
Author: josemi (modified)
Platform: ESP32 DevKitC using ESP-IDF
License: MIT
*/
#include <stdio.h>
#include <string.h>
#include <stdbool.h>
// FreeRTOS
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "freertos/semphr.h"
// Drivers
#include "driver/gpio.h"
#include "driver/ledc.h"
#include "driver/rmt_tx.h"
#include "esp_adc/adc_oneshot.h"
#include "sdkconfig.h"
// ESP-IDF
#include "esp_log.h"
#include "esp_timer.h"
#include "esp_rom_sys.h"
// NeoPixel (WS2812) via LED strip API
#include "led_strip.h"
#include "led_strip_rmt.h"
#include "led_strip_spi.h"
#include "led_strip_types.h"
#include "led_strip_interface.h"
// 1) API
#define TAG LEDSTRIP_API_TAG
#include "led_strip_api.c"
#undef TAG
// 2) RMT device
#define TAG LEDSTRIP_RMT_DEV_TAG
#include "led_strip_rmt_dev.c"
#undef TAG
// 3) RMT encoder
#define TAG LEDSTRIP_RMT_ENCODER_TAG
#include "led_strip_rmt_encoder.c"
#undef TAG
// 4) SPI device (not used here, but included for completeness)
#define TAG LEDSTRIP_SPI_DEV_TAG
#include "led_strip_spi_dev.c"
#undef TAG
//==============================================================================
// PIN / CHANNEL DEFINITIONS
//==============================================================================
// HC-SR04 front sensor
#define TRIG_FRONT GPIO_NUM_5
#define ECHO_FRONT GPIO_NUM_18
// HC-SR04 bottom sensor
#define TRIG_BOTTOM GPIO_NUM_4
#define ECHO_BOTTOM GPIO_NUM_15
// Analog noise sensor
#define SOUND_SENSOR_ADC_CH ADC_CHANNEL_6 // GPIO34
// Buzzer PWM
#define BUZZER_GPIO GPIO_NUM_27
// Claw‐servo PWM
#define SERVO_CLAW_GPIO GPIO_NUM_21
// Radar‐servo PWM
#define SERVO_RADAR_GPIO GPIO_NUM_22
// Stepper driver pins
#define STEPPER_STEP_GPIO GPIO_NUM_32
#define STEPPER_DIR_GPIO GPIO_NUM_33
// NeoPixel LED ring
#define NEOPIXEL_GPIO GPIO_NUM_13
#define NUM_PIXELS 16
// Servo pulse widths (in microseconds)
#define SERVO_MIN_US 1000 // fully closed / leftmost
#define SERVO_MAX_US 2000 // fully open / rightmost
// Segun un señor brasileño debo poner esto pa que vaya
#define CONFIG_LOG_DEFAULT_LEVEL_VERBOSE 1
#define CONFIG_LOG_MAXIMUM_LEVEL 5
//==============================================================================
// THRESHOLDS CONFIGURATION
//==============================================================================
#define TH_DIST_FAR 80.0f // cm
#define TH_DIST_CLOSE 30.0f // cm
#define TH_NOISE_ALERT 200 // 0–255
#define TH_DIST_FLOOR 20.0f // if 20cm, you're crouched
//==============================================================================
// FINITE-STATE MACHINE DEFINITION
//==============================================================================
typedef enum {
STATE_EV_FAR = 0,
STATE_EV_CLOSE,
STATE_EV_NEAR_CAUTIOUS,
STATE_EV_NEAR_ALERT
} odradek_state_t;
static const char* state_names[] = {
"EV_FAR",
"EV_CLOSE",
"EV_NEAR_CAUTIOUS",
"EV_NEAR_ALERT"
};
//==============================================================================
// SHARED CONTEXT STRUCTURE
//==============================================================================
typedef struct {
odradek_state_t current_state;
float front_distance;
float bottom_distance;
bool crouched;
uint8_t noise_level;
QueueHandle_t noise_queue;
} odradek_ctx_t;
static odradek_ctx_t ctx;
static SemaphoreHandle_t ctx_mutex;
static const char* TAG = "ODRADEK";
//==============================================================================
// TASK: NOISE SENSOR
// Reads ADC once every 100ms -> 8-bit -> queue + ctx.noise_level
//==============================================================================
static void task_noise_sensor(void *pv) {
adc_oneshot_unit_init_cfg_t init_cfg = { .unit_id = ADC_UNIT_1 };
adc_oneshot_unit_handle_t adc_handle;
ESP_ERROR_CHECK(adc_oneshot_new_unit(&init_cfg, &adc_handle));
adc_oneshot_chan_cfg_t chan_cfg = {
.atten = ADC_ATTEN_DB_11,
.bitwidth = ADC_BITWIDTH_DEFAULT
};
ESP_ERROR_CHECK(adc_oneshot_config_channel(adc_handle, SOUND_SENSOR_ADC_CH, &chan_cfg));
TickType_t last = xTaskGetTickCount();
for (;;) {
int raw;
ESP_ERROR_CHECK(adc_oneshot_read(adc_handle, SOUND_SENSOR_ADC_CH, &raw));
uint8_t level = raw >> 4;
xSemaphoreTake(ctx_mutex, portMAX_DELAY);
ctx.noise_level = level;
ESP_LOGI(TAG, "Noise level (raw 0–255): %d", level);
xSemaphoreGive(ctx_mutex);
xQueueSend(ctx.noise_queue, &level, 0);
vTaskDelayUntil(&last, pdMS_TO_TICKS(100));
}
}
//==============================================================================
// HELPER: Trigger HC-SR04 -> measure echo -> distance (cm)
//==============================================================================
static float read_distance_cm(gpio_num_t trig, gpio_num_t echo) {
gpio_set_level(trig, 1);
esp_rom_delay_us(10);
gpio_set_level(trig, 0);
const int64_t TIMEOUT_US = 30000;
int64_t start = esp_timer_get_time();
while (!gpio_get_level(echo)) {
if (esp_timer_get_time() - start > TIMEOUT_US) return -1.0f;
taskYIELD();
}
int64_t t0 = esp_timer_get_time(), t1 = t0;
while (gpio_get_level(echo)) {
if (esp_timer_get_time() - t0 > TIMEOUT_US) return -1.0f;
t1 = esp_timer_get_time();
taskYIELD();
}
double dt = (t1 - t0) * 1e-6;
return (float)(343.0 * dt * 100.0 / 2.0);
}
//==============================================================================
// TASK: FRONT DISTANCE SENSOR (200ms)
//==============================================================================
static void task_distance_sensor(void *pv) {
gpio_reset_pin(TRIG_FRONT);
gpio_set_direction(TRIG_FRONT, GPIO_MODE_OUTPUT);
gpio_reset_pin(ECHO_FRONT);
gpio_set_direction(ECHO_FRONT, GPIO_MODE_INPUT);
gpio_set_pull_mode(ECHO_FRONT, GPIO_PULLDOWN_ONLY);
TickType_t last = xTaskGetTickCount();
for (;;) {
float d = read_distance_cm(TRIG_FRONT, ECHO_FRONT);
xSemaphoreTake(ctx_mutex, portMAX_DELAY);
ctx.front_distance = d;
xSemaphoreGive(ctx_mutex);
vTaskDelayUntil(&last, pdMS_TO_TICKS(200));
}
}
//==============================================================================
// TASK: POSTURE SENSOR (200ms)
//==============================================================================
static void task_posture_sensor(void *pv) {
gpio_reset_pin(TRIG_BOTTOM);
gpio_set_direction(TRIG_BOTTOM, GPIO_MODE_OUTPUT);
gpio_reset_pin(ECHO_BOTTOM);
gpio_set_direction(ECHO_BOTTOM, GPIO_MODE_INPUT);
gpio_set_pull_mode(ECHO_BOTTOM, GPIO_PULLDOWN_ONLY);
TickType_t last = xTaskGetTickCount();
for (;;) {
float d = read_distance_cm(TRIG_BOTTOM, ECHO_BOTTOM);
xSemaphoreTake(ctx_mutex, portMAX_DELAY);
ctx.bottom_distance = d;
ctx.crouched = (d >= 0.0f && d < TH_DIST_FLOOR);
ESP_LOGI(TAG, "Bottom distance: %.1f cm -> crouched=%d", ctx.bottom_distance, ctx.crouched);
xSemaphoreGive(ctx_mutex);
vTaskDelayUntil(&last, pdMS_TO_TICKS(200));
}
}
//==============================================================================
// TASK: STATE MANAGER (100ms)
//==============================================================================
static inline void update_state(odradek_state_t s) {
xSemaphoreTake(ctx_mutex, portMAX_DELAY);
ctx.current_state = s;
xSemaphoreGive(ctx_mutex);
ESP_LOGI(TAG, "FSM -> %s", state_names[s]);
}
static void task_state_manager(void *pv) {
TickType_t last = xTaskGetTickCount();
uint8_t noise;
for (;;) {
if (xQueueReceive(ctx.noise_queue, &noise, 0) == pdPASS) {
xSemaphoreTake(ctx_mutex, portMAX_DELAY);
ctx.noise_level = noise;
xSemaphoreGive(ctx_mutex);
}
xSemaphoreTake(ctx_mutex, portMAX_DELAY);
float d = ctx.front_distance;
uint8_t n = ctx.noise_level;
bool crouched = ctx.crouched;
xSemaphoreGive(ctx_mutex);
ESP_LOGI("ODRADEK", "Distance, Noise, Crouched? : %.1f, %d, %d.", d, n, crouched);
odradek_state_t next;
if (crouched && n > TH_NOISE_ALERT) { next = STATE_EV_NEAR_ALERT; ESP_LOGI("ODRADEK", "FACTORES ESTO SALTO!!"); }
else if (d < TH_DIST_CLOSE) next = STATE_EV_NEAR_CAUTIOUS;
else if (d < TH_DIST_FAR) next = STATE_EV_CLOSE;
else next = STATE_EV_FAR;
update_state(next);
vTaskDelayUntil(&last, pdMS_TO_TICKS(100));
}
}
//==============================================================================
// TASK: VISUAL FEEDBACK (NeoPixel blinking)
//==============================================================================
static void task_visual_feedback(void *pv) {
led_strip_config_t strip_cfg = {
.strip_gpio_num = NEOPIXEL_GPIO,
.max_leds = NUM_PIXELS,
.led_model = LED_MODEL_WS2812,
.color_component_format = LED_STRIP_COLOR_COMPONENT_FMT_GRB,
.flags = { .invert_out = false }
};
led_strip_rmt_config_t rmt_cfg = {
.clk_src = RMT_CLK_SRC_DEFAULT,
.resolution_hz = 10 * 1000 * 1000,
.mem_block_symbols = 0,
.flags = { .with_dma = false }
};
led_strip_handle_t strip;
ESP_ERROR_CHECK(led_strip_new_rmt_device(&strip_cfg, &rmt_cfg, &strip));
// [far, close, cautious, alert]
const uint32_t intervals[4] = {1000, 750, 500, 250};
const uint8_t colors [4][3] = {
{ 0, 0, 255}, // blue
{ 0, 0, 255}, // blue
{255, 255, 0}, // yellow
{255, 128, 0} // orange
};
for (;;) {
xSemaphoreTake(ctx_mutex, portMAX_DELAY);
odradek_state_t st = ctx.current_state;
xSemaphoreGive(ctx_mutex);
uint32_t iv = intervals[st];
// ON
for (int i = 0; i < NUM_PIXELS; i++) {
led_strip_set_pixel(strip, i,
colors[st][0],
colors[st][1],
colors[st][2]
);
}
ESP_ERROR_CHECK(led_strip_refresh(strip));
vTaskDelay(pdMS_TO_TICKS(iv / 2));
// OFF
for (int i = 0; i < NUM_PIXELS; i++) {
led_strip_set_pixel(strip, i, 0, 0, 0);
}
ESP_ERROR_CHECK(led_strip_refresh(strip));
vTaskDelay(pdMS_TO_TICKS(iv / 2));
}
}
//==============================================================================
// TASK: ACOUSTIC FEEDBACK (“lup-dup” with lower‐pitched “lup”)
//==============================================================================
static void task_acoustic_alert(void *pv) {
// Configuración inicial del timer (ahora cambiaremos la frecuencia dinámicamente)
ledc_timer_config_t tcfg = {
.speed_mode = LEDC_HIGH_SPEED_MODE,
.timer_num = LEDC_TIMER_0,
.duty_resolution = LEDC_TIMER_8_BIT,
.freq_hz = 1000, // Valor inicial, se cambiará
.clk_cfg = LEDC_AUTO_CLK
};
ESP_ERROR_CHECK(ledc_timer_config(&tcfg));
ledc_channel_config_t ccfg = {
.gpio_num = BUZZER_GPIO,
.speed_mode = LEDC_HIGH_SPEED_MODE,
.channel = LEDC_CHANNEL_0,
.intr_type = LEDC_INTR_DISABLE,
.timer_sel = LEDC_TIMER_0,
.duty = 0,
.hpoint = 0
};
ESP_ERROR_CHECK(ledc_channel_config(&ccfg));
// Beat intervals per state (ms)
const uint32_t beat_iv[4] = {1500, 1000, 700, 400};
for (;;) {
// Snapshot state
xSemaphoreTake(ctx_mutex, portMAX_DELAY);
odradek_state_t st = ctx.current_state;
xSemaphoreGive(ctx_mutex);
uint32_t iv = beat_iv[st];
// "LUP" - más grave (800 Hz) y más largo (70 ms)
tcfg.freq_hz = 800;
ESP_ERROR_CHECK(ledc_timer_config(&tcfg));
ledc_set_duty(ccfg.speed_mode, ccfg.channel, 0xFF); // Máxima intensidad
ledc_update_duty(ccfg.speed_mode, ccfg.channel);
vTaskDelay(pdMS_TO_TICKS(70));
ledc_set_duty(ccfg.speed_mode, ccfg.channel, 0);
ledc_update_duty(ccfg.speed_mode, ccfg.channel);
vTaskDelay(pdMS_TO_TICKS(50)); // Pausa entre lup y dup
// "DUP" - más agudo (1200 Hz) y más corto (30 ms)
tcfg.freq_hz = 1200;
ESP_ERROR_CHECK(ledc_timer_config(&tcfg));
ledc_set_duty(ccfg.speed_mode, ccfg.channel, 0xC0); // 75% intensidad
ledc_update_duty(ccfg.speed_mode, ccfg.channel);
vTaskDelay(pdMS_TO_TICKS(30));
ledc_set_duty(ccfg.speed_mode, ccfg.channel, 0);
ledc_update_duty(ccfg.speed_mode, ccfg.channel);
// Espera el resto del intervalo
uint32_t remainder = (iv > 150) ? (iv - 150) : 0;
vTaskDelay(pdMS_TO_TICKS(remainder));
}
}
//==============================================================================
// TASK: MECHANISM CONTROL (two servos + stepper)
//==============================================================================
static void task_mechanism_control(void *pv) {
// — PWM @50Hz para ambos servos —
ledc_timer_config_t tcfg = {
.speed_mode = LEDC_LOW_SPEED_MODE,
.timer_num = LEDC_TIMER_1,
.duty_resolution = LEDC_TIMER_16_BIT,
.freq_hz = 50,
.clk_cfg = LEDC_AUTO_CLK
};
ESP_ERROR_CHECK(ledc_timer_config(&tcfg));
// Claw-servo (canal 1, GPIO 21)
ledc_channel_config_t claw = {
.gpio_num = SERVO_CLAW_GPIO,
.speed_mode = LEDC_LOW_SPEED_MODE,
.channel = LEDC_CHANNEL_1,
.intr_type = LEDC_INTR_DISABLE,
.timer_sel = LEDC_TIMER_1,
.duty = 0,
.hpoint = 0
};
ESP_ERROR_CHECK(ledc_channel_config(&claw));
// Radar-servo (canal 2, GPIO 22)
ledc_channel_config_t radar = {
.gpio_num = SERVO_RADAR_GPIO,
.speed_mode = LEDC_LOW_SPEED_MODE,
.channel = LEDC_CHANNEL_2,
.intr_type = LEDC_INTR_DISABLE,
.timer_sel = LEDC_TIMER_1,
.duty = 0,
.hpoint = 0
};
ESP_ERROR_CHECK(ledc_channel_config(&radar));
// Stepper setup
gpio_reset_pin(STEPPER_STEP_GPIO);
gpio_set_direction(STEPPER_STEP_GPIO, GPIO_MODE_OUTPUT);
gpio_reset_pin(STEPPER_DIR_GPIO);
gpio_set_direction(STEPPER_DIR_GPIO, GPIO_MODE_OUTPUT);
// Variables de estado internas
static int16_t radar_pos = (SERVO_MIN_US + SERVO_MAX_US) / 2;
static int8_t radar_dir = 1;
static bool claw_closing = true;
static uint32_t claw_timer = 0;
// Parámetros de velocidad
const uint32_t sweep_step_far = 2; // µs por iteración
const uint32_t sweep_step_close = 5;
const uint32_t beat_iv_far = 800; // ms entre clap lento
const uint32_t beat_iv_close = 400; // ms entre clap rápido
TickType_t last = xTaskGetTickCount();
for (;;) {
// 1) Leer estado compartido
xSemaphoreTake(ctx_mutex, portMAX_DELAY);
odradek_state_t st = ctx.current_state;
float dist = ctx.front_distance;
xSemaphoreGive(ctx_mutex);
// 2) Radar-servo
if (st == STATE_EV_FAR) {
uint32_t step = (st == STATE_EV_FAR) ? sweep_step_far : sweep_step_close;
radar_pos += radar_dir * step;
if (radar_pos >= SERVO_MAX_US) { radar_pos = SERVO_MAX_US; radar_dir = -1; }
else if (radar_pos <= SERVO_MIN_US) { radar_pos = SERVO_MIN_US; radar_dir = 1; }
} else {
// Lock onto la dirección última
float f = 1.0f - (dist / TH_DIST_FAR);
f = f < 0.0f ? 0.0f : (f > 1.0f ? 1.0f : f);
radar_pos = SERVO_MIN_US + (int)((SERVO_MAX_US - SERVO_MIN_US) * f);
}
{
uint32_t duty = (uint32_t)((uint32_t)radar_pos * ((1<<16) - 1) / 20000);
ledc_set_duty(radar.speed_mode, radar.channel, duty);
ledc_update_duty(radar.speed_mode, radar.channel);
}
// 3) Claw-servo (“clap”)
claw_timer += 10; // usamos un delay fijo de 10ms
uint32_t interval = (st == STATE_EV_FAR) ? beat_iv_far : beat_iv_close;
if (st == STATE_EV_FAR || st == STATE_EV_CLOSE) {
if (claw_timer >= interval) {
claw_timer = 0;
claw_closing = !claw_closing;
}
int us = claw_closing ? SERVO_MIN_US : SERVO_MAX_US;
uint32_t duty = (uint32_t)((uint32_t)us * ((1<<16)-1) / 20000);
ledc_set_duty(claw.speed_mode, claw.channel, duty);
ledc_update_duty(claw.speed_mode, claw.channel);
} else {
// Mantener abierta en modos cauteloso/alerta
uint32_t duty = (uint32_t)((uint32_t)SERVO_MAX_US * ((1<<16)-1) / 20000);
ledc_set_duty(claw.speed_mode, claw.channel, duty);
ledc_update_duty(claw.speed_mode, claw.channel);
}
// 4) Stepper sólo en estados de giro
if (st == STATE_EV_NEAR_CAUTIOUS || st == STATE_EV_NEAR_ALERT) {
gpio_set_level(STEPPER_DIR_GPIO, (st == STATE_EV_NEAR_ALERT) ? 0 : 1);
// un pulso breve
gpio_set_level(STEPPER_STEP_GPIO, 1);
esp_rom_delay_us(800);
gpio_set_level(STEPPER_STEP_GPIO, 0);
}
// 5) Pequeña pausa y vuelta al principio
vTaskDelayUntil(&last, pdMS_TO_TICKS(10));
}
}
//==============================================================================
// APP ENTRY POINT
//==============================================================================
void app_main(void) {
// Todos los niveles INFO o superiores
esp_log_level_set("ODRADEK", ESP_LOG_VERBOSE);
ESP_LOGI("ODRADEK", "Logging inicializado a INFO");
// initialize context
memset(&ctx, 0, sizeof(ctx));
ctx.current_state = STATE_EV_FAR;
ctx.front_distance = -1.0f;
ctx.bottom_distance = -1.0f;
// create mutex & queue
ctx_mutex = xSemaphoreCreateMutex();
configASSERT(ctx_mutex);
ctx.noise_queue = xQueueCreate(8, sizeof(uint8_t));
configASSERT(ctx.noise_queue);
// spawn tasks, highest -> lowest priority
xTaskCreate(task_visual_feedback, "Visual", 4096, NULL, 5, NULL);
vTaskDelay(pdMS_TO_TICKS(50));
xTaskCreate(task_noise_sensor, "Noise", 4096, NULL, 4, NULL);
xTaskCreate(task_distance_sensor, "DistF", 4096, NULL, 4, NULL);
xTaskCreate(task_posture_sensor, "DistB", 4096, NULL, 4, NULL);
vTaskDelay(pdMS_TO_TICKS(50));
xTaskCreate(task_state_manager, "FSM", 4096, NULL, 3, NULL);
xTaskCreate(task_acoustic_alert, "Audio", 4096, NULL, 3, NULL);
vTaskDelay(pdMS_TO_TICKS(50));
xTaskCreate(task_mechanism_control, "Mech", 8192, NULL, 2, NULL);
ESP_LOGI(TAG, "All tasks started — Odradek ready");
}
Barrido
Apertura garra