From 8978bf82c38098e42473999f0cfc9447621ec1e6 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Mon, 2 Sep 2024 10:49:40 +0800 Subject: [PATCH] update --- usrc/service/pump_ctrl_service.cpp | 270 +++++++++++++++++------------- usrc/service/pump_ctrl_service.hpp | 14 +- usrc/service/valve_state_ctrl_service.cpp | 16 ++ usrc/service/valve_state_ctrl_service.hpp | 1 + 4 files changed, 181 insertions(+), 120 deletions(-) diff --git a/usrc/service/pump_ctrl_service.cpp b/usrc/service/pump_ctrl_service.cpp index ed1a8ab..00f19a2 100644 --- a/usrc/service/pump_ctrl_service.cpp +++ b/usrc/service/pump_ctrl_service.cpp @@ -33,6 +33,16 @@ void PumpCtrlService::initialize() { }); } +#define WAIT_FOR_MOTOR_STOP() \ + waitForMotorStop(); \ + if (m_thread.getExitFlag()) { \ + break; \ + } + +void PumpCtrlService::tryUpdateMotorSetting() { + if (m_pumpCfgUpdateFlag) updateMotorSetting(); +} + void PumpCtrlService::updateMotorSetting() { for (size_t i = 0; i < PUMP_NUM; i++) { m_motors[i]->enable(false); @@ -80,100 +90,75 @@ void PumpCtrlService::stop() { ***********************************************************************************************************************/ void PumpCtrlService::reflux() { ZLOGI(TAG, "reflux"); - if (m_pumpCfgUpdateFlag) updateMotorSetting(); + tryUpdateMotorSetting(); - m_thread.start( - [this]() { - ValveStateSyncService::ins()->setValveState(1); - // int32_t pipeLenML = getCfgInt(kcfg_pipeLengthML); - - for (size_t i = 0; i < PUMP_NUM; i++) { - TMC51X0* cur_motor = m_motors[i]; - cur_motor->stop(); - cur_motor->setXACTUAL(0); - float pipeLenML = getCfgPipeLengthML(i); - ZLOGI(TAG, "pump %d move %f ml", i, -pipeLenML); - cur_motor->moveBy((int32_t)-pipeLenML * 1000, getMxRunRPM(i)); - } - - while (!m_thread.getExitFlag()) { - if (isAllReachTarget()) { - break; - } - osDelay(10); - } - - stopAll(); - ZLOGI(TAG, "reflux end"); - }, - []() { ValveStateSyncService::ins()->setValveState(0); }); + m_thread.start([this]() { + unlockAll(); + do { + doMotorsRefluxAllOnce(); + WAIT_FOR_MOTOR_STOP(); + } while (false); + stopAll(); + lock(); + ZLOGI(TAG, "reflux end"); + }); } void PumpCtrlService::acidPrefilling() { ZLOGI(TAG, "acidPrefilling"); - if (m_pumpCfgUpdateFlag) updateMotorSetting(); + tryUpdateMotorSetting(); - m_thread.start( - [this]() { - ValveStateSyncService::ins()->setValveState(1); - - for (size_t i = 0; i < PUMP_NUM; i++) { - TMC51X0* cur_motor = m_motors[i]; - cur_motor->stop(); - cur_motor->setXACTUAL(0); - float pipeLenML = getCfgPipeLengthML(i); - ZLOGI(TAG, "pump %d move %f ml", i, pipeLenML); - cur_motor->moveBy((int32_t)pipeLenML * 1000, getMxRunRPM(i)); - } - - while (!m_thread.getExitFlag()) { - if (isAllReachTarget()) break; - osDelay(10); - } - - // 短回流 - for (size_t i = 0; i < PUMP_NUM; i++) { - TMC51X0* cur_motor = m_motors[i]; - cur_motor->moveBy(-(int32_t)(getPumpAppendMl(i) * 1000), getMxRunRPM(i)); - } - - while (!isAllReachTarget()) { - osDelay(10); - } - - stopAll(); - ZLOGI(TAG, "acidPrefilling end"); - }, - []() { ValveStateSyncService::ins()->setValveState(0); }); + m_thread.start([this]() { + unlockAll(); + do { + doMotorsRefluxAllOnce(); // 回流 + WAIT_FOR_MOTOR_STOP(); + doMotorsAcidPrefillingOnce(); // 预填充 + WAIT_FOR_MOTOR_STOP(); + doMotorsLittleRefluxOnce(); // 小回流 + WAIT_FOR_MOTOR_STOP(); + } while (false); + stopAll(); + lock(); + ZLOGI(TAG, "reflux end"); + }); } void PumpCtrlService::autoMoveMutiTimes() { // doMoveOnce(); ZLOGI(TAG, "autoMoveMutiTimes"); - if (m_pumpCfgUpdateFlag) updateMotorSetting(); + tryUpdateMotorSetting(); m_thread.start([this]() { - float delayTime = getCfgFloat(kcfg_distrInterval); - ZLOGI(TAG, "delayTime %d", delayTime); - while (!m_thread.getExitFlag()) { - doMoveOnce(); - for (size_t i = 0; i < delayTime * 1000; i++) { - osDelay(1); - if (m_thread.getExitFlag()) { - break; - } - delayTime = getCfgFloat(kcfg_distrInterval); - } + unlock(); + doMotorsMoveByOnce(); + WAIT_FOR_MOTOR_STOP(); + doMotorsLittleRefluxOnce(); + WAIT_FOR_MOTOR_STOP(); + lock(); + m_thread.sleep(getCfgFloat(kcfg_distrInterval) * 1000); } - stopAll(); + lock(); }); } void PumpCtrlService::moveOnce() { + // doMoveOnce(); ZLOGI(TAG, "moveOnce"); - if (m_pumpCfgUpdateFlag) updateMotorSetting(); + tryUpdateMotorSetting(); + + m_thread.start([this]() { + unlock(); + do { + doMotorsMoveByOnce(); + WAIT_FOR_MOTOR_STOP(); + doMotorsLittleRefluxOnce(); + WAIT_FOR_MOTOR_STOP(); + } while (false); - m_thread.start([this]() { doMoveOnce(); }); + stopAll(); + lock(); + }); } bool PumpCtrlService::isWorking() { return m_thread.isworking(); } @@ -181,50 +166,6 @@ bool PumpCtrlService::isWorking() { return m_thread.isworking(); } /*********************************************************************************************************************** * BASIC * ***********************************************************************************************************************/ - -void PumpCtrlService::doMoveOnce() { - // - ZLOGI(TAG, "do moveOnce ..."); - ValveStateSyncService::ins()->setValveState(1); - - for (size_t i = 0; i < PUMP_NUM; i++) { - bool pumpselect = GSM->getPumpSelectState(i); - float distribut_ml = getCfgFloat(config_index_t(kcfg_acideval0 + i)); - - TMC51X0* cur_motor = m_motors[i]; - cur_motor->stop(); - cur_motor->setXACTUAL(0); - if (pumpselect) { - ZLOGI(TAG, "pump %d move %f ml", i, distribut_ml); - // - cur_motor->moveBy((int32_t)((distribut_ml + getPumpAppendMl(i)) * 1000), getMxRunRPM(i)); - } - } - - while (!isAllReachTarget() && !m_thread.getExitFlag()) { - osDelay(10); - } - // 回流 - for (size_t i = 0; i < PUMP_NUM; i++) { - bool pumpselect = GSM->getPumpSelectState(i); - - TMC51X0* cur_motor = m_motors[i]; - cur_motor->stop(); - cur_motor->setXACTUAL(0); - if (pumpselect) { - cur_motor->moveBy(-(int32_t)(getPumpAppendMl(i) * 1000), getMxRunRPM(i)); - } - } - - while (!isAllReachTarget() && !m_thread.getExitFlag()) { - osDelay(10); - } - - ZLOGI(TAG, "do moveOnce finish"); - stopAll(); - ValveStateSyncService::ins()->setValveState(0); -} - bool PumpCtrlService::isAllReachTarget() { for (size_t i = 0; i < PUMP_NUM; i++) { TMC5130RampStat state = m_motors[i]->getRampStat(); @@ -280,3 +221,96 @@ double PumpCtrlService::getMotorNowPosR(int32_t mid) { } return m_motors[mid]->getXactualRAW() / 51200.0; } + +void PumpCtrlService::doMotorsMoveByOnce() { + int32_t step0 = int32_t((getCfgFloat(kcfg_acideval0) + getPumpAppendMl(0)) * 1000); + int32_t step1 = int32_t((getCfgFloat(kcfg_acideval1) + getPumpAppendMl(1)) * 1000); + int32_t step2 = int32_t((getCfgFloat(kcfg_acideval2) + getPumpAppendMl(2)) * 1000); + int32_t step3 = int32_t((getCfgFloat(kcfg_acideval3) + getPumpAppendMl(3)) * 1000); + + step0 = GSM->getPumpSelectState(0) ? step0 : 0; + step1 = GSM->getPumpSelectState(1) ? step1 : 0; + step2 = GSM->getPumpSelectState(2) ? step2 : 0; + step3 = GSM->getPumpSelectState(3) ? step3 : 0; + + ZLOGI(TAG, "doMotorsMoveByOnce, moveBy %d %d %d %d", step0, step1, step2, step3); + moveBy(step0, step1, step2, step3); +} +void PumpCtrlService::doMotorsRefluxAllOnce() { + int32_t step0 = int32_t(-(getCfgPipeLengthML(0) * 1.2) * 1000); // *1.2是为了保证液体完全回流 + int32_t step1 = int32_t(-(getCfgPipeLengthML(1) * 1.2) * 1000); // *1.2是为了保证液体完全回流 + int32_t step2 = int32_t(-(getCfgPipeLengthML(2) * 1.2) * 1000); // *1.2是为了保证液体完全回流 + int32_t step3 = int32_t(-(getCfgPipeLengthML(3) * 1.2) * 1000); // *1.2是为了保证液体完全回流 + + // step0 = GSM->getPumpSelectState(0) ? step0 : 0; + // step1 = GSM->getPumpSelectState(1) ? step1 : 0; + // step2 = GSM->getPumpSelectState(2) ? step2 : 0; + // step3 = GSM->getPumpSelectState(3) ? step3 : 0; + + ZLOGI(TAG, "doMotorsRefluxOnce, moveBy %d %d %d %d", step0, step1, step2, step3); + moveBy(step0, step1, step2, step3); +} + +void PumpCtrlService::doMotorsAcidPrefillingOnce() { + int32_t step0 = int32_t((getCfgPipeLengthML(0)) * 1000); + int32_t step1 = int32_t((getCfgPipeLengthML(1)) * 1000); + int32_t step2 = int32_t((getCfgPipeLengthML(2)) * 1000); + int32_t step3 = int32_t((getCfgPipeLengthML(3)) * 1000); + + step0 = GSM->getPumpSelectState(0) ? step0 : 0; + step1 = GSM->getPumpSelectState(1) ? step1 : 0; + step2 = GSM->getPumpSelectState(2) ? step2 : 0; + step3 = GSM->getPumpSelectState(3) ? step3 : 0; + + ZLOGI(TAG, "doMotorsAcidPrefillingOnce, moveBy %d %d %d %d", step0, step1, step2, step3); + moveBy(step0, step1, step2, step3); +} + +void PumpCtrlService::doMotorsLittleRefluxOnce() { + int32_t step0 = int32_t(-(getPumpAppendMl(0)) * 1000); // + int32_t step1 = int32_t(-(getPumpAppendMl(1)) * 1000); // + int32_t step2 = int32_t(-(getPumpAppendMl(2)) * 1000); // + int32_t step3 = int32_t(-(getPumpAppendMl(3)) * 1000); // + + step0 = GSM->getPumpSelectState(0) ? step0 : 0; + step1 = GSM->getPumpSelectState(1) ? step1 : 0; + step2 = GSM->getPumpSelectState(2) ? step2 : 0; + step3 = GSM->getPumpSelectState(3) ? step3 : 0; + + ZLOGI(TAG, "doMotorsLittleRefluxOnce, moveBy %d %d %d %d", step0, step1, step2, step3); + moveBy(step0, step1, step2, step3); +} + +void PumpCtrlService::lock() { + ZLOGI(TAG, "lockAll"); + ValveStateSyncService::ins()->setValveState(0); +} +void PumpCtrlService::unlock() { + bool select0 = GSM->getPumpSelectState(0); + bool select1 = GSM->getPumpSelectState(1); + bool select2 = GSM->getPumpSelectState(2); + bool select3 = GSM->getPumpSelectState(3); + ZLOGI(TAG, "unlock %d %d %d %d", select0, select1, select2, select3); + ValveStateSyncService::ins()->setValveState(select0, select1, select2, select3); + return; +} + +void PumpCtrlService::unlockAll() { + ZLOGI(TAG, "unlockAll"); + ValveStateSyncService::ins()->setValveState(1); +} + +void PumpCtrlService::moveBy(int32_t steps0, int32_t steps1, int32_t steps2, int32_t steps3) { + m_motors[0]->moveBy(steps0, getMxRunRPM(0)); + m_motors[1]->moveBy(steps1, getMxRunRPM(1)); + m_motors[2]->moveBy(steps2, getMxRunRPM(2)); + m_motors[3]->moveBy(steps3, getMxRunRPM(3)); +} + +void PumpCtrlService::waitForMotorStop() { + while (!m_thread.getExitFlag()) { + if (isAllReachTarget()) break; + osDelay(5); + } + return; +} diff --git a/usrc/service/pump_ctrl_service.hpp b/usrc/service/pump_ctrl_service.hpp index b72237f..14f4ffa 100644 --- a/usrc/service/pump_ctrl_service.hpp +++ b/usrc/service/pump_ctrl_service.hpp @@ -23,6 +23,7 @@ class PumpCtrlService { void initialize(); void updateMotorSetting(); + void tryUpdateMotorSetting(); public: bool isWorking(); @@ -40,9 +41,18 @@ class PumpCtrlService { double getMotorNowPosR(int32_t mid); private: - bool isAllReachTarget(); + void doMotorsMoveByOnce(); + void doMotorsRefluxAllOnce(); + void doMotorsLittleRefluxOnce(); + void doMotorsAcidPrefillingOnce(); + + void lock(); + void unlock(); + void unlockAll(); + void moveBy(int32_t steps0, int32_t steps1, int32_t steps2, int32_t steps3); void stopAll(); - void doMoveOnce(); + bool isAllReachTarget(); + void waitForMotorStop(); }; } // namespace iflytop diff --git a/usrc/service/valve_state_ctrl_service.cpp b/usrc/service/valve_state_ctrl_service.cpp index 3503b54..efcbbde 100644 --- a/usrc/service/valve_state_ctrl_service.cpp +++ b/usrc/service/valve_state_ctrl_service.cpp @@ -63,6 +63,22 @@ void ValveStateSyncService::setValveState(int valveIndex, bool state) { } } +void ValveStateSyncService::setValveState(bool state0, bool state1, bool state2, bool state3) { + { + zlock_guard l(lock); + output0state = (output0state & ~(1 << 0)) | (state0 << 0); + output0state = (output0state & ~(1 << 1)) | (state1 << 1); + output0state = (output0state & ~(1 << 2)) | (state2 << 2); + output0state = (output0state & ~(1 << 3)) | (state3 << 3); + forceupdate = true; + } + + m_thread.wake(); + while (!forceupdate) { + osDelay(2); + } +} + #define SETBIT(byte, off, valve) (byte = (byte & ~(1 << off)) | (valve << off)) void ValveStateSyncService::setRGBState(bool r, bool g, bool b) { diff --git a/usrc/service/valve_state_ctrl_service.hpp b/usrc/service/valve_state_ctrl_service.hpp index d8206e2..0e38878 100644 --- a/usrc/service/valve_state_ctrl_service.hpp +++ b/usrc/service/valve_state_ctrl_service.hpp @@ -30,6 +30,7 @@ class ValveStateSyncService { void setValveState(int valveIndex, bool state); void setValveState(bool state); + void setValveState(bool state0, bool state1, bool state2, bool state3); void setRGBState(bool r, bool g, bool b); void setWarningState(bool warning);