StepMotorCtrl/app/tasks/work_task.c

#include "work_task.h"
#include "system_driver.h"
#include "sys_data_bus.h"
#include "stepper_control.h"
#include "limit_switch.h"
#include "led_driver.h"
#include "config.h"

#define DI_MIN  LIMIT_SW_MIN
#define DI_MAX  LIMIT_SW_MAX

#define WORK_TASK_LED_IDLE_MS       500
#define WORK_TASK_LED_RUN_MS        100
#define WORK_TASK_LED_CALIB_MS      80
#define WORK_TASK_LED_STOPPED_MS    0

typedef enum {
    MACHINE_STANDBY = 0,
    MACHINE_MOVING,
    MACHINE_CALIBRATING,
    MACHINE_STOPPED,
} machine_mode_t;

static uint32_t last_run_tick;
static uint32_t run_count;
static machine_mode_t machine_mode;
static uint16_t last_cmd;
static uint32_t led_toggle_tick;
static uint8_t led_state_on;

static void work_update_data_bus(void)
{
    sys_data_bus_value_t val;

    val.i = stepper_get_position();
    sys_data_bus_write(SYS_DATA_BUS_MOTOR_POSITION, val);

    val.i = stepper_get_target();
    sys_data_bus_write(SYS_DATA_BUS_MOTOR_TARGET, val);

    val.u = (uint32_t)stepper_get_state();
    sys_data_bus_write(SYS_DATA_BUS_MOTOR_STATE, val);

    val.u = limit_switch_get_state(DI_MIN) == LIMIT_SW_STATE_TRIGGERED ? 1 : 0;
    sys_data_bus_write(SYS_DATA_BUS_DI_MIN_STATE, val);

    val.u = limit_switch_get_state(DI_MAX) == LIMIT_SW_STATE_TRIGGERED ? 1 : 0;
    sys_data_bus_write(SYS_DATA_BUS_DI_MAX_STATE, val);
}

static void work_update_status(void)
{
    sys_data_bus_value_t val;

    switch (machine_mode) {
        case MACHINE_STANDBY:
            val.u = STEPPER_STATUS_IDLE;
            break;
        case MACHINE_MOVING:
            val.u = STEPPER_STATUS_RUNNING;
            break;
        case MACHINE_CALIBRATING:
            val.u = STEPPER_STATUS_CALIB;
            break;
        case MACHINE_STOPPED:
            val.u = STEPPER_STATUS_STOPPED;
            break;
        default:
            val.u = STEPPER_STATUS_IDLE;
            break;
    }

    sys_data_bus_write(SYS_DATA_BUS_MOTOR_STATUS, val);
}

static uint16_t work_read_command(void)
{
    sys_data_bus_value_t val;

    val = sys_data_bus_read(SYS_DATA_BUS_MOTOR_CMD);

    return (uint16_t)(val.u & 0xFFFF);
}

static void work_clear_command(void)
{
    sys_data_bus_value_t val;

    val.u = STEPPER_CMD_NONE;
    sys_data_bus_write(SYS_DATA_BUS_MOTOR_CMD, val);
}

static void work_dispatch_command(uint16_t cmd)
{
    switch (cmd) {
        case STEPPER_CMD_GO_MAX:
            machine_mode = MACHINE_MOVING;
            stepper_move_to_max();
            break;

        case STEPPER_CMD_GO_MIN:
            machine_mode = MACHINE_MOVING;
            stepper_move_to_min();
            break;

        case STEPPER_CMD_STOP:
            machine_mode = MACHINE_STOPPED;
            stepper_stop();
            break;

        case STEPPER_CMD_GO_POSITION:
        {
            sys_data_bus_value_t target;

            target = sys_data_bus_read(SYS_DATA_BUS_MOTOR_TARGET);
            machine_mode = MACHINE_MOVING;
            stepper_move_to(target.i);
            break;
        }

        case STEPPER_CMD_GO_ANGLE:
        {
            sys_data_bus_value_t angle_reg;

            angle_reg = sys_data_bus_read(SYS_DATA_BUS_ANGLE_TARGET);
            machine_mode = MACHINE_MOVING;
            stepper_move_to_angle((float)angle_reg.u * 0.1f);
            break;
        }

        case STEPPER_CMD_CALIBRATE:
            machine_mode = MACHINE_CALIBRATING;
            stepper_calibrate();
            break;

        case STEPPER_CMD_CALIB_END:
            if (machine_mode == MACHINE_CALIBRATING) {
                stepper_stop();
            }
            machine_mode = MACHINE_STOPPED;
            break;

        case STEPPER_CMD_READ_STATUS:
            break;

        default:
            break;
    }
}

static void work_led_update(void)
{
    uint32_t now;
    uint32_t interval;

    now = system_get_tick();

    switch (machine_mode) {
        case MACHINE_STANDBY:
            interval = WORK_TASK_LED_IDLE_MS;
            break;
        case MACHINE_MOVING:
            interval = WORK_TASK_LED_RUN_MS;
            break;
        case MACHINE_CALIBRATING:
            interval = WORK_TASK_LED_CALIB_MS;
            break;
        case MACHINE_STOPPED:
            led_set(LED_ON);
            return;
        default:
            interval = WORK_TASK_LED_IDLE_MS;
            break;
    }

    if ((now - led_toggle_tick) >= interval) {
        led_toggle_tick = now;
        led_state_on = !led_state_on;
        led_set(led_state_on ? LED_ON : LED_OFF);
    }
}

void work_task_init(void)
{
    last_run_tick = 0;
    run_count = 0;
    machine_mode = MACHINE_STANDBY;
    last_cmd = STEPPER_CMD_NONE;
    led_toggle_tick = 0;
    led_state_on = 0;

    led_init();
}

void work_task_run(void)
{
    uint16_t cmd;

    last_run_tick = system_get_tick();
    run_count++;

    work_update_data_bus();
    work_update_status();
    work_led_update();

    cmd = work_read_command();

    if (cmd != STEPPER_CMD_NONE && cmd != last_cmd) {
        last_cmd = cmd;
        work_clear_command();
        work_dispatch_command(cmd);
    }

    if (machine_mode == MACHINE_MOVING && !stepper_is_running()) {
        machine_mode = MACHINE_STANDBY;
    }

    if (machine_mode == MACHINE_CALIBRATING && !stepper_is_running()) {
        machine_mode = MACHINE_STANDBY;
    }
}

uint32_t work_task_get_last_run_tick(void)
{
    return last_run_tick;
}

uint32_t work_task_get_run_count(void)
{
    return run_count;
}