p_chinagoldgroup_hand_acid
/
hand_acid_mainboard_bak
4
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

update

sunlight
zhaohe 11 months ago
parent
3ffbdfab94
commit
8978bf82c3
4 changed files with 181 additions and 120 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 270
      usrc/service/pump_ctrl_service.cpp
  2. 14
      usrc/service/pump_ctrl_service.hpp
  3. 16
      usrc/service/valve_state_ctrl_service.cpp
  4. 1
      usrc/service/valve_state_ctrl_service.hpp

270
usrc/service/pump_ctrl_service.cpp
View File

@ -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;
}

14
usrc/service/pump_ctrl_service.hpp
View File

@ -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

16
usrc/service/valve_state_ctrl_service.cpp
View File

@ -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) {

1
usrc/service/valve_state_ctrl_service.hpp
View File

@ -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);

Write Preview
Loading…
Cancel
Save