From 7367266623755620e4b386a8f0cafad81b8b0d28 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Fri, 21 Jun 2024 09:38:50 +0800 Subject: [PATCH] update --- .../pipette_module/pipette_ctrl_module_v2.cpp | 151 ++++++++++----------- .../pipette_module/pipette_ctrl_module_v2.hpp | 2 + components/sensors/smtp2_v2/smtp2_v2.cpp | 22 +-- components/sensors/smtp2_v2/smtp2_v2.hpp | 8 +- 4 files changed, 89 insertions(+), 94 deletions(-) diff --git a/components/pipette_module/pipette_ctrl_module_v2.cpp b/components/pipette_module/pipette_ctrl_module_v2.cpp index 4d4a48d..7988a86 100644 --- a/components/pipette_module/pipette_ctrl_module_v2.cpp +++ b/components/pipette_module/pipette_ctrl_module_v2.cpp @@ -24,7 +24,7 @@ using namespace iflytop; } \ } -#define PIPETTE_PREPARE_POS 500 +#define PIPETTE_PREPARE_POS 800 void PipetteModule::initialize(int32_t id, config_t *config, StepMotorCtrlModule *zmotor, hardward_config_t *hardwaredcfg) { // ZASSERT(config != nullptr); @@ -44,6 +44,8 @@ void PipetteModule::initialize(int32_t id, config_t *config, StepMotorCtrlModule 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_prepare_pos = PIPETTE_PREPARE_POS; + m_config.lld_prepare_distribut_pos = 100; ZASSERT(zmotor); m_zmotor = zmotor; @@ -76,6 +78,8 @@ int32_t PipetteModule::module_xxx_reg(int32_t param_id, bool read, int32_t &val) PROCESS_REG_CFG(kreg_pipette_lld_pump_vel, lld_pump_vel); PROCESS_REG_CFG(kreg_pipette_lld_motor_vel_rpm, lld_motor_vel_rpm); PROCESS_REG_CFG(kreg_pipette_lld_detect_period_ms, lld_detect_period_ms); + PROCESS_REG_CFG(kreg_pipette_lld_prepare_pos, lld_prepare_pos); + PROCESS_REG_CFG(kreg_pipette_lld_prepare_distribut_pos, lld_prepare_distribut_pos); default: return err::kmodule_not_find_reg; @@ -165,11 +169,18 @@ int32_t PipetteModule::pipette_lld_is_detect_liquid(int32_t *detect_liquid) { / ***********************************************************************************************************************/ int32_t PipetteModule::do_pipette_ctrl_init_device() { creg.module_status = 1; + + // 检查tip头状态 + int32_t tipstate = 0; + int32_t ret = m_smtp2.pump_get_tip_state(&tipstate); + if (ret != 0) return ret; + if (tipstate != 0) return err::kpipette_error_tipisload_when_lld_prepare; + m_thread.stop(); m_thread.start( [this]() { // DO_IN_THREAD(befor_run()); - DO_IN_THREAD(m_smtp2.pump_init()); + DO_IN_THREAD(m_smtp2.pump_init(m_config.pump_vmax)); while (true) { if (!check_when_run()) break; if (m_thread.getExitFlag()) break; @@ -182,13 +193,15 @@ int32_t PipetteModule::do_pipette_ctrl_init_device() { } }, [this]() { // exit fn - ZLOGI(TAG, "do_pipette_ctrl_put_tip finish"); after_run(); - m_state.load_val_ul = 0; - m_state.aspirated = 0; - m_state.lld_prepared = 0; - m_state.pipette_is_inited = 1; + if (creg.module_errorcode == 0) { + m_state.load_val_ul = 0; + m_state.aspirated = 0; + m_state.lld_prepared = 0; + m_state.pipette_is_inited = 1; + } + ZLOGI(TAG, "do_pipette_ctrl_put_tip finish,ecode:%d", creg.module_errorcode); } // ); @@ -206,7 +219,18 @@ int32_t PipetteModule::do_pipette_ctrl_put_tip() { if (m_thread.getExitFlag()) break; int32_t isbusy = 0; - DO_IN_THREAD(m_smtp2.pump_get_state(&isbusy)); + + int32_t ecode = m_smtp2.pump_get_state(&isbusy); + if (ecode != 0) { + if (ecode == err::kpipette_error_TipDrop || ecode == err::kpipette_error_TipPopError) { + break; + } else { + ZLOGE(TAG, "do do_pipette_ctrl_put_tip fail, error %s(%d)", err::error2str(ecode), ecode); + creg.module_errorcode = ecode; + break; + } + } + if (isbusy == 0) break; m_thread.sleep(10); @@ -214,10 +238,13 @@ int32_t PipetteModule::do_pipette_ctrl_put_tip() { }, [this]() { // exit fn - ZLOGI(TAG, "do_pipette_ctrl_put_tip finish"); after_run(); - m_state.load_val_ul = 0; - m_state.aspirated = 0; + if (creg.module_errorcode == 0) { + m_state.load_val_ul = 0; + m_state.aspirated = 0; + } + ZLOGI(TAG, "do_pipette_ctrl_put_tip finish,ecode:%d", creg.module_errorcode); + } // ); return 0; @@ -228,7 +255,7 @@ int32_t PipetteModule::do_pipette_ctrl_move_to_ul(int32_t ul) { m_thread.start( [this, ul]() { // DO_IN_THREAD(befor_run()); - DO_IN_THREAD(m_smtp2.pump_move_to_ul(ul)); + DO_IN_THREAD(m_smtp2.pump_move_to_ul(m_config.pump_vmax, ul)); while (true) { if (!check_when_run()) break; if (m_thread.getExitFlag()) break; @@ -241,8 +268,8 @@ int32_t PipetteModule::do_pipette_ctrl_move_to_ul(int32_t ul) { } }, [this]() { // exit fn - ZLOGI(TAG, "do_pipette_ctrl_move_to_ul finish"); after_run(); + ZLOGI(TAG, "do_pipette_ctrl_move_to_ul finish,ecode:%d", creg.module_errorcode); } // ); return 0; @@ -278,7 +305,7 @@ int32_t PipetteModule::do_pipette_lld_prepare() { m_thread.start( [this]() { // DO_IN_THREAD(befor_run()); - DO_IN_THREAD(m_smtp2.pump_move_to_ul(PIPETTE_PREPARE_POS)); // 移动到中点 + DO_IN_THREAD(m_smtp2.pump_move_to_ul(m_config.pump_vmax, PIPETTE_PREPARE_POS)); // 移动到中点 while (true) { if (!check_when_run()) break; @@ -292,13 +319,12 @@ int32_t PipetteModule::do_pipette_lld_prepare() { } }, [this]() { // exit fn - ZLOGI(TAG, "do_pipette_lld_prepare finish"); after_run(); - if (creg.module_errorcode != 0) { - m_state.pipette_is_inited = 1; - m_state.load_val_ul = 0; - m_state.aspirated = 0; + if (creg.module_errorcode == 0) { + m_state.lld_prepared = 1; } + ZLOGI(TAG, "do_pipette_lld_prepare finish,ecode:%d", creg.module_errorcode); + } // ); return 0; @@ -320,6 +346,7 @@ int32_t PipetteModule::do_pipette_lld(lld_mode_t mode, int32_t zdpos, int32_t c_ */ if (m_state.lld_prepared == 0) { + ZLOGI(TAG, "do_pipette_lld fail, not prepare"); return err::kpipette_error_not_lld_prepare; } @@ -347,6 +374,21 @@ int32_t PipetteModule::do_pipette_lld(lld_mode_t mode, int32_t zdpos, int32_t c_ DO_IN_THREAD(m_smtp2.pump_get_pressure(&start_pressure)); start_motor_pos = submotor->getXACTUAL(); + ZLOGI(TAG, "start lld before distribut"); + DO_IN_THREAD(m_smtp2.pump_distribut(m_config.pump_vmax, m_config.lld_prepare_distribut_pos)); + 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; + } + ZLOGI(TAG, "lld before distribut ok"); + + ZLOGI(TAG, "start lld"); + submotor->moveBy(zdpos, m_config.lld_motor_vel_rpm); + osDelay(50); if (mode == kclld) { DO_IN_THREAD(m_smtp2.pump_clld(c_threshold)); } else if (mode == kplld) { @@ -354,8 +396,6 @@ int32_t PipetteModule::do_pipette_lld(lld_mode_t mode, int32_t zdpos, int32_t c_ } else if (mode == kmixlld) { DO_IN_THREAD(m_smtp2.pump_distribut_mlld(m_config.lld_pump_vel, c_threshold, p_threshold)); } - osDelay(50); - submotor->moveBy(zdpos, m_config.lld_motor_vel_rpm); while (true) { if (!check_when_run()) break; @@ -385,6 +425,7 @@ int32_t PipetteModule::do_pipette_lld(lld_mode_t mode, int32_t zdpos, int32_t c_ // 探测到液面 if (detect_liquid == 1) { + ZLOGI(TAG, "detect liquid"); m_state.detected_liquid = 1; submotor->stop(); push_snesor_sample_data(motorpos, capacitance, pressure); @@ -396,19 +437,24 @@ int32_t PipetteModule::do_pipette_lld(lld_mode_t mode, int32_t zdpos, int32_t c_ if (motorerror != 0) { creg.module_errorcode = motorerror; ZLOGI(TAG, "motorerror %d", motorerror); - break; + return; } // 电机停了也没有探测到液面 if (motorstoped) { + ZLOGI(TAG, "motorstoped,but not detect liquid"); creg.module_errorcode = 0; m_state.detected_liquid = 0; push_snesor_sample_data(motorpos, capacitance, pressure); - break; + return; } + push_snesor_sample_data(motorpos, capacitance, pressure); m_thread.sleep(m_config.lld_detect_period_ms); } + + + }, [this]() { // exit fn auto submotor = m_zmotor->getMotor(); @@ -436,7 +482,7 @@ int32_t PipetteModule::do_pipette_aspirate(int32_t ul) { m_thread.start( [this, ul]() { // DO_IN_THREAD(befor_run()); - DO_IN_THREAD(m_smtp2.pump_aspirate(ul)); + 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; @@ -473,7 +519,7 @@ int32_t PipetteModule::do_pipette_distribut(int32_t ul) { m_thread.start( [this, ul]() { // DO_IN_THREAD(befor_run()); - DO_IN_THREAD(m_smtp2.pump_distribut(ul)); + DO_IN_THREAD(m_smtp2.pump_distribut(m_config.aspirate_distribut_pump_vel, ul)); while (true) { if (!check_when_run()) break; if (m_thread.getExitFlag()) break; @@ -498,60 +544,6 @@ int32_t PipetteModule::do_pipette_distribut(int32_t ul) { return 0; } -#if 0 -int32_t PipetteModule::do_pipette_clld_test(int32_t zdpos) { - m_thread.stop(); - creg.module_status = 1; - m_thread.start( - [this, zdpos]() { // - DO_IN_THREAD(befor_run()); - - m_zmotor->module_active_cfg(); - m_zmotor->step_motor_enable(1); - - auto submotor = m_zmotor->getMotor(); - submotor->moveBy(zdpos, m_config.lld_velocity); - - int32_t zmotor_start_pos = submotor->getXACTUAL(); - - capturedata_num = 0; - - while (true) { - if (!check_when_run()) break; - if (m_thread.getExitFlag()) break; - - bool motorstoped = false; - int32_t motorerror = 0; - int32_t motorpos = 0; - int32_t capacitance = 0; - - m_smtp2.pump_get_capacitance(&capacitance); - motorpos = submotor->getXACTUAL() - zmotor_start_pos; - submotor->readMotorState(&motorstoped, &motorerror); - - if (motorerror != 0) { - creg.module_errorcode = motorerror; - break; - } - - if (motorstoped) { - break; - } - - push_snesor_sample_data(motorpos, capacitance, 0); - m_thread.sleep(m_config.lld_detect_period_ms); - } - }, - [this]() { // exit fn - ZLOGI(TAG, "do_pipette_clld_test finish"); - after_run(); - } // - ); - return 0; -} -int32_t PipetteModule::do_pipette_plld_test(int32_t zdpos) { return 0; } -#endif - 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; @@ -585,6 +577,7 @@ int32_t PipetteModule::befor_run() { creg.module_errorcode = 0; DO(module_active_cfg()); DO(m_smtp2.pump_get_motor_pos_ul(&m_state.pump_before_pos_ul)); + return 0; } int32_t PipetteModule::after_run() { int32_t err = 0; diff --git a/components/pipette_module/pipette_ctrl_module_v2.hpp b/components/pipette_module/pipette_ctrl_module_v2.hpp index b392155..9147e76 100644 --- a/components/pipette_module/pipette_ctrl_module_v2.hpp +++ b/components/pipette_module/pipette_ctrl_module_v2.hpp @@ -49,6 +49,8 @@ class PipetteModule : public ZIModule, public ZIPipetteCtrlModule { int32_t lld_pump_vel; // lld,泵机吸吐速度 int32_t lld_motor_vel_rpm; // lld,z轴电机移动速度 int32_t lld_detect_period_ms; // lld,探测周期 + int32_t lld_prepare_pos; // lld,页面探测准备位置 + int32_t lld_prepare_distribut_pos; // lld前，预分配位置 } config_t; typedef struct { diff --git a/components/sensors/smtp2_v2/smtp2_v2.cpp b/components/sensors/smtp2_v2/smtp2_v2.cpp index 61a69b9..e428f30 100644 --- a/components/sensors/smtp2_v2/smtp2_v2.cpp +++ b/components/sensors/smtp2_v2/smtp2_v2.cpp @@ -67,15 +67,15 @@ int32_t SMTP2V2::pump_get_motor_pos_ul(int32_t* ul) { return ret; } -int32_t SMTP2V2::pump_stop() { return SEND_CMD("H"); } +int32_t SMTP2V2::pump_stop() { return SEND_CMD("T"); } int32_t SMTP2V2::pump_set_vstart(int32_t vel) { return SEND_CMD("v%d", vel); } int32_t SMTP2V2::pump_set_vstop(int32_t vel) { return SEND_CMD("c%d", vel); } int32_t SMTP2V2::pump_set_vmax(int32_t vel) { return SEND_CMD("V%d", vel); } int32_t SMTP2V2::pump_set_acc_and_dec(int32_t acc, int32_t dec) { return SEND_CMD("L%d,%d", acc, dec); } int32_t SMTP2V2::pump_set_plld_start_delay(int32_t delay_ms) { return SEND_CMD("T%d", delay_ms); } -int32_t SMTP2V2::pump_put_tip() { return SEND_CMD("E"); } -int32_t SMTP2V2::pump_init() { return SEND_CMD("Z"); } +int32_t SMTP2V2::pump_put_tip() { return SEND_CMD("E1"); } +int32_t SMTP2V2::pump_init(int32_t v) { return SEND_CMD("V%dZ", v); } int32_t SMTP2V2::pump_reset() { _sendcmd(true, "/1!0R\r"); // 复位指令没有回执，所以这里只能使用方法_sendcmd return 0; @@ -92,7 +92,7 @@ static int32_t Get1fromfloat(const float& val) { return (int32_t)temp1; } -int32_t SMTP2V2::pump_move_to_ul(int32_t ul) { +int32_t SMTP2V2::pump_move_to_ul(int32_t v, int32_t ul) { /** * @brief 分辨率在0.319ul进行操作 * @@ -102,18 +102,18 @@ int32_t SMTP2V2::pump_move_to_ul(int32_t ul) { */ ZLOGI(TAG, "pump_move_to_ul %d", ul); int stepNum = ul / 0.319 + 0.5; //+0.5 是为了浮点数强转成正数时采用四舍五入的方式 - return SEND_CMD("N%dA%d", 0 /*模式*/, stepNum); + return SEND_CMD("N%dV%dA%d", 0 /*模式*/, v, stepNum); } -int32_t SMTP2V2::pump_aspirate(int32_t ul) { +int32_t SMTP2V2::pump_aspirate(int32_t v, int32_t ul) { ZLOGI(TAG, "pump_aspirate %d", ul); int stepNum = ul / 0.319 + 0.5; - return SEND_CMD("N%dP%d", 1 /*模式*/, stepNum); + return SEND_CMD("N%dV%dP%d", 1 /*模式*/, v, stepNum); } -int32_t SMTP2V2::pump_distribut(int32_t ul) { +int32_t SMTP2V2::pump_distribut(int32_t v, int32_t ul) { ZLOGI(TAG, "pump_distribut %d", ul); int stepNum = ul / 0.319 + 0.5; - return SEND_CMD("N%dD%d", 2 /*模式*/, stepNum); + return SEND_CMD("N%dV%dD%d", 2 /*模式*/, v, stepNum); } int32_t SMTP2V2::pump_distribut_plld(int32_t pumpv, int32_t pressure_threshold) { @@ -123,8 +123,8 @@ int32_t SMTP2V2::pump_distribut_plld(int32_t pumpv, int32_t pressure_threshold) } int32_t SMTP2V2::pump_distribut_plld_get_state(int32_t* detected) { - int32_t isbusy; - int32_t ret = pump_get_state(&isbusy); + int32_t isbusy = 0; + int32_t ret = pump_get_state(&isbusy); if (isbusy != 0) { *detected = 0; } else { diff --git a/components/sensors/smtp2_v2/smtp2_v2.hpp b/components/sensors/smtp2_v2/smtp2_v2.hpp index 49b7cb2..9807523 100644 --- a/components/sensors/smtp2_v2/smtp2_v2.hpp +++ b/components/sensors/smtp2_v2/smtp2_v2.hpp @@ -181,11 +181,11 @@ class SMTP2V2 { /*********************************************************************************************************************** * ACTION * ***********************************************************************************************************************/ - int32_t pump_init(); // 泵机初始化(归零) + int32_t pump_init(int32_t v); // 泵机初始化(归零) int32_t pump_reset(); // 泵机复位 int32_t pump_stop(); // 停止 int32_t pump_put_tip(); // 丢弃TIP - int32_t pump_move_to_ul(int32_t ul); // + int32_t pump_move_to_ul(int32_t v, int32_t ul); // int32_t pump_distribut_plld(int32_t pumpv, int32_t pressure_threshold); // plld,分配探测 int32_t pump_distribut_plld_get_state(int32_t* detected); @@ -196,8 +196,8 @@ class SMTP2V2 { int32_t pump_distribut_mlld(int32_t pumpv, int32_t c_threshold, int32_t pressure_threshold); // int32_t pump_distribut_mlld_get_state(int32_t* detected); - int32_t pump_aspirate(int32_t ul); - int32_t pump_distribut(int32_t ul); + int32_t pump_aspirate(int32_t v, int32_t ul); + int32_t pump_distribut(int32_t v, int32_t ul); public: //