From 4e378e2ec50f78f3c0dd0dc3d47bdfd3974f03ed Mon Sep 17 00:00:00 2001 From: zhaohe Date: Wed, 19 Apr 2023 16:20:57 +0800 Subject: [PATCH] update --- dep/libtrinamic | 2 +- src/board/hardware.cpp | 2 +- src/board/hardware.hpp | 11 +- src/board/project_board.hpp | 7 +- src/lncubator_rotating_control_service.cpp | 201 +++++++++++++++++++++++------ src/lncubator_rotating_control_service.hpp | 79 ++++++++---- 6 files changed, 233 insertions(+), 69 deletions(-) diff --git a/dep/libtrinamic b/dep/libtrinamic index d438155..b0450ea 160000 --- a/dep/libtrinamic +++ b/dep/libtrinamic @@ -1 +1 @@ -Subproject commit d4381558b25522ebd43c86d125f8177d38c578e3 +Subproject commit b0450ea2b7b4892637b4885c2518d3b8db245588 diff --git a/src/board/hardware.cpp b/src/board/hardware.cpp index e82b951..b4430e8 100644 --- a/src/board/hardware.cpp +++ b/src/board/hardware.cpp @@ -213,7 +213,7 @@ void Hardware::STM32_HAL_onGPIO_EXTI_Callback(uint16_t gpioNum) { if (gpioNum == TMC_HOME_REF_GPIO_PIN) { if (STM32_HAL_GPIO_IS_THIS_PIN_TRIGGER_IRQ(TMC_HOME_REF_GPIO)) { for (size_t i = 0; i < m_listenerNum; i++) { - m_listener[i]->Hardware_onHomeRefSwitch(); + m_listener[i]->Hardware_onHomeRefSwitchISR(); } } } diff --git a/src/board/hardware.hpp b/src/board/hardware.hpp index 64947b5..b650b8c 100644 --- a/src/board/hardware.hpp +++ b/src/board/hardware.hpp @@ -18,7 +18,7 @@ namespace iflytop { class HardwareListener { public: virtual void Hardware_OnHardwareException(){}; - virtual void Hardware_onHomeRefSwitch(){}; + virtual void Hardware_onHomeRefSwitchISR(){}; }; class Hardware : public IflytopCanSlaveListener, // @@ -28,16 +28,14 @@ class Hardware : public IflytopCanSlaveListener, // private: /* data */ - public: + private: TMC4361A tmc4361motor1; TMP117 tmp117[4]; FanStateMonitor m_fanStateMonitor[6]; IflytopCanSlave canSlaveService; bool m_canOnRxDataFlag; - /* data */ - bool m_fanState[6]; - + bool m_fanState[6]; HardwareListener *m_listener[MAX_HARDWARE_LISTENER_NUM]; int m_listenerNum; @@ -60,7 +58,8 @@ class Hardware : public IflytopCanSlaveListener, // void hardwareinit(); void periodicJob(); - int32_t port_tmc4361_get_version(uint8_t channel); + int32_t port_tmc4361_get_version(uint8_t channel); + TMC4361A *getMotor1() { return &tmc4361motor1; } /******************************************************************************* * 测试 * diff --git a/src/board/project_board.hpp b/src/board/project_board.hpp index 4ffdd15..7fc5e61 100644 --- a/src/board/project_board.hpp +++ b/src/board/project_board.hpp @@ -22,4 +22,9 @@ #define TMC_HOME_REF_GPIO_PIN GPIO_PIN_3 #define TMC_HOME_REF_GPIO_PORT GPIOD -#define TMC_HOME_REF_GPIO_IRQ_MODE GPIO_MODE_IT_RISING \ No newline at end of file +#define TMC_HOME_REF_GPIO_IRQ_MODE GPIO_MODE_IT_RISING + +typedef enum { + kok = 0, + kGoHomeFail, +} exception_id_t; \ No newline at end of file diff --git a/src/lncubator_rotating_control_service.cpp b/src/lncubator_rotating_control_service.cpp index be950f9..5b5b414 100644 --- a/src/lncubator_rotating_control_service.cpp +++ b/src/lncubator_rotating_control_service.cpp @@ -3,67 +3,196 @@ using namespace iflytop; #define TAG "LRCS" -const uint32_t LncubatorRotatingControlService::mc_onecircle_ustep_num = (256 * 200 * 50); -const uint32_t LncubatorRotatingControlService::mc_default_velocity = 500000; // 500k +const uint32_t LncubatorRotatingControlService::mc_onecircle_ustep_num = (256 * 200 * 50); +const uint32_t LncubatorRotatingControlService::mc_default_velocity = 500000; // 500kpps +const uint32_t LncubatorRotatingControlService::mc_one_sub_position_width = (256 * 200 * 50) / 20; LncubatorRotatingControlService::LncubatorRotatingControlService() {} LncubatorRotatingControlService::~LncubatorRotatingControlService() {} - -/** - * @brief - * - * - * 1. 定位 - * 2. 标记零点位置 - * 3. - * - */ - void LncubatorRotatingControlService::initialize(Hardware* hardware) { // - m_hardware = hardware; - m_motor = &m_hardware->tmc4361motor1; - m_os = m_hardware; - m_workfinished = false; - m_dowhat = kidle; - m_maxVelocity = mc_default_velocity; + m_hardware = hardware; + m_motor = m_hardware->getMotor1(); + m_os = m_hardware; + m_workfinished = false; + m_dowhat = kidle; + m_maxVelocity = mc_default_velocity; + m_inletPosition = 0; + m_outletPosition = m_inletPosition + mc_one_sub_position_width / 2; /** * @brief 电机必须工作在位置模式 */ m_motor->setMotorInPositionMode(); hardware->registerListener(this); + m_motor->registerListener(this, TMC4361A::event_mask_t::ktarget_reached); } -void LncubatorRotatingControlService::moveToHome() { - m_dowhat = krotateToHome; - m_motor->moveTo(mc_onecircle_ustep_num * 2, m_maxVelocity); + +void LncubatorRotatingControlService::setVelocity(int32_t velocityMax) { m_maxVelocity = velocityMax; } +void LncubatorRotatingControlService::registerListener(LncubatorRotatingControlServiceListener* listener) { // + m_listener = listener; } -void LncubatorRotatingControlService::Hardware_onHomeRefSwitch() { + +/******************************************************************************* + * motorHelp * + *******************************************************************************/ +int32_t LncubatorRotatingControlService::tmc_getPosition() { + int32_t position = m_motor->getENC_POS(); + return normalizePosition(position); +} +void LncubatorRotatingControlService::tmc_roateToPosition(int32_t position) { + int32_t _position = normalizePosition(position); + m_motor->moveTo(position, m_maxVelocity); +} +int32_t LncubatorRotatingControlService::normalizePosition(int32_t position) { + while (true) { + if (position > mc_onecircle_ustep_num / 2) + position = position - mc_onecircle_ustep_num; + else if (position < mc_onecircle_ustep_num / 2) + position = position + mc_onecircle_ustep_num; + else + break; + } + if (position == mc_onecircle_ustep_num / 2) position = position - 1; + if (position == -mc_onecircle_ustep_num / 2) position = position + 1; + return position; +} +void LncubatorRotatingControlService::tmc_moveBy(int32_t relativePosition) { m_motor->moveBy(relativePosition, m_maxVelocity); } +/******************************************************************************* + * TriggerEvent * + *******************************************************************************/ +void LncubatorRotatingControlService::Hardware_onHomeRefSwitchISR() { /** * @brief 电机零点光电触发 */ m_homeKeyTriggered = true; } -void LncubatorRotatingControlService::setVelocity(uint32_t velocityMax) { m_maxVelocity = velocityMax; } -void LncubatorRotatingControlService::rotate(uint32_t count) { m_motor->moveBy(count, m_maxVelocity); } +void LncubatorRotatingControlService::triggerEvent(interevent_t event) {} + void LncubatorRotatingControlService::periodicJob() { - if (krotateToHome == m_dowhat) { - doRotateToHomeJob(); + if (kmoveToHomeJob == m_dowhat) { + doRotateToHomeJob(kPeriodicJobEvent); + } else if (kmoveSubPositionJob == m_dowhat) { + doMoveToSubPositionJob(kPeriodicJobEvent); + } else if (kmoveByJob == m_dowhat) { + doMoveByJob(kPeriodicJobEvent); } -} - -void LncubatorRotatingControlService::doRotateToHomeJob() { if (m_homeKeyTriggered) { m_homeKeyTriggered = false; + if (kmoveToHomeJob == m_dowhat) { + doRotateToHomeJob(kPeriodicJobEvent); + } else if (kmoveSubPositionJob == m_dowhat) { + doMoveToSubPositionJob(kPeriodicJobEvent); + } else if (kmoveByJob == m_dowhat) { + doMoveByJob(kPeriodicJobEvent); + } + } +} +void LncubatorRotatingControlService::TMC4361A_onEvent(uint32_t event) { + if (event & TMC4361A::event_mask_t::ktarget_reached) { + if (kmoveToHomeJob == m_dowhat) { + doRotateToHomeJob(kTMC4361AReachTargetEvent); + } else if (kmoveSubPositionJob == m_dowhat) { + doMoveToSubPositionJob(kTMC4361AReachTargetEvent); + } else if (kmoveByJob == m_dowhat) { + doMoveByJob(kTMC4361AReachTargetEvent); + } + } +} +/******************************************************************************* + * RotateToHomeJob * + *******************************************************************************/ + +void LncubatorRotatingControlService::moveToHome() { + if (m_dowhat != kidle) return; + m_dowhat = kmoveToHomeJob; + moveBy(mc_onecircle_ustep_num * 2); +} +void LncubatorRotatingControlService::doRotateToHomeJob(interevent_t event) { + if (event == kPeriodicJobEvent) { + if (m_homeKeyTriggered) { + m_homeKeyTriggered = false; + /** + * @brief 零点光电触发,记录零点位置 + */ + m_motor->stop(); + m_motor->setZeroPoint(); + m_dowhat = kidle; + ZLOGI(TAG, "find Zero Point"); + if (m_listener) m_listener->LncubatorRotatingControlService_onMoveToHomeJobFinished(); + } + } else if (event == kTMC4361AReachTargetEvent) { /** - * @brief 零点光电触发,记录零点位置 + * @brief 电机转了两圈依然没有找到零点 */ - m_homePosition = m_motor->getENC_POS(); - m_motor->stop(); + if (m_listener) m_listener->LncubatorRotatingControlService_onException(kGoHomeFail); + m_dowhat = kidle; + } +} +void LncubatorRotatingControlService::calibrationPosition(refpoint_t refpoint) { + /** + * @brief 此时是孵育船1对其了入口 + */ + if (refpoint == REFPOINT_TYPE_INLET) { + m_inletPosition = tmc_getPosition(); + m_outletPosition = normalizePosition(m_inletPosition + mc_onecircle_ustep_num / 2); + } +} + +/******************************************************************************* + * moveToSubPosition * + *******************************************************************************/ +void LncubatorRotatingControlService::moveToSubPosition(refpoint_t refpoint, int32_t positionIndex) { + if (m_dowhat != kidle) return; + m_dowhat = kmoveSubPositionJob; + // 出口 + if (refpoint == REFPOINT_TYPE_OUTLET) { + int32_t targetPosition = m_outletPosition + positionIndex * mc_one_sub_position_width; + tmc_roateToPosition(targetPosition); + } else if (refpoint == REFPOINT_TYPE_INLET) { + int32_t targetPosition = m_inletPosition + positionIndex * mc_one_sub_position_width; + tmc_roateToPosition(targetPosition); + } else { + ZLOGE(TAG, "refpoint error"); + } +} +void LncubatorRotatingControlService::doMoveToSubPositionJob(interevent_t event) { + if (event == kPeriodicJobEvent) { + } else if (event == kTMC4361AReachTargetEvent) { + /** + * @brief moveToTarget + */ + if (m_listener) m_listener->LncubatorRotatingControlService_onMoveSubPositionJobFinished(); m_dowhat = kidle; - ZLOGI(TAG, "home position:%d", m_homePosition); } } -void LncubatorRotatingControlService::registerListener(LncubatorRotatingControlServiceListener* listener) { // - m_listener = listener; +/******************************************************************************* + * moveByJob * + *******************************************************************************/ +void LncubatorRotatingControlService::moveBy(int32_t pluseCount) { + if (m_dowhat == kidle || m_dowhat == kmoveByJob) { + m_dowhat = kmoveByJob; + tmc_moveBy(pluseCount); + } +} +void LncubatorRotatingControlService::doMoveByJob(interevent_t event) { + if (event == kPeriodicJobEvent) { + } else if (event == kTMC4361AReachTargetEvent) { + /** + * @brief moveToTarget + */ + if (m_listener) m_listener->LncubatorRotatingControlService_onMoveByJobFinished(); + m_dowhat = kidle; + } +} +/******************************************************************************* + * STOP * + *******************************************************************************/ +void LncubatorRotatingControlService::stop() { + if (m_dowhat == kidle) return; + if (m_dowhat == kmoveToHomeJob) { + m_motor->stop(); + m_dowhat = kidle; + return; + } } diff --git a/src/lncubator_rotating_control_service.hpp b/src/lncubator_rotating_control_service.hpp index 8fcfe42..4781d7b 100644 --- a/src/lncubator_rotating_control_service.hpp +++ b/src/lncubator_rotating_control_service.hpp @@ -2,6 +2,7 @@ #include "board\hardware.hpp" #include "libiflytop_micro\stm32\basic\stm32_hal.hpp" #include "libiflytop_micro\stm32\component\state_machine\state_machine.hpp" +#include "libtrinamic\src\ic\tmc4361A.hpp" namespace iflytop { /** @@ -17,84 +18,114 @@ namespace iflytop { * 7. 读取校准值 * 8. 写入校准值 * + * 约定: + * 一圈位置的位置为 (-256*200/2)<0<(-256*200/2) + * * * */ #define LNCUBATOR_MAX_POSITION_COUNT 20 #define ZEROOFFSETCORRECTION 0 +#define FIND_HOME_POINT_TIMEOUT_MS (2 * 60 * 1000) class LncubatorRotatingControlServiceListener { public: - virtual void OnlncubatorRotatingControlServiceException() = 0; - virtual void OnlncubatorRotatingControlServiceJobFinished() = 0; + virtual void LncubatorRotatingControlService_onException(exception_id_t exception) = 0; + virtual void LncubatorRotatingControlService_onMoveToHomeJobFinished() = 0; + virtual void LncubatorRotatingControlService_onMoveSubPositionJobFinished() = 0; + virtual void LncubatorRotatingControlService_onMoveByJobFinished() = 0; }; -class LncubatorRotatingControlService : public HardwareListener { +class LncubatorRotatingControlService : public HardwareListener, // + public TMC4361AListener { public: typedef enum { REFPOINT_TYPE_OUTLET = 0, // 出口 REFPOINT_TYPE_INLET, // 入口 REFPOINT_TYPE_ZERO, // 零点 } refpoint_t; - + /******************************************************************************* + * 常量配置 * + *******************************************************************************/ static const uint32_t mc_onecircle_ustep_num; static const uint32_t mc_default_velocity; + static const uint32_t mc_one_sub_position_width; private: typedef enum { kidle, - krotateToTarget, - krotateToHome, - kspeedMode, + kmoveSubPositionJob, + kmoveToHomeJob, + kmoveByJob, } dowhat_t; + typedef enum { + kPeriodicJobEvent, + kHomeSwitchTriggeredEvent, + kTMC4361AReachTargetEvent, + } interevent_t; + /* data */ Hardware* m_hardware; IflytopMicroOS* m_os; TMC4361A* m_motor; dowhat_t m_dowhat; - // 零点位置 - uint32_t m_homePosition; + int32_t m_inletPosition; // 入口对其第0个孵育船的位置 + int32_t m_outletPosition; // 出口对其第0个孵育船的位置,出口理论上应该是入口+ + // 便宜修正，第一个仓位中心距离零点的距离 - uint32_t m_homeOffsetCorrection; - uint32_t m_maxVelocity = 0; - bool m_workfinished; - bool m_homeKeyTriggered; + int32_t m_maxVelocity = 0; + bool m_workfinished; + bool m_homeKeyTriggered; LncubatorRotatingControlServiceListener* m_listener; - private: public: LncubatorRotatingControlService(); ~LncubatorRotatingControlService(); void initialize(Hardware* hardware); - void setVelocity(uint32_t velocityMax); - void rotate(uint32_t count); - void registerListener(LncubatorRotatingControlServiceListener* listener); + void periodicJob(); + + void setVelocity(int32_t velocityMax); + void setInletPositionAndOutletPosition(int32_t inletPosition, int32_t outletPosition); + int32_t getVelocity() { return m_maxVelocity; } + int32_t getInletPosition() { return m_inletPosition; } + int32_t getOutletPosition() { return m_outletPosition; } void moveToHome(); - void moveTo(refpoint_t refpoint, uint32_t positionIndex); + void moveBy(int32_t pluseCount); + void moveToSubPosition(refpoint_t refpoint, int32_t positionIndex); void stop(); - /** * @brief 人工校准当前位置为指定位置 * * @param refpoint 参考点 * @param positionIndex 第几个位置 */ - void calibrationPosition(refpoint_t refpoint, uint32_t positionIndex); - - void periodicJob(); + void calibrationPosition(refpoint_t refpoint); /******************************************************************************* * OVERRIDE HardwareListener * *******************************************************************************/ - virtual void Hardware_onHomeRefSwitch(); + virtual void Hardware_onHomeRefSwitchISR(); + /******************************************************************************* + * OVERRIDE TMC4361AListener * + *******************************************************************************/ + virtual void TMC4361A_onEvent(uint32_t event); + + private: + void triggerEvent(interevent_t event); + void doRotateToHomeJob(interevent_t event); + void doMoveToSubPositionJob(interevent_t event); + void doMoveByJob(interevent_t event); private: - void doRotateToHomeJob(); + int32_t tmc_getPosition(); + void tmc_roateToPosition(int32_t position); + void tmc_moveBy(int32_t position); + int32_t normalizePosition(int32_t position); }; } // namespace iflytop