|
|
|
@ -9,7 +9,9 @@ static TMC51X0* m_motors[4]; |
|
|
|
#define PUMP_NUM 4
|
|
|
|
|
|
|
|
static CfgItermCache cfgcache; |
|
|
|
void PumpCtrlService::initialize() { |
|
|
|
#define APPEND_ML 1
|
|
|
|
|
|
|
|
void PumpCtrlService::initialize() { |
|
|
|
m_motors[0] = AppHardware::ins()->getPump(0); |
|
|
|
m_motors[1] = AppHardware::ins()->getPump(1); |
|
|
|
m_motors[2] = AppHardware::ins()->getPump(2); |
|
|
|
@ -17,13 +19,24 @@ void PumpCtrlService::initialize() { |
|
|
|
|
|
|
|
m_thread.init("PumpCtrlService", 1024); |
|
|
|
updateMotorSetting(); |
|
|
|
|
|
|
|
AppEventBus::ins()->regOnEvent([this](AppEvent_t* event) { |
|
|
|
if (event->type == kAppEvent_ConfigChangeEvent) { |
|
|
|
// 电机参数发生改变
|
|
|
|
int cfi = event->d.configIndex; |
|
|
|
if ((cfi >= kcfg_mLPR0 && cfi <= kcfg_mLPR3) || //
|
|
|
|
(cfi >= kcfg_m0irun && cfi <= kcfg_m3irun)) { |
|
|
|
m_pumpCfgUpdateFlag = true; |
|
|
|
} |
|
|
|
} |
|
|
|
}); |
|
|
|
} |
|
|
|
|
|
|
|
void PumpCtrlService::updateMotorSetting() { |
|
|
|
for (size_t i = 0; i < PUMP_NUM; i++) { |
|
|
|
m_motors[i]->enable(false); |
|
|
|
|
|
|
|
m_motors[i]->setIHOLD_IRUN(STEPMOTOR_IHOLD, STEPMOTOR_IRUN, STEPMOTOR_IHOLDDELAY); |
|
|
|
m_motors[i]->setIHOLD_IRUN(STEPMOTOR_IHOLD, getMxIRun(i), STEPMOTOR_IHOLDDELAY); |
|
|
|
m_motors[i]->setScale(getMotorMLPR(i) * 1000); |
|
|
|
m_motors[i]->setScaleDenominator(1); |
|
|
|
m_motors[i]->setMotorShaft(MOTOR_SHAFT); |
|
|
|
m_motors[i]->setVstart(MOTOR_VSTART); |
|
|
|
@ -34,9 +47,22 @@ void PumpCtrlService::updateMotorSetting() { |
|
|
|
m_motors[i]->setDmax(MOTOR_AMAX); |
|
|
|
m_motors[i]->setV1(MOTOR_V1); |
|
|
|
m_motors[i]->setTzerowait(MOTOR_TZEROWAIT); |
|
|
|
m_motors[i]->setScale(getMotorULToTruns(i)); |
|
|
|
m_motors[i]->enable(true); |
|
|
|
|
|
|
|
ZLOGI(TAG, "M[%d] setIHOLD_IRUN %d %d %d", i, STEPMOTOR_IHOLD, getMxIRun(i), STEPMOTOR_IHOLDDELAY); |
|
|
|
ZLOGI(TAG, "M[%d] setScale %d", i, getMotorMLPR(i)); |
|
|
|
ZLOGI(TAG, "M[%d] setScaleDenominator %d", i, 1); |
|
|
|
ZLOGI(TAG, "M[%d] setMotorShaft %d", i, MOTOR_SHAFT); |
|
|
|
ZLOGI(TAG, "M[%d] setVstart %d", i, MOTOR_VSTART); |
|
|
|
ZLOGI(TAG, "M[%d] setVstop %d", i, MOTOR_VSTART); |
|
|
|
ZLOGI(TAG, "M[%d] setA1 %d", i, MOTOR_A1); |
|
|
|
ZLOGI(TAG, "M[%d] setD1 %d", i, MOTOR_A1); |
|
|
|
ZLOGI(TAG, "M[%d] setAmax %d", i, MOTOR_AMAX); |
|
|
|
ZLOGI(TAG, "M[%d] setDmax %d", i, MOTOR_AMAX); |
|
|
|
ZLOGI(TAG, "M[%d] setV1 %d", i, MOTOR_V1); |
|
|
|
ZLOGI(TAG, "M[%d] setTzerowait %d", i, MOTOR_TZEROWAIT); |
|
|
|
} |
|
|
|
m_pumpCfgUpdateFlag = false; |
|
|
|
} |
|
|
|
|
|
|
|
void PumpCtrlService::stop() { |
|
|
|
@ -51,6 +77,8 @@ void PumpCtrlService::stop() { |
|
|
|
***********************************************************************************************************************/ |
|
|
|
void PumpCtrlService::reflux() { |
|
|
|
ZLOGI(TAG, "reflux"); |
|
|
|
if (m_pumpCfgUpdateFlag) updateMotorSetting(); |
|
|
|
|
|
|
|
m_thread.start([this]() { |
|
|
|
int32_t pipeLenML = getCfgInt(kcfg_pipeLengthML); |
|
|
|
int32_t vel = getCfgInt(config_index_t(kcfg_pumpDefVel)); |
|
|
|
@ -76,6 +104,8 @@ void PumpCtrlService::reflux() { |
|
|
|
} |
|
|
|
void PumpCtrlService::acidPrefilling() { |
|
|
|
ZLOGI(TAG, "acidPrefilling"); |
|
|
|
if (m_pumpCfgUpdateFlag) updateMotorSetting(); |
|
|
|
|
|
|
|
m_thread.start([this]() { |
|
|
|
int32_t pipeLenML = getCfgInt(kcfg_pipeLengthML); |
|
|
|
int32_t vel = getCfgInt(config_index_t(kcfg_pumpDefVel)); |
|
|
|
@ -95,6 +125,17 @@ void PumpCtrlService::acidPrefilling() { |
|
|
|
osDelay(10); |
|
|
|
} |
|
|
|
|
|
|
|
// 短回流
|
|
|
|
for (size_t i = 0; i < PUMP_NUM; i++) { |
|
|
|
int32_t vel = getCfgInt(kcfg_pumpDefVel); |
|
|
|
TMC51X0* cur_motor = m_motors[i]; |
|
|
|
cur_motor->moveBy(-(int32_t)(APPEND_ML) * 1000, vel); |
|
|
|
} |
|
|
|
|
|
|
|
while (!isAllReachTarget()) { |
|
|
|
osDelay(10); |
|
|
|
} |
|
|
|
|
|
|
|
stopAll(); |
|
|
|
ZLOGI(TAG, "acidPrefilling end"); |
|
|
|
}); |
|
|
|
@ -103,8 +144,11 @@ void PumpCtrlService::acidPrefilling() { |
|
|
|
void PumpCtrlService::autoMoveMutiTimes() { |
|
|
|
// doMoveOnce();
|
|
|
|
ZLOGI(TAG, "autoMoveMutiTimes"); |
|
|
|
if (m_pumpCfgUpdateFlag) updateMotorSetting(); |
|
|
|
|
|
|
|
m_thread.start([this]() { |
|
|
|
int32_t delayTime = getCfgInt(kcfg_distrInterval); |
|
|
|
float delayTime = getCfgFloat(kcfg_distrInterval); |
|
|
|
ZLOGI(TAG, "delayTime %d", delayTime); |
|
|
|
|
|
|
|
while (!m_thread.getExitFlag()) { |
|
|
|
doMoveOnce(); |
|
|
|
@ -113,6 +157,7 @@ void PumpCtrlService::autoMoveMutiTimes() { |
|
|
|
if (m_thread.getExitFlag()) { |
|
|
|
break; |
|
|
|
} |
|
|
|
delayTime = getCfgFloat(kcfg_distrInterval); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
@ -121,6 +166,8 @@ void PumpCtrlService::autoMoveMutiTimes() { |
|
|
|
} |
|
|
|
void PumpCtrlService::moveOnce() { |
|
|
|
ZLOGI(TAG, "moveOnce"); |
|
|
|
if (m_pumpCfgUpdateFlag) updateMotorSetting(); |
|
|
|
|
|
|
|
m_thread.start([this]() { doMoveOnce(); }); |
|
|
|
} |
|
|
|
|
|
|
|
@ -133,23 +180,43 @@ bool PumpCtrlService::isWorking() { return m_thread.isworking(); } |
|
|
|
void PumpCtrlService::doMoveOnce() { |
|
|
|
//
|
|
|
|
ZLOGI(TAG, "do moveOnce ..."); |
|
|
|
|
|
|
|
for (size_t i = 0; i < PUMP_NUM; i++) { |
|
|
|
bool pumpselect = GSM->getPumpSelectState(i); |
|
|
|
float distribut_ml = getCfgFloat(config_index_t(kcfg_acideval0 + i)); |
|
|
|
int32_t vel = getCfgInt(config_index_t(kcfg_pumpDefVel)); |
|
|
|
int32_t vel = getCfgInt(kcfg_pumpDefVel); |
|
|
|
|
|
|
|
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 * 1000, vel); |
|
|
|
cur_motor->moveBy((int32_t)(distribut_ml + APPEND_ML) * 1000, vel); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
while (!isAllReachTarget()) { |
|
|
|
osDelay(10); |
|
|
|
} |
|
|
|
|
|
|
|
// 回流
|
|
|
|
for (size_t i = 0; i < PUMP_NUM; i++) { |
|
|
|
bool pumpselect = GSM->getPumpSelectState(i); |
|
|
|
int32_t vel = getCfgInt(kcfg_pumpDefVel); |
|
|
|
|
|
|
|
TMC51X0* cur_motor = m_motors[i]; |
|
|
|
cur_motor->stop(); |
|
|
|
cur_motor->setXACTUAL(0); |
|
|
|
if (pumpselect) { |
|
|
|
cur_motor->moveBy(-(int32_t)(APPEND_ML) * 1000, vel); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
while (!isAllReachTarget()) { |
|
|
|
osDelay(10); |
|
|
|
} |
|
|
|
|
|
|
|
ZLOGI(TAG, "do moveOnce finish"); |
|
|
|
stopAll(); |
|
|
|
} |
|
|
|
|
|
|
|
|
