diff --git a/components/pipette_module/pipette_ctrl_module_v2.cpp b/components/pipette_module/pipette_ctrl_module_v2.cpp index 83635f2..80f65a0 100644 --- a/components/pipette_module/pipette_ctrl_module_v2.cpp +++ b/components/pipette_module/pipette_ctrl_module_v2.cpp @@ -25,7 +25,7 @@ using namespace iflytop; } #define PIPETTE_PREPARE_POS 800 -#define LLF_DPOS 1000 +#define LLF_DPOS 1000 void PipetteModule::initialize(int32_t id, config_t *config, StepMotorCtrlModule *zmotor, hardward_config_t *hardwaredcfg) { // ZASSERT(config != nullptr); @@ -43,8 +43,8 @@ void PipetteModule::initialize(int32_t id, config_t *config, StepMotorCtrlModule m_config.aspirate_distribut_pump_vel = 500; // m_config.pump_vmax = 1000; m_config.lld_pump_vel = 100; // lld推荐使用速度为50->200,这里 - m_config.lld_motor_vel_rpm = 50; - m_config.lld_detect_period_ms = 100; // 100ms + m_config.lld_motor_vel_rpm = 100; + m_config.lld_detect_period_ms = 30; // 100ms m_config.lld_prepare_pos = PIPETTE_PREPARE_POS; m_config.lld_prepare_distribut_pos = 100; @@ -157,7 +157,7 @@ int32_t PipetteModule::pipette_distribut(int32_t ul) { int32_t PipetteModule::pipette_shake_up(int32_t ul, int32_t times) { ZLOGI(TAG, "pipette_shake_up %d %d", ul, times); if (creg.module_status == 1) return err::kdevice_is_busy; - return 0; + return do_pipette_shake_up(ul, 0, times); } int32_t PipetteModule::pipette_lld_is_detect_liquid(int32_t *detect_liquid) { // @@ -177,7 +177,7 @@ int32_t PipetteModule::pipette_distribut_llf(int32_t ul, int32_t zmotor_v) { // } int32_t PipetteModule::pipette_shake_up_llf(int32_t ul, int32_t zmotor_v, int32_t times) { // ZLOGI(TAG, "pipette_shake_up_llf %d %d %d %d", ul, zmotor_v, times); - return 0; + return do_pipette_shake_up(ul, zmotor_v, times); } /*********************************************************************************************************************** @@ -542,7 +542,7 @@ int32_t PipetteModule::do_pipette_distribut(int32_t ul, int32_t zmotor_v) { m_thread.stop(); creg.module_status = 1; m_thread.start( - + [this, ul, zmotor_v]() { // auto submotor = m_zmotor->getMotor(); pipette_enable_zmotor(1); @@ -578,6 +578,107 @@ int32_t PipetteModule::do_pipette_distribut(int32_t ul, int32_t zmotor_v) { return 0; } +int32_t PipetteModule::do_pipette_shake_up(int32_t ul, int32_t zmotor_v, int32_t times) { + /** + * @brief + */ + + ZLOGI(TAG, "do_pipette_shake_up %d", ul); + + if (m_state.load_val_ul != 0) { // 移液枪摇匀必须为空 + return err::kpipette_error_pump_load_val_is_not_empty; + } + + m_thread.stop(); + creg.module_status = 1; + m_thread.start( + + [this, ul, zmotor_v,times]() { // + auto submotor = m_zmotor->getMotor(); + pipette_enable_zmotor(1); + DO_IN_THREAD(befor_run()); + int32_t dpos = 0; + + int32_t startpos = 0; + int32_t endpos = 0; + int32_t dotimes = 0; + + // 吸液 + startpos = submotor->getXACTUAL(); + if (zmotor_v != 0) submotor->moveBy(LLF_DPOS, zmotor_v); + DO_IN_THREAD(m_smtp2.pump_aspirate(m_config.aspirate_distribut_pump_vel, ul)); + while (true) { + if (!check_when_run()) break; + if (m_thread.getExitFlag()) break; + + int32_t isbusy = 0; + DO_IN_THREAD(m_smtp2.pump_get_state(&isbusy)); + if (isbusy == 0) break; + + m_thread.sleep(10); + } + endpos = submotor->getXACTUAL(); + dpos = abs(endpos - startpos); + + while (true) { + // 分配 + ZLOGI(TAG, "pump_distribut ..."); + DO_IN_THREAD(m_smtp2.pump_distribut(m_config.aspirate_distribut_pump_vel, ul)); + if (zmotor_v != 0) submotor->moveBy(-dpos, zmotor_v); + while (true) { + if (!check_when_run()) return; + if (m_thread.getExitFlag()) return; + + int32_t isbusy = 0; + bool motorIsReachTarget = false; + DO_IN_THREAD(m_smtp2.pump_get_state(&isbusy)); + motorIsReachTarget = submotor->isReachTarget(); + + if (isbusy == 0 && motorIsReachTarget) break; + + m_thread.sleep(10); + } + dotimes++; + if (dotimes >= times) { + ZLOGI(TAG, "shake end ...") + break; + } + + // 吸收 + ZLOGI(TAG, "pump_aspirate ..."); + if (zmotor_v != 0) submotor->moveBy(dpos, zmotor_v); + DO_IN_THREAD(m_smtp2.pump_aspirate(m_config.aspirate_distribut_pump_vel, ul)); + while (true) { + if (!check_when_run()) return; + if (m_thread.getExitFlag()) return; + + int32_t isbusy = 0; + bool motorIsReachTarget = false; + DO_IN_THREAD(m_smtp2.pump_get_state(&isbusy)); + motorIsReachTarget = submotor->isReachTarget(); + + if (isbusy == 0 && motorIsReachTarget) break; + + m_thread.sleep(10); + } + } + + }, + [this]() { // exit fn + after_run(); + m_smtp2.pump_stop(); + m_zmotor->getMotor()->stop(); + + m_state.load_val_ul = m_state.load_val_ul + m_state.dul; + if (m_state.load_val_ul < 0) { + m_state.load_val_ul = 0; + } + ZLOGI(TAG, "do_pipette_distribut finish, load_val_ul:%d"); + } // + ); + return 0; +} + int32_t PipetteModule::pipette_enable_zmotor(int32_t enable) { return m_zmotor->step_motor_enable(enable); } int32_t PipetteModule::pipette_get_sensor_sample_data(int32_t index, int32_t *motor_pos, int32_t *cval, int32_t *pval) { if (index > capturedata_num) return err::kparam_out_of_range; @@ -656,33 +757,16 @@ void PipetteModule::push_snesor_sample_data(int32_t motor_pos, int32_t cap_val, /******************************************************************************* * PRIVATE * *******************************************************************************/ -int32_t PipetteModule::read_pos_ul(int32_t *val) { +int32_t PipetteModule::read_pos_ul(int32_t *val) { // + *val = m_state.load_val_ul; return 0; - // return m_smtp2.read_pos_ul(*val); } int32_t PipetteModule::read_capactitance(int32_t *val) { + m_smtp2.pump_get_capacitance(val); return 0; - // return m_smtp2.read_capacitance_val(*val); } int32_t PipetteModule::read_tip_state(int32_t *val) { *val = 0; + m_smtp2.pump_get_tip_state(val); return 0; - // bool tipstate = false; - // int32_t ecode = m_smtp2.read_tip_state(tipstate); - // *val = tipstate ? 1 : 0; - // return ecode; -} -int32_t PipetteModule::_read_error_status() { - return 0; - // bool isbusy = false; - // err::error_t errorcode = err::ksucc; - // int32_t retecode = m_smtp2.getState(isbusy, errorcode); - // if (retecode != 0) { - // return retecode; - // } - // if (errorcode != err::ksucc && errorcode != err::kpipette_error_NoError) { - // return errorcode; - // } - - // return 0; } \ No newline at end of file diff --git a/components/pipette_module/pipette_ctrl_module_v2.hpp b/components/pipette_module/pipette_ctrl_module_v2.hpp index 9c977bd..1e1a09b 100644 --- a/components/pipette_module/pipette_ctrl_module_v2.hpp +++ b/components/pipette_module/pipette_ctrl_module_v2.hpp @@ -164,6 +164,7 @@ class PipetteModule : public ZIModule, public ZIPipetteCtrlModule { int32_t do_pipette_aspirate(int32_t ul, int32_t zmotor_v); int32_t do_pipette_distribut(int32_t ul, int32_t zmotor_v); + int32_t do_pipette_shake_up(int32_t ul, int32_t zmotor_v, int32_t times); private: int32_t befor_run(); @@ -176,7 +177,6 @@ class PipetteModule : public ZIModule, public ZIPipetteCtrlModule { int32_t read_tip_state(int32_t *val); private: - int32_t _read_error_status(); private: void push_snesor_sample_data(int32_t motor_pos, int32_t cap_val, int32_t pressure_val);