#include <Arduino.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>

// Task function prototypes
void TaskMotorControl(void *pvParameters);
void TaskSensorData(void *pvParameters);

void setup() {
    Serial.begin(115200);

    // Create Motor Control Task
    xTaskCreate(
        TaskMotorControl,  // Task function
        "Motor Control",   // Task name
        2048,              // Stack size in bytes
        NULL,              // Task input parameter
        2,                 // Priority (higher number = higher priority)
        NULL);             // Task handle

    // Create Sensor Data Task
    xTaskCreate(
        TaskSensorData,    // Task function
        "Sensor Data",     // Task name
        2048,              // Stack size in bytes
        NULL,              // Task input parameter
        1,                 // Priority
        NULL);             // Task handle
}

void loop() {
    // FreeRTOS tasks run independently of the loop
}

void TaskMotorControl(void *pvParameters) {
    while (true) {
        // Example motor control code
        Serial.println("Controlling motor...");
        vTaskDelay(pdMS_TO_TICKS(100));  // Delay for 100 milliseconds
    }
}

void TaskSensorData(void *pvParameters) {
    while (true) {
        // Example sensor data acquisition code
        Serial.println("Reading sensor data...");
        vTaskDelay(pdMS_TO_TICKS(500));  // Delay for 500 milliseconds
    }
}