#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/gpio.h"
#include "esp_timer.h"
#include "esp_random.h"
#include "rom/ets_sys.h" // For busy-waiting
// --- CONFIGURATION ---
// GPIO 0 is the BOOT button (Active Low)
#define ESTOP_BUTTON_PIN GPIO_NUM_0
// Window sizes
#define WIN_10 10
#define WIN_100 100
#define WIN_1000 1000
#define WIN_10000 10000
// Global buffers
static int data_history[WIN_10000];
static int data_index = 0;
// --- 1. SENSOR SIMULATION (Blocking) ---
int collect_sensor_data() {
// Busy wait for 50ms to simulate hardware read time
ets_delay_us(50000);
return (int)(esp_random() % 100);
}
// --- 2. HEAVY MATH (CPU Hog) ---
void compute_statistics(float *mean_out, float *std_out, int window_size) {
double sum = 0.0;
// Pass 1: Mean
for(int i = 0; i < window_size; i++) {
int idx = (data_index - i + WIN_10000) % WIN_10000;
sum += data_history[idx];
}
double mean = sum / window_size;
// Pass 2: Std Dev
double sum_sq_diff = 0.0;
for(int i = 0; i < window_size; i++) {
int idx = (data_index - i + WIN_10000) % WIN_10000;
sum_sq_diff += pow((data_history[idx] - mean), 2);
}
*mean_out = (float)mean;
*std_out = (float)sqrt(sum_sq_diff / window_size);
}
// --- 3. NETWORK SIMULATION (Jittery) ---
void send_telemetry(int payload_size) {
int delay_ms = payload_size / 20;
// 10% chance of "network timeout"
if ((esp_random() % 10) == 0) {
printf(">> [NETWORK] Retry... blocking...\n");
delay_ms += 500;
}
// Blocking delay
vTaskDelay(pdMS_TO_TICKS(delay_ms));
}
// --- MAIN ---
void app_main(void)
{
// CRITICAL: Disable stdout buffering so prints appear immediately
setvbuf(stdout, NULL, _IONBF, 0);
// Setup GPIO
gpio_reset_pin(ESTOP_BUTTON_PIN);
gpio_set_direction(ESTOP_BUTTON_PIN, GPIO_MODE_INPUT);
gpio_set_pull_mode(ESTOP_BUTTON_PIN, GPIO_PULLUP_ONLY);
memset(data_history, 0, sizeof(data_history));
printf("\n\n");
printf("======================================\n");
printf(" TYPICAL SUPER LOOP DEMO \n");
printf("======================================\n");
printf("Press BOOT button to test latency.\n\n");
while (1) {
int64_t start_time = esp_timer_get_time();
// 1. Collect
int raw_data = collect_sensor_data();
data_index = (data_index + 1) % WIN_10000;
data_history[data_index] = raw_data;
// 2. Compute
float m10, s10, m100, s100, m1k, s1k, m10k, s10k;
compute_statistics(&m10, &s10, WIN_10);
compute_statistics(&m100, &s100, WIN_100);
compute_statistics(&m1k, &s1k, WIN_1000);
compute_statistics(&m10k, &s10k, WIN_10000);
// 3. Send
int payload_size = (esp_random() % 2 == 0) ? 100 : 5000;
send_telemetry(payload_size);
// 4. Input Check (The Trap)
if (gpio_get_level(ESTOP_BUTTON_PIN) == 0) {
int64_t current_time = esp_timer_get_time();
int64_t latency = (current_time - start_time) / 1000;
printf("\n\n****************************************\n");
printf("!!! E-STOP BUTTON DETECTED !!!\n");
printf("Input Latency: %lld ms\n", latency);
printf("****************************************\n\n");
}
int64_t loop_duration = (esp_timer_get_time() - start_time) / 1000;
printf("Loop: %4lld ms | Val: %3d | 10k Mean: %5.2f\n",
loop_duration, raw_data, m10k);
// Safety yield
vTaskDelay(1);
}
}