Learn. Build. Improve.

// MOGUL TEAM 30524 - Deterministic Cascaded PID Loop
// Target: ESP32-S3 Core 1 FreeRTOS Task (1000Hz / 1ms tick)
#include "esp_timer.h"
#include "driver/mcpwm.h"

void vPIDControlTask(void *pvParameters) {
    TickType_t xLastWakeTime = xTaskGetTickCount();
    const TickType_t xFrequency = pdMS_TO_TICKS(1); // Exact 1000Hz update

    float error = 0.0f, prevError = 0.0f, integral = 0.0f;
    const float Kp = 1.420f, Ki = 0.015f, Kd = 0.285f;
    const float MAX_INTEGRAL = 250.0f; // Anti-windup limit

    for (;;) {
        // Read 15-channel normalized line centroid (-7000 to +7000)
        float currentPosition = getCalibratedLinePosition();
        error = 0.0f - currentPosition; // Center target is 0

        // Anti-windup clamped integral accumulation
        integral += error;
        if (integral > MAX_INTEGRAL) integral = MAX_INTEGRAL;
        else if (integral < -MAX_INTEGRAL) integral = -MAX_INTEGRAL;

        float derivative = (error - prevError);
        float steeringCorrection = (Kp * error) + (Ki * integral) + (Kd * derivative);
        prevError = error;

        // Apply differential PWM duty cycles to Faulhaber motors
        driveMotorsDifferential(BASE_SPEED, steeringCorrection);

        // Deterministic delay: wait until exactly 1ms has elapsed
        vTaskDelayUntil(&xLastWakeTime, xFrequency);
    }
}