AutoFilm-ESP32/components/hal_motor/hal_motor.c

#include <stdint.h>
#include <stdbool.h>
#include <stdatomic.h>

#include "esp_err.h"
#include "esp_log.h"
#include "driver/gpio.h"
#include "driver/rmt_tx.h"
#include "driver/rmt_encoder.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

#include "board.h"
#include "hal_motor.h"

#define STEPS_PER_REV 4800
#define DEFAULT_RPM 60
#define ACCEL 9600
#define RMT_RES_HZ 1000000
#define CRUISE_HZ ((DEFAULT_RPM * STEPS_PER_REV) / 60) /* 4800 */
#define RAMP_STAGES 8
#define BATCH 32

static const char *TAG = "motor";

static gpio_num_t s_pin_en = GPIO_NUM_NC;
static gpio_num_t s_pin_dir = GPIO_NUM_NC;
static gpio_num_t s_pin_step = GPIO_NUM_NC;
static int s_en_disable_level = 1;
static TaskHandle_t s_task;
static atomic_uint s_stop_req;
static float s_cw;
static float s_ccw;
static uint32_t s_rpm = DEFAULT_RPM;
static bool s_enabled;
static bool s_inited;
static rmt_channel_handle_t s_chan;
static rmt_encoder_handle_t s_enc;

static void en_disable(void)
{
    if (s_pin_en != GPIO_NUM_NC) {
        gpio_set_level(s_pin_en, s_en_disable_level);
    }
    s_enabled = false;
}

static void en_enable(void)
{
    if (s_pin_en != GPIO_NUM_NC) {
        gpio_set_level(s_pin_en, !s_en_disable_level);
    }
    s_enabled = true;
}

static void rmt_abort(void)
{
    if (s_chan != NULL) {
        rmt_disable(s_chan);
        rmt_enable(s_chan);
    }
}

static uint32_t period_us_for_hz(uint32_t hz)
{
    if (hz < 100) {
        hz = 100;
    }
    return RMT_RES_HZ / hz;
}

static bool emit_steps(uint32_t n, uint32_t hz)
{
    if (n == 0) {
        return true;
    }
    uint32_t period = period_us_for_hz(hz);
    uint32_t half = period / 2;
    if (half == 0) {
        half = 1;
    }
    rmt_symbol_word_t sym = {
        .duration0 = (uint16_t)half,
        .level0 = 1,
        .duration1 = (uint16_t)(period - half),
        .level1 = 0,
    };
    while (n > 0) {
        if (atomic_load(&s_stop_req) != 0) {
            rmt_abort();
            return false;
        }
        uint32_t chunk = n > BATCH ? BATCH : n;
        rmt_symbol_word_t burst[BATCH];
        for (uint32_t i = 0; i < chunk; i++) {
            burst[i] = sym;
        }
        rmt_transmit_config_t tcfg = {
            .loop_count = 0,
        };
        if (rmt_transmit(s_chan, s_enc, burst, chunk * sizeof(rmt_symbol_word_t), &tcfg) != ESP_OK) {
            return false;
        }
        if (rmt_tx_wait_all_done(s_chan, 2000) != ESP_OK) {
            rmt_abort();
            return false;
        }
        n -= chunk;
    }
    return atomic_load(&s_stop_req) == 0;
}

static bool run_move(long steps)
{
    if (steps == 0) {
        return true;
    }
    int dir = (steps > 0) ? 1 : 0;
    if (steps < 0) {
        steps = -steps;
    }
    gpio_set_level(s_pin_dir, dir);

    uint32_t cruise = (s_rpm * STEPS_PER_REV) / 60;
    if (cruise == 0) {
        cruise = CRUISE_HZ;
    }
    /* Short accel table: ACCEL ≈9600 steps/s^2 into cruise. */
    uint32_t remaining = (uint32_t)steps;
    uint32_t ramp = remaining / 8;
    if (ramp > 200) {
        ramp = 200;
    }
    if (ramp < RAMP_STAGES) {
        ramp = (remaining < RAMP_STAGES) ? remaining : RAMP_STAGES;
    }
    uint32_t start_hz = cruise / 4;
    if (start_hz < 200) {
        start_hz = 200;
    }
    for (int i = 0; i < RAMP_STAGES && remaining > 0; i++) {
        uint32_t n = ramp / RAMP_STAGES;
        if (n == 0) {
            n = 1;
        }
        if (n > remaining) {
            n = remaining;
        }
        uint32_t hz = start_hz + ((cruise - start_hz) * (uint32_t)(i + 1)) / RAMP_STAGES;
        if (!emit_steps(n, hz)) {
            return false;
        }
        remaining -= n;
    }
    if (remaining > 0) {
        if (!emit_steps(remaining, cruise)) {
            return false;
        }
    }
    return true;
}

static void motor_task(void *arg)
{
    (void)arg;
    for (;;) {
        ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
        if (atomic_load(&s_stop_req) != 0) {
            rmt_abort();
            en_disable();
            continue;
        }
        en_enable();
        while (atomic_load(&s_stop_req) == 0) {
            if (!run_move((long)((float)STEPS_PER_REV * s_cw))) {
                break;
            }
            if (atomic_load(&s_stop_req) != 0) {
                break;
            }
            if (!run_move((long)(-((float)STEPS_PER_REV * s_ccw)))) {
                break;
            }
        }
        rmt_abort();
        en_disable();
    }
}

void autofilm_motor_task(void *arg)
{
    motor_task(arg);
}

void hal_motor_init(void)
{
    if (s_inited) {
        return;
    }
    s_pin_en = board_pin_motor_en();
    s_pin_dir = board_pin_motor_dir();
    s_pin_step = board_pin_motor_step();
    s_en_disable_level = board_motor_en_disable_level();
    if (s_pin_en == GPIO_NUM_NC || s_pin_dir == GPIO_NUM_NC || s_pin_step == GPIO_NUM_NC) {
        ESP_LOGW(TAG, "no motor pins on this board");
        s_inited = true;
        return;
    }
    gpio_config_t io = {
        .pin_bit_mask = (1ULL << s_pin_en) | (1ULL << s_pin_dir),
        .mode = GPIO_MODE_OUTPUT,
        .pull_up_en = GPIO_PULLUP_DISABLE,
        .pull_down_en = GPIO_PULLDOWN_DISABLE,
        .intr_type = GPIO_INTR_DISABLE,
    };
    gpio_config(&io);
    gpio_set_level(s_pin_en, s_en_disable_level);
    gpio_set_level(s_pin_dir, 0);
    s_enabled = false;
    atomic_store(&s_stop_req, 0);

    rmt_tx_channel_config_t txcfg = {
        .clk_src = RMT_CLK_SRC_DEFAULT,
        .gpio_num = s_pin_step,
        .mem_block_symbols = 64,
        .resolution_hz = RMT_RES_HZ,
        .trans_queue_depth = 4,
    };
    rmt_new_tx_channel(&txcfg, &s_chan);
    rmt_copy_encoder_config_t enc_cfg = {};
    rmt_new_copy_encoder(&enc_cfg, &s_enc);
    rmt_enable(s_chan);

    if (s_task == NULL) {
        xTaskCreatePinnedToCore(motor_task, "motor", 4096, NULL,
                                configMAX_PRIORITIES - 2, &s_task, 0);
    }
    s_inited = true;
    ESP_LOGI(TAG, "init en=%d step=%d dir=%d dis_lvl=%d",
             s_pin_en, s_pin_step, s_pin_dir, s_en_disable_level);
}

void hal_motor_enable(bool on)
{
    if (on) {
        en_enable();
    } else {
        en_disable();
    }
}

void hal_motor_request_stop(void)
{
    atomic_store(&s_stop_req, 1);
    en_disable();
    rmt_abort();
    if (s_task != NULL) {
        xTaskNotifyGive(s_task);
    }
}

esp_err_t hal_motor_agitate_start(float cw_revs, float ccw_revs, uint32_t rpm)
{
    s_cw = cw_revs;
    s_ccw = ccw_revs;
    s_rpm = (rpm == 0) ? DEFAULT_RPM : rpm;
    atomic_store(&s_stop_req, 0);
    if (s_task != NULL) {
        xTaskNotifyGive(s_task);
    }
    return ESP_OK;
}

void hal_motor_agitate_stop(void)
{
    atomic_store(&s_stop_req, 1);
    rmt_abort();
    en_disable();
}

bool hal_motor_is_enabled(void)
{
    return s_enabled;
}