main.cpp

#include <Arduino.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <freertos/queue.h>
#include <esp_timer.h>
#include <math.h>

// ================================================================
//  MOD SECIMI — Sadece birini aktif birak!
//  MODE_COLLECT   : Veri toplama (Serial'e ham veri basar)
//  MODE_INFERENCE : Gercek zamanli tahmin
// ================================================================
// #define MODE_COLLECT
#define MODE_INFERENCE

#ifdef MODE_INFERENCE
#include "model.h"
// ---------------------------------------------------------------
//  ESP32‑S3 EMG REAL‑TIME INFERENCE (model.h ONLY)
// ---------------------------------------------------------------

Eloquent::ML::Port::RandomForest clf;   // model generated by micromlgen
#endif

// -----------------------------------------------------------------
//  SETTINGS
// -----------------------------------------------------------------
const int EMG_PIN = 4;                 // ADC1_CH3
const int SAMPLING_RATE_HZ = 2000;     // 2 kHz acquisition
const int64_t SAMPLE_PERIOD_US = 1000000 / SAMPLING_RATE_HZ;

// -----------------------------------------------------------------
//  50 Hz NOTCH FILTER (IIR, order‑2)
// -----------------------------------------------------------------
const float NOTCH_FREQ = 50.0;
const float NOTCH_R    = 0.95;
const float cosW0 = cos(2.0 * PI * NOTCH_FREQ / SAMPLING_RATE_HZ);
const float K0 = (1.0 - 2.0 * NOTCH_R * cosW0 + NOTCH_R * NOTCH_R) / (2.0 - 2.0 * cosW0);
const float b0 = K0;
const float b1 = -2.0 * K0 * cosW0;
const float b2 = K0;
const float a1 = -2.0 * NOTCH_R * cosW0;
const float a2 = NOTCH_R * NOTCH_R;

float x_prev1 = 0, x_prev2 = 0;   // input history
float y_prev1 = 0, y_prev2 = 0;   // output history

float applyNotchFilter(float x) {
    float y = b0 * x + b1 * x_prev1 + b2 * x_prev2 - a1 * y_prev1 - a2 * y_prev2;
    x_prev2 = x_prev1;  x_prev1 = x;
    y_prev2 = y_prev1;  y_prev1 = y;
    return y;
}

// -----------------------------------------------------------------
//  FIR LOW-PASS FILTER (10-tap Moving Average)
// -----------------------------------------------------------------
const int FIR_TAPS = 10;
float fir_buffer[FIR_TAPS] = {0};
int fir_idx = 0;

float applyFIRFilter(float x) {
    fir_buffer[fir_idx] = x;
    fir_idx = (fir_idx + 1) % FIR_TAPS;
    
    float sum = 0;
    for (int i = 0; i < FIR_TAPS; ++i) {
        sum += fir_buffer[i];
    }
    return sum / (float)FIR_TAPS;
}

// -----------------------------------------------------------------
//  QUEUE AND DATA STRUCTURE
// -----------------------------------------------------------------
QueueHandle_t emgQueue;
struct EMGData { uint16_t value; };

// -----------------------------------------------------------------
//  ADC TASK – reads raw ADC, removes DC, applies notch, recenters, pushes
// -----------------------------------------------------------------
void adcTask(void *pvParameters) {
    EMGData data;
    int64_t nextSampleTime = esp_timer_get_time();
    float dc_offset = 2047.0;   // initial centre (mid‑scale)

    // quick DC calibration (200 samples)
    float sum = 0;
    for (int i = 0; i < 200; ++i) {
        sum += analogRead(EMG_PIN);
        delayMicroseconds(500);
    }
    dc_offset = sum / 200.0;
    Serial.print("[OK] Initial DC offset: ");
    Serial.println(dc_offset);

    for (;;) {
        uint16_t rawVal = analogRead(EMG_PIN);
        // ultra‑slow high‑pass to track DC
        dc_offset = 0.999 * dc_offset + 0.001 * (float)rawVal;
        // AC component
        float ac_val = (float)rawVal - dc_offset;
        // notch filter on AC
        float filtered_ac = applyNotchFilter(ac_val);
        // FIR low-pass filter on AC
        float fir_ac = applyFIRFilter(filtered_ac);
        // bring back to centre (2047 raw = 1.65 V)
        float final_val = fir_ac + 2047.0;
        // clip to ADC range
        if (final_val < 0)   final_val = 0;
        if (final_val > 4095) final_val = 4095;
        data.value = (uint16_t)final_val;   // filtered value used for inference
        xQueueSend(emgQueue, &data, 0);

        // timing – keep 2 kHz loop
        nextSampleTime += SAMPLE_PERIOD_US;
        int64_t now = esp_timer_get_time();
        if (nextSampleTime > now) {
            delayMicroseconds(nextSampleTime - now);
        }
    }
}

// -----------------------------------------------------------------
//  SERIAL TASK – sliding‑window feature extraction + model inference
// -----------------------------------------------------------------
const int WINDOW_SIZE = 50;   // corresponds to ~25 ms at 2 kHz
const int STEP_SIZE   = 25;   // 50 % overlap
float window_buffer[WINDOW_SIZE];
int   window_idx = 0;

void serialTask(void *pvParameters) {
    EMGData data;
    for (;;) {
        if (xQueueReceive(emgQueue, &data, portMAX_DELAY) == pdPASS) {
#ifdef MODE_COLLECT
            // Python plotting.py kodunun okuyabilmesi icin SADECE sayiyi yolla
            Serial.println(data.value);
#endif

#ifdef MODE_INFERENCE
            // fill window (ADC -> volts to match training data scale)
            window_buffer[window_idx++] = (float)data.value * 3.3f / 4095.0f;
            if (window_idx >= WINDOW_SIZE) {
                // ----- feature extraction -----
                float sum = 0, sum_sq = 0;
                float min_val = window_buffer[0];
                float max_val = window_buffer[0];
                for (int i = 0; i < WINDOW_SIZE; ++i) {
                    float v = window_buffer[i];
                    sum    += v;
                    sum_sq += v * v;
                    if (v < min_val) min_val = v;
                    if (v > max_val) max_val = v;
                }
                float mean_val = sum / (float)WINDOW_SIZE;
                float rms_val  = sqrt(sum_sq / (float)WINDOW_SIZE);
                // variance & std
                float var_sum = 0;
                for (int i = 0; i < WINDOW_SIZE; ++i) {
                    var_sum += (window_buffer[i] - mean_val) * (window_buffer[i] - mean_val);
                }
                float var_val = var_sum / (float)WINDOW_SIZE;
                float std_val = sqrt(var_val);
                float features[6] = {mean_val, std_val, var_val, rms_val, min_val, max_val};
                // ----- inference -----
                int prediction = clf.predict(features);
                // ----- STM32'ye UART paketi: [0xAA][prediction][0x55] -----
                uint8_t packet[3] = {0xAA, (uint8_t)prediction, 0x55};
                Serial1.write(packet, 3);
                // ----- output -----
                Serial.print("Raw:");
                Serial.print(data.value);
                Serial.print(", Prediction:");
                Serial.print(prediction * 1000); // 0 / 1000 / 2000 for easy plotting
                if (prediction == 0)      Serial.println(" // REST (Idle)");
                else if (prediction == 1) Serial.println(" // BICEPS");
                else if (prediction == 2) Serial.println(" // ELBOW");
                else                      Serial.println();
                // slide window (keep last STEP_SIZE samples)
                for (int i = 0; i < (WINDOW_SIZE - STEP_SIZE); ++i) {
                    window_buffer[i] = window_buffer[i + STEP_SIZE];
                }
                window_idx = WINDOW_SIZE - STEP_SIZE;
            }
#endif
        }
    }
}

// -----------------------------------------------------------------
//  SETUP & LOOP
// -----------------------------------------------------------------
void setup() {
    Serial.begin(921600);
    Serial1.begin(115200, SERIAL_8N1, 18, 17);  // STM32 hatti: RX=GPIO18, TX=GPIO17
    delay(1000);
#ifdef MODE_COLLECT
    Serial.println("--- EMG VERI TOPLAMA MODU (Tek Kanal) ---");
#endif
#ifdef MODE_INFERENCE
    Serial.println("--- EMG REAL‑TIME INFERENCE (model.h) ---");
#endif
    analogSetAttenuation(ADC_11db);   // 0‑3.3 V
    analogReadResolution(12);         // 0‑4095

    // create queue
    emgQueue = xQueueCreate(200, sizeof(EMGData));
    if (emgQueue == NULL) {
        Serial.println("[HATA] Queue creation failed");
        return;
    }
    // start tasks
    xTaskCreatePinnedToCore(adcTask,    "ADC_Task",    2048, NULL, 2, NULL, 0);
    xTaskCreatePinnedToCore(serialTask, "Serial_Task", 4096, NULL, 1, NULL, 1);
    Serial.println("[OK] Tasks started");
}

void loop() {
    // nothing – FreeRTOS tasks run independently
    vTaskDelete(NULL);
}