jaula.c

#include <Arduino_FreeRTOS.h>
#include <queue.h>
#include <Stepper.h>
#include <Ultrasonic.h>

#define pino_trigger 4
#define pino_echo 5

void TaskUltrasonic(void * pvParameters);
void TaskStepper(void * pvParameters);
void TaskButton(void * pvParameters);

QueueHandle_t xStepperQueue;
QueueHandle_t xUltrasonicQueue;

void setup() {

    xStepperQueue = xQueueCreate(5, sizeof(uint8_t));
    xUltrasonicQueue = xQueueCreate(5, sizeof(uint8_t));

    xTaskCreate(
        TaskUltrasonic, (const portCHAR * )
        "TaskUltrasonic", 128, NULL, 2, NULL);

    xTaskCreate(
        TaskStepper, (const portCHAR * )
        "TaskStepper", 128 // This stack size can be checked & adjusted by reading Highwater
        , NULL, 3 // priority
        , NULL);

    xTaskCreate(
        TaskButton, (const portCHAR * )
        "TaskButton", 128 // This stack size can be checked & adjusted by reading Highwater
        , NULL, 3 // priority
        , NULL);
}

void loop() {

}
/*--------------------------------------------------*/
/*---------------------- Tasks ---------------------*/
/*--------------------------------------------------*/

void TaskUltrasonic(void * pvParameters) {
    (void) pvParameters;

    Ultrasonic ultrasonic(pino_trigger, pino_echo);

    // Serial.begin(9600);

    uint8_t state;

    int directionB = 2;
    int detected = false;

    while (1) {
        float cmMsec;
        long microsec = ultrasonic.timing();
        cmMsec = ultrasonic.convert(microsec, Ultrasonic::CM);

        if (xQueueReceive(xUltrasonicQueue, & state, 1)) {
            if (state == 1) {
                detected = false;
            }
        }

        //Serial.println(cmMsec);

        if (cmMsec < 10.0 && detected == false) {
            detected = true;
            xQueueSendToBack(xStepperQueue, & directionB, 0);
        }

        vTaskDelay(50 / portTICK_PERIOD_MS);
    }
}

void TaskStepper(void * pvParameters) {
    (void) pvParameters;

    const int stepsPerRevolution = 500;
    Stepper myStepper(stepsPerRevolution, 8, 10, 9, 11);
    myStepper.setSpeed(64);

    uint8_t state;

    int directionB = 0;

    while (1) {
        if (xQueueReceive(xStepperQueue, & state, portMAX_DELAY)) {
            if (state == 1) {
                directionB = 1;
            } else if (state == 2) {
                directionB = -1;
            }
        }

        int k, stepV, stepD;
        stepV = 200;

        for (k = 0; k <= 30; k++) {
            stepD = stepV * directionB * 1;
            myStepper.step(stepD);

            if (k == 30) {
                directionB = 0;
            }

            vTaskDelay(10 / portTICK_PERIOD_MS);
        }
    }

}

void TaskButton(void * pvParameters) {
    (void) pvParameters;

    int state = 0;
    int newstate = 0;

    const int button_a = 2;

    pinMode(button_a, INPUT);

    while (1) {
        if (digitalRead(button_a) != state) {
            vTaskDelay(20);

            newstate = digitalRead(button_a);
            if (newstate != state) {
                state = newstate;

                if (state == HIGH) {
                    xQueueSendToBack(xStepperQueue, & state, 0);
                    xQueueSendToBack(xUltrasonicQueue, & state, 0);
                }
            }
        }

        vTaskDelay(20 / portTICK_PERIOD_MS);
    }
}