Sub_Spray_STM32/User/BSP/status/elc_motor.cpp

//
// Created by iflyt on 2025/3/3.
//

#include "elc_motor.h"

#include <base/apphal.hpp>
#include <base/apphardware.hpp>
#include <can_control/can_protocol_factory.h>
#include <cmath>

#include "elc_motor_helper.h"

#define TEST 1

#define TAG "ECL MOTOR"

#define TIMER_IS_FINE 0

using namespace iflytop;


// 定时器回调函数
void ECLMotor::readTimerCallback(void *arg) {
    ECLMotor *motor = static_cast<ECLMotor *>(arg);
    motor->readMotorPosition();
}

// 读取电机位置
void ECLMotor::readMotorPosition() {
    // 清除标志位
    TMC51X0 *motor = AppHardware::ins()->getPump(index_);
    if (motor == nullptr) {
        return;
    }

    osMutexAcquire(mutex_, osWaitForever);
    bool last_is_move_finished = is_move_finished_;
    osMutexRelease(mutex_);

    int32_t x_actual = 0;
    int32_t enc_val = 0;
    int32_t rt_speed = 0;

    enc_val = motor->getEncVal(); // 获取编码器值
    x_actual = motor->getXACTUAL(); // 获取实时位置
    rt_speed = motor->getVACTUAL(); // 获取实时速度

    TMC5130RampStat ramp_stat = motor->getRampStat();
    bool is_move_finished = motor->isReachTarget(&ramp_stat);

    int32_t postion = 0;
    if (0 == enc_val) {
        postion = x_actual;
    } else {
        postion = enc_val;
    }
    // 移动中
    osMutexAcquire(mutex_, osWaitForever);
    is_move_finished_ = is_move_finished;
    x_actual_ = x_actual;
    enc_val_ = enc_val;
    rt_speed_ = rt_speed;
    osMutexRelease(mutex_);

    if (!last_is_move_finished && is_move_finished) {
        //处理移动结束的问题
        if (!isHomeSuc()) {
            ZLOGI(TAG, "[TIMER] HOMING EVENT_MOTOR_MOVE_FINISHED");
            osMutexAcquire(mutex_, osWaitForever);
            home_event_.event_type = EVENT_MOTOR_MOVE_FINISHED;
            osMutexRelease(mutex_);
            osMessageQueuePut(homeEventQueue, &home_event_, 0U, 0U);
        }
        // 发送CAN 消息
        {
#if CAN_MODULE_ENABLE
            CanMessage msg = CanProtocolFactory::createUploadEvent(seq_id_move_finished_, Motor_Move_Finished, index_);
            AppHardware::ins()->can0Controller.sendMessage(msg);
#endif
        }
        ZLOGI(TAG, "【FIRST DETECT】 MOVE FINISHED");
    }
}

// 设置读取速率
void ECLMotor::startReadTimer() {
    osTimerStart(readTimer, 50); // // 50毫秒
}

ECLMotor::ECLMotor() {
    // 创建互斥锁
    mutex_ = osMutexNew(NULL);
    if (mutex_ == NULL) {
        // 处理互斥锁创建失败
        ZLOGI(TAG, "mutex_ create failed");
    }

    // 初始化电机位置和目标位置
    x_actual_ = 0;
    target_pos_ = 0;
    target_speed_ = 10; // 10 / 60 *  6s ==== 6 S 可以转一圈
    is_move_finished_ = false;
    enc_val_ = 0;

    is_home_init_ = false;
    is_home_suc_ = false;
    home_state_ = STATE_INIT;
    isRunHomingTask = true;

    // 创建定时器
    osTimerAttr_t timerAttr = {
        .name = "MotorReadTimer",
        .attr_bits = 0U,
        .cb_mem = NULL,
        .cb_size = 0U
    };
    readTimer = osTimerNew(readTimerCallback, osTimerPeriodic, this, &timerAttr);
    if (readTimer == NULL) {
        // 处理定时器创建失败
    }

    // 创建消息队列
    homeEventQueue = osMessageQueueNew(10, sizeof(HomeEvent), NULL);
    if (homeEventQueue == NULL) {
        // 处理消息队列创建失败的情况
        ZLOGE(TAG, "Failed to create home event message queue");
    }

    // 创建任务并获取线程句柄
    home_task_attr.name = "HomeStateTask";
    home_task_attr.stack_size = 256 * 8;
    home_task_attr.priority = osPriorityHigh;
    homingStateMachineThreadId = osThreadNew(homingTask, this, &home_task_attr);
    if (homingStateMachineThreadId == NULL) {
        // 处理线程创建失败的情况
        ZLOGE(TAG, "THREAD INDEX %d Create Failed", index_);
    }

    // 启动定时器
    startReadTimer();
}

ECLMotor::~ECLMotor() {
    isRunHomingTask = false;
    if (homingStateMachineThreadId != NULL) {
        osThreadTerminate(homingStateMachineThreadId);
    }
    if (homeEventQueue != NULL) {
        osMessageQueueDelete(homeEventQueue);
    }
    osMutexDelete(mutex_);
    osTimerDelete(readTimer);
}

// 读取电机的位置
int32_t ECLMotor::getPosition() {
    osMutexAcquire(mutex_, osWaitForever);
    const int32_t pos = x_actual_;
    osMutexRelease(mutex_);
    const int32_t py_pos = ELCMotorHelper::isFlip(index_) ? -pos : pos;
    return py_pos;
}

// 设置电机的目标位置
void ECLMotor::setTargetPosition(int32_t target) {
    TMC51X0 *motor = AppHardware::ins()->getPump(index_);
    if (motor == nullptr) {
        return;
    }

    if (ELCMotorHelper::isFlip(index_)) {
        target = -target;
        ZLOGI(TAG, "【MOTOR 【%d】TARGET FLIP】", index_);
    }
    motor->moveTo(target, this->target_speed_);
    osMutexAcquire(mutex_, osWaitForever);
    target_pos_ = target;
    osMutexRelease(mutex_);
}

void ECLMotor::setShiftPosition(int32_t shift_target) {
    TMC51X0 *motor = AppHardware::ins()->getPump(index_);
    if (motor == nullptr) {
        return;
    }
    if (ELCMotorHelper::isFlip(index_)) {
        shift_target = -shift_target;
        ZLOGI(TAG, "【MOTOR 【%d】SHIFT TARGET FLIP】", index_);
    }
    motor->moveBy(shift_target, abs(target_speed_));

    osMutexAcquire(mutex_, osWaitForever);
    target_pos_ = target_pos_ + shift_target;
    osMutexRelease(mutex_);
}

void ECLMotor::moveToWithSpeed(int32_t target, int32_t speed) {
    TMC51X0 *motor = AppHardware::ins()->getPump(index_);
    if (motor == nullptr) {
        return;
    }
    if (ELCMotorHelper::isFlip(index_)) {
        target = -target;
        ZLOGI(TAG, "【MOTOR 【%d】TARGET FLIP】", index_);
    }
    motor->moveTo(target, abs(speed));

    osMutexAcquire(mutex_, osWaitForever);
    target_pos_ = target;
    osMutexRelease(mutex_);
}


// 设置电机速度
void ECLMotor::setSpeed(int32_t speed) {
    TMC51X0 *motor = AppHardware::ins()->getPump(index_);
    if (motor == nullptr) {
        return;
    }
    osMutexAcquire(mutex_, osWaitForever);
    this->target_speed_ = speed;
    // 当前正在移动
    if (!is_move_finished_) {
        motor->setVMax(this->target_speed_);
    }
    osMutexRelease(mutex_);
}

// 获取电机速度
int32_t ECLMotor::getRTSpeed() {
    osMutexAcquire(mutex_, osWaitForever);
    const int32_t spd = rt_speed_;
    osMutexRelease(mutex_);
    return spd;
}

// 获取电机运动状态
bool ECLMotor::isMoveFinished() {
    osMutexAcquire(mutex_, osWaitForever);
    bool is_move_finshed = is_move_finished_;
    osMutexRelease(mutex_);
    return is_move_finshed;
}

// 获取编码器位置
int32_t ECLMotor::getEncoderPosition() {
    osMutexAcquire(mutex_, osWaitForever);
    int32_t encPos = enc_val_;
    osMutexRelease(mutex_);
    const int32_t py_pos = ELCMotorHelper::isFlip(index_) ? -encPos : encPos;
    return py_pos;
}

// 设置编码器位置
void ECLMotor::setEncoderPosition(int32_t encoderPos) {
    osMutexAcquire(mutex_, osWaitForever);
    enc_val_ = encoderPos;
    osMutexRelease(mutex_);
}

void ECLMotor::setMotorIndex(int32_t index) {
    this->index_ = index;
}

bool ECLMotor::isHomeInit() {
    return is_home_init_;
}

bool ECLMotor::isHomeSuc() {
    osMutexAcquire(mutex_, osWaitForever);
    bool is_home_suc = is_home_suc_;
    osMutexRelease(mutex_);
    return is_home_suc;
}

void ECLMotor::runhomeSuc() {
    TMC51X0 *motor = AppHardware::ins()->getPump(index_);
    if (motor == nullptr) {
        return;
    }
    // 发送 CAN 信息
    motor->setEncVal(0);
    motor->setXACTUAL(0);

    // 置标志位
    osMutexAcquire(mutex_, osWaitForever);
    this->is_home_init_ = true;
    this->is_home_suc_ = true;
    this->home_state_ = STATE_INIT;
    osMutexRelease(mutex_);

    ZLOGI(TAG, "【 MOVE TO HOME SUC 】");
    {
#if CAN_MODULE_ENABLE
        CanMessage msg = CanProtocolFactory::createUploadEvent(seq_id_home_, Motor_Home_Finished, index_);
        AppHardware::ins()->can0Controller.sendMessage(msg);
#endif
    }
}

void ECLMotor::startMoveHome() {
    home_start_tick_ = 0;

    osMutexAcquire(mutex_, osWaitForever);
    this->is_home_suc_ = false;
    osMutexRelease(mutex_);

    const bool isAtOrigin = ELCMotorHelper::isAtOrigin(this->index_);
    // 当前是否在原点
    if (isAtOrigin) {
        home_event_.event_type = EVENT_IN_ORIGIN;
        ZLOGI(TAG, "[PUT] EVENT IN ORIGIN");
        osStatus_t status = osMessageQueuePut(homeEventQueue, &home_event_, 0U, 0U);
        if (status != osOK) {
            ZLOGE(TAG, "EVENT IN ORIGIN SEND FAIL, status: %d", status);
        }
    } else {
        home_event_.event_type = EVENT_START_HOMING;
        ZLOGI(TAG, "[PUT] EVENT_START_HOMING");
        osStatus_t status = osMessageQueuePut(homeEventQueue, &home_event_, 0U, 0U);
        if (status != osOK) {
            ZLOGE(TAG, "EVENT_START_HOMING SEND FAIL, status: %d", status);
        }
    }
}

void ECLMotor::setHomeSeqId(uint8_t seq_id) {
    this->seq_id_home_ = seq_id;
}

void ECLMotor::setMoveFinishSeqId(uint8_t seq_id) {
    this->seq_id_move_ = seq_id;
}

void ECLMotor::homingTask(void *arg) {
    ECLMotor *eclMotor = static_cast<ECLMotor *>(arg);
    HomeEvent home_event;

    while (eclMotor->isRunHomingTask) {
        osStatus status = osMessageQueueGet(eclMotor->homeEventQueue, &home_event, NULL, osWaitForever);
        if (status == osOK) {
            TMC51X0 *motor = AppHardware::ins()->getPump(eclMotor->index_);
            if (motor == nullptr) {
                return;
            }
#if 1
            switch (home_event.event_type) {
                case EVENT_START_HOMING: {
                        osMutexAcquire(eclMotor->mutex_, osWaitForever);
                        eclMotor->home_state_ = STATE_BACK_TO_ORIGIN_MIDDLE;
                        osMutexRelease(eclMotor->mutex_);
                        // 中速倒转
                        int speed = ELCMotorHelper::getRPerMinMiddleSpeed(eclMotor->index_);
                        int absSpeed = abs(speed);
                        eclMotor->ECL_Rotate(-absSpeed);
                        ZLOGI(TAG, "【HOME】 BACK TO ORIGIN MIDDLE");
                    break;
                }
                case EVENT_ORIGIN_ENTER: {
                    // if first enter
                    if(eclMotor->home_state_ == STATE_BACK_TO_ORIGIN_MIDDLE) {
                        // 停止
                        // 中速向前10mm
                        osMutexAcquire(eclMotor->mutex_, osWaitForever);
                        eclMotor->home_state_ = STATE_LEAVE_ORIGIN_MIDDLE;
                        osMutexRelease(eclMotor->mutex_);

                        // 位置清零
                        motor->setEncVal(0);
                        motor->setXACTUAL(0);

                        const int32_t speed = ELCMotorHelper::getRPerMinMiddleSpeed(eclMotor->index_);
                        eclMotor->moveToWithSpeed(ELCMotorHelper::getForwordMoveStep(eclMotor->index_), speed);
                        // 4000 4 圈
                        ZLOGI(TAG, "【HOME】 [EVENT_ORIGIN_ENTER] FORWARD TO ORIGIN MIDDLE");
                    }
                    // if second enter
                    if (eclMotor->home_state_ == STATE_BACK_TO_ORIGIN_LOW) {
                        // 位置清零
                        // 电机停止
                        // 回归原点成功
                        eclMotor->runhomeSuc();
                    }
                    break;
                }
                case EVENT_IN_ORIGIN: {
                    ZLOGI(TAG, "[]EVENT IN ORIGIN");
                    // 中速向前10mm
                    motor->setEncVal(0);
                    motor->setXACTUAL(0);

                    // 前向移动 1mm
                    osMutexAcquire(eclMotor->mutex_, osWaitForever);
                    eclMotor->home_state_ = STATE_LEAVE_ORIGIN_MIDDLE;
                    osMutexRelease(eclMotor->mutex_);

                    const int32_t speed = ELCMotorHelper::getRPerMinMiddleSpeed(eclMotor->index_);
                    eclMotor->moveToWithSpeed(ELCMotorHelper::getForwordMoveStep(eclMotor->index_), speed);
                    ZLOGI(TAG, "【HOME】 FORWARD TO ORIGIN MIDDLE");
                    break;
                }
                case EVENT_MOTOR_MOVE_FINISHED: {
                    ZLOGI(TAG, "[]EVENT MOTOR MOVE FINISHED");
                    if (eclMotor->home_state_ == STATE_LEAVE_ORIGIN_MIDDLE) {
                        // 低速回归
                        osMutexAcquire(eclMotor->mutex_, osWaitForever);
                        eclMotor->home_state_ = STATE_BACK_TO_ORIGIN_LOW;
                        osMutexRelease(eclMotor->mutex_);
                        const int speed = ELCMotorHelper::getRPerMinLowSpeed(eclMotor->index_);
                        eclMotor->ECL_Rotate(-speed);
                        ZLOGI(TAG, "【HOME】 BACK TO ORIGIN LOW");
                    }
                    break;
                }
                default:
                    break;
            }
#endif
        } else {
            ZLOGI(TAG, "GET MESSAGE FAILED");
        }
    }
}


void ECLMotor::moveToHome() {
    this->startMoveHome();
}

void ECLMotor::runZeroLimit(bool isEnter) {
    // 处理回home 位逻辑
    if (!isHomeSuc()) {
        TMC51X0 *motor = AppHardware::ins()->getPump(index_);
        if (motor == nullptr) {
            return;
        }

        if (isEnter) {
            home_event_.event_type = EVENT_ORIGIN_ENTER;
            motor->stop();
            osDelay(10);
            ZLOGI(TAG, "[PUT] EVENT_ORIGIN_ENTER");
        } else {
            home_event_.event_type = EVENT_ORIGIN_LEAVE;
            ZLOGI(TAG, "[PUT] EVENT_ORIGIN_LEAVE");
        }

        osMessageQueuePut(homeEventQueue, &home_event_, 0U, 0U);
        return;
    }

    if (!isEnter) {
        return;
    }

    TMC51X0 *motor = AppHardware::ins()->getPump(index_);
    if (motor == nullptr) {
        return;
    }
    motor->stop();
    {
#if CAN_MODULE_ENABLE
        CanMessage msg = CanProtocolFactory::createUploadEvent(seq_id_move_, Motor_Move_Finished, index_);
        AppHardware::ins()->can0Controller.sendMessage(msg);
#endif
    }
    // 重置编码器 和 位置
    motor->setXACTUAL(0);
    motor->setEncVal(0);
    osMutexAcquire(mutex_, osWaitForever);
    this->x_actual_ = 0;
    this->enc_val_ = 0;
    this->target_pos_ = 0;
    osMutexRelease(mutex_);
    ZLOGI("ECL", "MOTOR [%d] runZeroLimit", this->index_);
}

void ECLMotor::runEndLimit(bool isEnter) {
    // 发送CAN 消息
    if (!isEnter) {
        return;
    }
    this->runStop();
}

void ECLMotor::runE_Stop() {
    // 发送CAN 消息
    osMutexAcquire(mutex_, osWaitForever);
    this->is_home_suc_ = false;
    osMutexRelease(mutex_);

    this->runStop();
}

void ECLMotor::runPause() {
    // 发送CAN 消息
    this->runStop();
}

void ECLMotor::runStop() {
    TMC51X0 *motor = AppHardware::ins()->getPump(index_);
    if (motor == nullptr) {
        return;
    }
    motor->stop();
    // 发送CAN 消息
#if CAN_MODULE_ENABLE
    CanMessage msg = CanProtocolFactory::createUploadEvent(seq_id_home_, Motor_Move_Finished, index_);
    AppHardware::ins()->can0Controller.sendMessage(msg);
#endif
}

void ECLMotor::ECL_Rotate(int32_t speed, const bool is_flip) {
    TMC51X0 *motor = AppHardware::ins()->getPump(index_);
    if (motor == nullptr) {
        return;
    }

    if (ELCMotorHelper::isFlip(index_) && is_flip) {
        speed = -speed;
        ZLOGI(TAG, "ECL MOTOR ECL REAL INDEX 【%d】 FLIP", index_);
    }

    motor->rotate(speed);
    ZLOGI(TAG, "ECL MOTOR %d SPEED  %d", index_, speed);
}