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() {
    // 清除标志位
#if 0
    if(isHoming()) {
        if(++home_count_ == max_home_count_) {
            // 强制清零
            home_count_ = 0;
            homeState_ = STATE_INIT;
            ZLOGI(TAG, "【HOME STATE COUNT FORCE CLEAR】");
        }
    }
#endif

    TMC51X0 *motor = AppHardware::ins()->getPump(index_);
    if (motor == nullptr) {
        return;
    }

    bool is_reached = false;
    bool is_move_finished = false;


    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();
    is_reached = motor->isReachTarget(&ramp_stat);

    int32_t postion = 0;
    int32_t distance = 0;
    if (0 == enc_val) {
        postion = x_actual;
    } else {
        postion = enc_val;
    }
    distance = abs(target_pos_ - postion);
    if (distance < 50 && is_reached) {
        is_move_finished = true;
    }

    if (is_reached) {
        is_move_finished = true;
    }

#if 1
#else
    if(is_moving)
    {
        if(0 == enc_val) {
            postion = x_actual;
        }
        else {
            postion = enc_val;
        }

        const bool is_forward = target_pos_ - postion > 0;
        distance = abs(target_pos_ - postion);

        // 检查目标距离与实际距离是否一致
        if(distance < 20 || is_forward != isFoward_) {
            // 电机移动停止
            motor->stop();
            // 发送CAN 消息
            is_moving = false;
            this->runStop();
            ZLOGI(TAG, "MOTOR DETECT STOP!!! ");
#if TIMER_IS_FINE
            setReadRate(READ_RATE_LOW);
#endif

        }
    }
#endif
    bool is_moving = false;
    // 移动中
    osMutexAcquire(mutex_, osWaitForever);
    is_moving = is_moving_;
    if (is_move_finished) {
        is_moving_ = false;
    }
    x_actual_ = x_actual;
    enc_val_ = enc_val;
    rt_speed_ = rt_speed;
    osMutexRelease(mutex_);

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

// 设置读取速率
void ECLMotor::setReadRate(ReadRate rate) {
    readRate = rate;
    uint32_t period;
    switch (rate) {
        case READ_RATE_LOW:
            period = 500; // 1秒
            break;
        case READ_RATE_MEDIUM:
            period = 100; // 500毫秒
            break;
        case READ_RATE_HIGH:
            period = 50; // 50毫秒
            break;
        default:
            period = 1000;
            break;
    }
    osTimerStop(readTimer);
    osTimerStart(readTimer, period);
}

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_moving_ = 0;
    enc_val_ = 0;

    isHomed_ = false;
    homeState_ = STATE_INIT;
    isRunHomingTask = true;

    // 初始读取速率
    readRate = READ_RATE_HIGH;

    // 创建定时器
    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_);
    }


    // 启动定时器
    setReadRate(readRate);
}

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);
    int32_t pos = x_actual_;
    osMutexRelease(mutex_);
    return 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;
    is_moving_ = true;
    osMutexRelease(mutex_);

#if 0
    TMC51X0 *motor = AppHardware::ins()->getPump(index_);
    if(motor == nullptr) {
        return;
    }
    if(is_Flip[index_]) {
        target = -target;
        ZLOGI(TAG, "【MOTOR 【%d】TARGET FLIP】", index_);
    }

    osMutexAcquire(mutex_, osWaitForever);
    int32_t postion = 0;

#if 0
    if(0 == enc_val_) {
        postion = x_actual_;
    }
    else {
        postion = enc_val_;
    }
#else
    if(0 == enc_val_) {
        postion = enc_val_;
    }
    else {
        postion = x_actual_;
    }
#endif

    const bool is_forward = target - postion > 0;
    isFoward_ = is_forward;
    target_pos_ = target;
    is_moving_ = true;
    osMutexRelease(mutex_);
#if TIMER_IS_FINE
    this->setReadRate(READ_RATE_HIGH);
#endif

    int32_t abs_speed = abs(speed_);
    int32_t speed = is_forward ? abs_speed : -abs_speed;

    // 运行
    this->ECL_Rotate(speed, false);
#endif
}

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;
    is_moving_ = true;
    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;
    is_moving_ = true;
    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 0
    if(is_moving_) {
        motor->moveTo(target_pos_, this->target_speed_);
    }
#else
    if (is_moving_) {
        motor->setVMax(this->target_speed_);
    }
#endif
    osMutexRelease(mutex_);
}

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

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

// 获取编码器位置
int32_t ECLMotor::getEncoderPosition() {
    osMutexAcquire(mutex_, osWaitForever);
    int32_t encPos = enc_val_;
    osMutexRelease(mutex_);
    return encPos;
}

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

// 新增设置是否运动的接口
void ECLMotor::setMoving(bool moving) {
    osMutexAcquire(mutex_, osWaitForever);
    is_moving_ = moving;
    osMutexRelease(mutex_);
}

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

bool ECLMotor::isHomed() {
    return isHomed_;
}

bool ECLMotor::isHoming() {
    bool isDoHome = homeState_ != STATE_INIT;
    return isDoHome;
}

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

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

void ECLMotor::startMoveHome() {
    // 避免有相同的操作
    bool isDoHoming = this->isHoming();
    if (isDoHoming) {
        ZLOGI(TAG, "MOTOR [%d] HOME FAILED, Current is Homing", this->index_);
        // return;
    }
    home_count_ = 0; // 开始前清零
    bool isAtOrigin = ELCMotorHelper::isAtOrigin(this->index_);
    ZLOGI(TAG, "START NOT IN HOME")
    // 当前是否在原点
    if (isAtOrigin) {
        home_event_.event_type = EVENT_IN_ORIGIN;
        ZLOGI(TAG, "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 {
        if (homeState_ == STATE_INIT) {
            home_event_.event_type = EVENT_START_HOMING;
            ZLOGI(TAG, "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_finished_ = 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;
            }

            ZLOGI(TAG, "HANDLE NOT HOMED");
#if 1
            switch (home_event.event_type) {
                case EVENT_START_HOMING: {
                    ZLOGI(TAG, "[]EVENT_START_HOMING");
                    if (eclMotor->homeState_ == STATE_INIT) {
                        eclMotor->homeState_ = STATE_BACK_TO_ORIGIN_MIDDLE;
                        // 中速倒转
                        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: {
                    // ZLOGI(TAG, "[]EVENT_ORIGIN_ENTER");
                    // if first enter
                    if (eclMotor->homeState_ == STATE_BACK_TO_ORIGIN_MIDDLE) {
                        // 停止
                        // 中速向前10mm
                        // int speed = ELCMotorHelper::getRPerMinMiddleSpeed(eclMotor->index_);
                        // eclMotor->ECL_Rotate(speed);
                        eclMotor->homeState_ = STATE_LEAVE_ORIGIN_MIDDLE;

                        // 位置清零
                        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->homeState_ == STATE_BACK_TO_ORIGIN_LOW) {
                        // 位置清零
                        // 电机停止
                        // 回归原点成功
                        eclMotor->runhomeSuc();
                    }
                    break;
                }
                case EVENT_ENCODER_NEAR_ORIGN: {
                    // ZLOGI(TAG, "[]EVENT_ENCODER_NEAR_ORIGN");
                    if (eclMotor->homeState_ == STATE_LEAVE_ORIGIN_MIDDLE) {
                        // 停止
                        // 低速回归
                        eclMotor->homeState_ = STATE_BACK_TO_ORIGIN_LOW;

                        int speed = ELCMotorHelper::getRPerMinLowSpeed(eclMotor->index_);
                        eclMotor->ECL_Rotate(-speed);
                        ZLOGI(TAG, "【HOME】 BACK TO ORIGIN LOW");
                    }
                    break;
                }
                case EVENT_IN_ORIGIN: {
                    // ZLOGI(TAG, "[]EVENT_IN_ORIGIN");
                    if (eclMotor->homeState_ == STATE_INIT) {
                        // 中速向前10mm
                        eclMotor->homeState_ = STATE_FORWARD_TO_ORIGIN_MIDDLE;

                        int speed = ELCMotorHelper::getRPerMinMiddleSpeed(eclMotor->index_);
                        eclMotor->ECL_Rotate(speed);

                        ZLOGI(TAG, "【HOME】 FORWARD TO ORIGIN MIDDLE");
                    }
                    break;
                }
                case EVENT_ORIGIN_LEAVE: {
                    // ZLOGI(TAG, "[]EVENT_ORIGIN_LEAVE");
                    if (eclMotor->homeState_ == STATE_FORWARD_TO_ORIGIN_MIDDLE) {
                        // 停止 前向移动 1mm
                        eclMotor->homeState_ = STATE_LEAVE_ORIGIN_MIDDLE;
                        motor->stop();
                        // 位置清零
                        motor->setEncVal(0);
                        motor->setXACTUAL(0);

                        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->homeState_ == STATE_LEAVE_ORIGIN_MIDDLE) {
                        // 低速回归
                        eclMotor->homeState_ = STATE_BACK_TO_ORIGIN_LOW;
                        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");
        }
        ZLOGI(TAG, "RUN MOTOR [END] %d", eclMotor->index_);
    }
}


void ECLMotor::moveToHome() {
#if 0
    TMC51X0 *motor = AppHardware::ins()->getPump(index_);
    if (motor == nullptr) {
        return;
    }
    osMutexAcquire(mutex_, osWaitForever);
    is_moving_ = false;
    osMutexRelease(mutex_);

    int32_t abs_speed = abs(target_speed_);

    int32_t roback_speed = -abs_speed;
#if TIMER_IS_FINE
    this->setReadRate(READ_RATE_HIGH);
#endif

    // 倒转回HOME
    this->ECL_Rotate(roback_speed);
    ZLOGI(TAG, "ECL MOTOR HOME ROBACK SPEED  %d", roback_speed);
#else
    this->startMoveHome();
#endif
}

void ECLMotor::runZeroLimit(bool isEnter) {
    // 处理回home 位逻辑
    if (isHoming()) {
        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();
    osDelay(10); {
        CanMessage msg = CanProtocolFactory::createUploadEvent(seq_id_move_finished_, Motor_Move_Finished, index_);
        AppHardware::ins()->can0Controller.sendMessage(msg);
    }
    osDelay(10);
    // 重置编码器 和 位置
    motor->setXACTUAL(0);
    motor->setEncVal(0);
    osMutexAcquire(mutex_, osWaitForever);
    this->x_actual_ = 0;
    this->enc_val_ = 0;
    this->target_pos_ = 0;
    this->is_moving_ = false;
    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 消息
    this->runStop();
}

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

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

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

void ECLMotor::ECL_Rotate(int32_t speed, const bool is_flip) {
#ifdef  TEST
    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);
#else
    motor->rotate(roback_speed);
#endif
}