|
|
|
@ -10,9 +10,9 @@ static TMC51X0* m_motors[4]; |
|
|
|
#define PUMP_NUM 4
|
|
|
|
|
|
|
|
static CfgItermCache cfgcache; |
|
|
|
#define APPEND_ML 10
|
|
|
|
|
|
|
|
// #define APPEND_ML 2
|
|
|
|
|
|
|
|
#define PUSH_DIS_INFO(info) AppEventBus::ins()->pushEvent(createStateDisplayInfoEvent(info))
|
|
|
|
|
|
|
|
void PumpCtrlService::initialize() { |
|
|
|
m_motors[0] = AppHardware::ins()->getPump(0); |
|
|
|
@ -35,6 +35,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); |
|
|
|
@ -70,8 +80,10 @@ void PumpCtrlService::updateMotorSetting() { |
|
|
|
|
|
|
|
void PumpCtrlService::stop() { |
|
|
|
ZLOGI(TAG, "stop"); |
|
|
|
stopAll(); |
|
|
|
m_thread.stop(); |
|
|
|
stopAll(); |
|
|
|
|
|
|
|
ZLOGI(TAG, "stop finish"); |
|
|
|
} |
|
|
|
|
|
|
|
@ -80,102 +92,81 @@ 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); |
|
|
|
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); }); |
|
|
|
PUSH_DIS_INFO("开始回流"); |
|
|
|
|
|
|
|
m_thread.start([this]() { |
|
|
|
unlockAll(); |
|
|
|
do { |
|
|
|
doMotorsRefluxAllOnce(); |
|
|
|
WAIT_FOR_MOTOR_STOP(); |
|
|
|
} while (false); |
|
|
|
stopAll(); |
|
|
|
lock(); |
|
|
|
ZLOGI(TAG, "reflux end"); |
|
|
|
PUSH_DIS_INFO("回流结束"); |
|
|
|
}); |
|
|
|
} |
|
|
|
void PumpCtrlService::acidPrefilling() { |
|
|
|
ZLOGI(TAG, "acidPrefilling"); |
|
|
|
if (m_pumpCfgUpdateFlag) updateMotorSetting(); |
|
|
|
|
|
|
|
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); |
|
|
|
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)(APPEND_ML) * 1000, getMxRunRPM(i)); |
|
|
|
} |
|
|
|
|
|
|
|
while (!isAllReachTarget()) { |
|
|
|
osDelay(10); |
|
|
|
} |
|
|
|
|
|
|
|
stopAll(); |
|
|
|
ZLOGI(TAG, "acidPrefilling end"); |
|
|
|
}, |
|
|
|
[]() { ValveStateSyncService::ins()->setValveState(0); }); |
|
|
|
tryUpdateMotorSetting(); |
|
|
|
PUSH_DIS_INFO("开始预充管路"); |
|
|
|
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"); |
|
|
|
PUSH_DIS_INFO("结束预充管路"); |
|
|
|
}); |
|
|
|
} |
|
|
|
|
|
|
|
void PumpCtrlService::autoMoveMutiTimes() { |
|
|
|
// doMoveOnce();
|
|
|
|
ZLOGI(TAG, "autoMoveMutiTimes"); |
|
|
|
if (m_pumpCfgUpdateFlag) updateMotorSetting(); |
|
|
|
|
|
|
|
tryUpdateMotorSetting(); |
|
|
|
PUSH_DIS_INFO("开始自动加液"); |
|
|
|
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(); |
|
|
|
sleep(getCfgFloat(kcfg_distrInterval) * 1000); |
|
|
|
} |
|
|
|
|
|
|
|
stopAll(); |
|
|
|
lock(); |
|
|
|
PUSH_DIS_INFO("停止加液"); |
|
|
|
}); |
|
|
|
} |
|
|
|
void PumpCtrlService::moveOnce() { |
|
|
|
// doMoveOnce();
|
|
|
|
ZLOGI(TAG, "moveOnce"); |
|
|
|
if (m_pumpCfgUpdateFlag) updateMotorSetting(); |
|
|
|
tryUpdateMotorSetting(); |
|
|
|
PUSH_DIS_INFO("加一次液"); |
|
|
|
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(); |
|
|
|
PUSH_DIS_INFO("停止加液"); |
|
|
|
}); |
|
|
|
} |
|
|
|
|
|
|
|
bool PumpCtrlService::isWorking() { return m_thread.isworking(); } |
|
|
|
@ -183,52 +174,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 + APPEND_ML) * 1000, getMxRunRPM(i)); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
while (!isAllReachTarget() && !m_thread.getExitFlag()) { |
|
|
|
// ZLOGI(TAG, "wait for motor move to target");
|
|
|
|
osDelay(10); |
|
|
|
} |
|
|
|
ZLOGI(TAG, "do moveOnce finish"); |
|
|
|
|
|
|
|
// 回流
|
|
|
|
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)(APPEND_ML) * 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(); |
|
|
|
@ -284,3 +229,106 @@ 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; |
|
|
|
} |
|
|
|
|
|
|
|
void PumpCtrlService::sleep(int32_t ms) { |
|
|
|
for (size_t i = 0; i < ms;) { |
|
|
|
if (m_thread.getExitFlag()) { |
|
|
|
break; |
|
|
|
} |
|
|
|
osDelay(10); |
|
|
|
i += 10; |
|
|
|
} |
|
|
|
} |
sunlight
11 months ago