diff --git a/User/BSP/base/apphardware.cpp b/User/BSP/base/apphardware.cpp index bfefa5c..9d3ab15 100644 --- a/User/BSP/base/apphardware.cpp +++ b/User/BSP/base/apphardware.cpp @@ -55,7 +55,7 @@ void AppHardware::initialize() { #if 0 MOTO1.setIHOLD_IRUN(0, 7, 100); #else - MOTO1.setIHOLD_IRUN(0, 8, 100); + MOTO1.setIHOLD_IRUN(5, 10, 100); #endif MOTO1.setScale(1000); MOTO1.setEncResolution(-1000); @@ -68,7 +68,7 @@ void AppHardware::initialize() { #if 0 MOTO2.setIHOLD_IRUN(0, 10, 100); #else - MOTO2.setIHOLD_IRUN(0, 8, 100); + MOTO2.setIHOLD_IRUN(5, 10, 100); #endif MOTO2.setScale(1000); MOTO2.setEncResolution(-1000); @@ -78,7 +78,7 @@ void AppHardware::initialize() { MOTO3.initialize(2, tmc5130cfg3); MOTO3.enable(false); - MOTO3.setIHOLD_IRUN(0, 8, 100); + MOTO3.setIHOLD_IRUN(5, 10, 100); MOTO3.setScale(1000); MOTO3.setEncResolution(-1000); diff --git a/User/BSP/status/elc_motor.cpp b/User/BSP/status/elc_motor.cpp index 4cfb506..500f0ed 100644 --- a/User/BSP/status/elc_motor.cpp +++ b/User/BSP/status/elc_motor.cpp @@ -65,7 +65,7 @@ void ECLMotor::readMotorPosition() { if (!last_is_move_finished && is_move_finished) { //处理移动结束的问题 - if (!isHomeSuc()) { + if (isRunHomingTask) { ZLOGI(TAG, "[TIMER] HOMING EVENT_MOTOR_MOVE_FINISHED"); osMutexAcquire(mutex_, osWaitForever); home_event_.event_type = EVENT_MOTOR_MOVE_FINISHED; @@ -106,7 +106,7 @@ ECLMotor::ECLMotor() { is_home_init_ = false; is_home_suc_ = false; home_state_ = STATE_INIT; - isRunHomingTask = true; + isRunHomingTask = false; // 创建定时器 osTimerAttr_t timerAttr = { @@ -263,10 +263,6 @@ 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_; @@ -288,6 +284,7 @@ void ECLMotor::runhomeSuc() { this->is_home_init_ = true; this->is_home_suc_ = true; this->home_state_ = STATE_INIT; + isRunHomingTask = false; osMutexRelease(mutex_); ZLOGI(TAG, "【 MOVE TO HOME SUC 】"); @@ -300,23 +297,26 @@ void ECLMotor::runhomeSuc() { } void ECLMotor::startMoveHome() { - home_start_tick_ = 0; - osMutexAcquire(mutex_, osWaitForever); this->is_home_suc_ = false; + isRunHomingTask = true; osMutexRelease(mutex_); const bool isAtOrigin = ELCMotorHelper::isAtOrigin(this->index_); // 当前是否在原点 if (isAtOrigin) { + osMutexAcquire(mutex_, osWaitForever); home_event_.event_type = EVENT_IN_ORIGIN; + osMutexRelease(mutex_); 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 { + osMutexAcquire(mutex_, osWaitForever); home_event_.event_type = EVENT_START_HOMING; + osMutexRelease(mutex_); ZLOGI(TAG, "[PUT] EVENT_START_HOMING"); osStatus_t status = osMessageQueuePut(homeEventQueue, &home_event_, 0U, 0U); if (status != osOK) { @@ -337,8 +337,11 @@ void ECLMotor::homingTask(void *arg) { ECLMotor *eclMotor = static_cast(arg); HomeEvent home_event; - while (eclMotor->isRunHomingTask) { + while (true) { osStatus status = osMessageQueueGet(eclMotor->homeEventQueue, &home_event, NULL, osWaitForever); + if(!eclMotor->isRunHomingTask) { + continue; + } if (status == osOK) { TMC51X0 *motor = AppHardware::ins()->getPump(eclMotor->index_); if (motor == nullptr) { @@ -354,7 +357,7 @@ void ECLMotor::homingTask(void *arg) { int speed = ELCMotorHelper::getRPerMinMiddleSpeed(eclMotor->index_); int absSpeed = abs(speed); eclMotor->ECL_Rotate(-absSpeed); - ZLOGI(TAG, "【HOME】 BACK TO ORIGIN MIDDLE"); + ZLOGI(TAG, "【HOME】 BACK TO ORIGIN MIDDLE 中速回原点"); break; } case EVENT_ORIGIN_ENTER: { @@ -373,7 +376,7 @@ void ECLMotor::homingTask(void *arg) { 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"); + ZLOGI(TAG, "【HOME】 [EVENT_ORIGIN_ENTER] FORWARD TO ORIGIN MIDDLE 中速向前10mm"); } // if second enter if (eclMotor->home_state_ == STATE_BACK_TO_ORIGIN_LOW) { @@ -381,11 +384,12 @@ void ECLMotor::homingTask(void *arg) { // 电机停止 // 回归原点成功 eclMotor->runhomeSuc(); + ZLOGI(TAG, "【HOME】 [EVENT_ORIGIN_ENTER] HOME SUC 回原点成功"); } break; } case EVENT_IN_ORIGIN: { - ZLOGI(TAG, "[]EVENT IN ORIGIN"); + ZLOGI(TAG, "[]EVENT IN ORIGIN 进入原点"); // 中速向前10mm motor->setEncVal(0); motor->setXACTUAL(0); @@ -397,28 +401,40 @@ void ECLMotor::homingTask(void *arg) { const int32_t speed = ELCMotorHelper::getRPerMinMiddleSpeed(eclMotor->index_); eclMotor->moveToWithSpeed(ELCMotorHelper::getForwordMoveStep(eclMotor->index_), speed); - ZLOGI(TAG, "【HOME】 FORWARD TO ORIGIN MIDDLE"); + ZLOGI(TAG, "【HOME】 FORWARD TO ORIGIN MIDDLE 中速向前10mm"); break; } case EVENT_MOTOR_MOVE_FINISHED: { - ZLOGI(TAG, "[]EVENT MOTOR MOVE FINISHED"); - if (eclMotor->home_state_ == STATE_LEAVE_ORIGIN_MIDDLE) { + ZLOGI(TAG, "[]EVENT MOTOR MOVE FINISHED 移动结束"); + + osMutexAcquire(eclMotor->mutex_, osWaitForever); + HomeState home_state =eclMotor->home_state_; + bool is_move_finished = eclMotor->is_move_finished_; + osMutexRelease(eclMotor->mutex_); + + if(!is_move_finished) { + // 电机没有移动结束 + ZLOGI(TAG, "【HOME】 MOTOR NOT MOVE FINISHED 电机没有移动结束"); + break; + } + + if (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"); + ZLOGI(TAG, "【HOME】 BACK TO ORIGIN LOW 低速回原点"); } break; } default: + ZLOGI(TAG, "【HOME】 未处理事件类型 EVENT TYPE %d", home_event.event_type); break; } #endif } else { - ZLOGI(TAG, "GET MESSAGE FAILED"); } } } @@ -430,7 +446,7 @@ void ECLMotor::moveToHome() { void ECLMotor::runZeroLimit(bool isEnter) { // 处理回home 位逻辑 - if (!isHomeSuc()) { + if (isRunHomingTask) { TMC51X0 *motor = AppHardware::ins()->getPump(index_); if (motor == nullptr) { return; @@ -439,11 +455,8 @@ void ECLMotor::runZeroLimit(bool isEnter) { 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); @@ -486,10 +499,6 @@ void ECLMotor::runEndLimit(bool isEnter) { void ECLMotor::runE_Stop() { // 发送CAN 消息 - osMutexAcquire(mutex_, osWaitForever); - this->is_home_suc_ = false; - osMutexRelease(mutex_); - this->runStop(); } @@ -499,11 +508,16 @@ void ECLMotor::runPause() { } void ECLMotor::runStop() { + osMutexAcquire(mutex_, osWaitForever); + this->isRunHomingTask = false; + osMutexRelease(mutex_); + 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_); diff --git a/User/BSP/status/elc_motor.h b/User/BSP/status/elc_motor.h index 39c1462..57a37bb 100644 --- a/User/BSP/status/elc_motor.h +++ b/User/BSP/status/elc_motor.h @@ -59,7 +59,6 @@ private: int32_t target_pos_; // 目标位置 int32_t target_speed_; // 目标速度 bool is_move_finished_; // 是否已经移动结束 - bool isFoward_; // 是否正向运动 osTimerId_t readTimer; osMutexId_t mutex_; @@ -70,10 +69,7 @@ private: HomeEvent home_event_; osMessageQueueId_t homeEventQueue; // 回原点事件队列 HomeState home_state_{STATE_INIT}; // home 状态机 - DistanceLevel level_ { DistanceLow }; - int32_t home_start_tick_ { 0 }; // 回 home 点计数 超时清零 - int32_t max_home_tick_ { 60 * 1000 }; // 60 s 后自动重置 home 状态 bool is_home_init_{ false }; // 是否已经第一次（初始化回过原点）回过原点 bool is_home_suc_ { false }; // 是否已经成功回原点 @@ -87,14 +83,9 @@ public: ~ECLMotor(); void setMotorIndex(int32_t index); - // 是否已经回归到原点 - bool isHomeInit(); - // 是否正在回原点的逻辑 bool isHomeSuc(); - void runhomeSuc(); - void startMoveHome(); void setHomeSeqId(uint8_t seq_id); @@ -128,7 +119,8 @@ public: void setEncoderPosition(int32_t encoderPos); int32_t getRTSpeed(); - +private: + void runhomeSuc(); // 回原点任务 static void homingTask(void *arg); diff --git a/User/BSP/status/elc_motor_helper.cpp b/User/BSP/status/elc_motor_helper.cpp index c584cbc..2a3414e 100644 --- a/User/BSP/status/elc_motor_helper.cpp +++ b/User/BSP/status/elc_motor_helper.cpp @@ -14,6 +14,8 @@ const int32_t ELCMotorHelper::rEncPosLimitArray[MOTOR_NUM] = { }; #if MOTOR_REAL + +#if SUB_SPRAY_VERVION == SUB_SPRAY_VERVION_V1 const int32_t ELCMotorHelper::rPerMinHighSpeedArray[MOTOR_NUM] = { 600, 600, 600 }; @@ -23,6 +25,18 @@ const int32_t ELCMotorHelper::rPerMinMiddleSpeedArray[MOTOR_NUM] = { const int32_t ELCMotorHelper::rPerMinLowSpeedArray[MOTOR_NUM] = { 60, 30, 30 }; +#elif SUB_SPRAY_VERVION == SUB_SPRAY_VERVION_V2 +const int32_t ELCMotorHelper::rPerMinHighSpeedArray[MOTOR_NUM] = { + 600, 600, 600 +}; +const int32_t ELCMotorHelper::rPerMinMiddleSpeedArray[MOTOR_NUM] = { + 300, 300, 300 +}; +const int32_t ELCMotorHelper::rPerMinLowSpeedArray[MOTOR_NUM] = { + 60, 60, 60 +}; +#else +#endif // SUB_SPRAY_VERVION #else const int32_t ELCMotorHelper::rPerMinHighSpeedArray[MOTOR_NUM] = { 600, 600, 600 @@ -42,8 +56,9 @@ const int32_t ELCMotorHelper::encLockRotorTH[MOTOR_NUM] = {500, 500, 500}; // const bool ELCMotorHelper::isFlip_[MOTOR_NUM] = {true, false, true}; #elif SUB_SPRAY_VERVION == SUB_SPRAY_VERVION_V2 const bool ELCMotorHelper::isFlip_[MOTOR_NUM] = {true, true, false}; - +// const bool ELCMotorHelper::isFlip_[MOTOR_NUM] = {true, false, false}; #else + #endif diff --git a/User/components/tmcdriver/tmc51x0/tmc51x0.cpp b/User/components/tmcdriver/tmc51x0/tmc51x0.cpp index c81d96c..8bb131d 100644 --- a/User/components/tmcdriver/tmc51x0/tmc51x0.cpp +++ b/User/components/tmcdriver/tmc51x0/tmc51x0.cpp @@ -87,16 +87,16 @@ void TMC51X0::initialize(int mid, TMC51X0Cfg cfg) { writeInt(TMC5130_VACTUAL, 0x00000000); writeInt(TMC5130_TZEROWAIT, 0); - setVstart(100); + setVstart(0); setA1(15); setAmax(15); setV1(500); setDmax(15); setD1(15); - setVstop(100); + setVstop(10); setTzerowait(100); - setIHOLD_IRUN(0, 30, 100); + setIHOLD_IRUN(0, 5, 100); enable(false); }