diff --git a/sdk/components/pipette_module/pipette_ctrl_module_v1.cpp b/sdk/components/pipette_module/pipette_ctrl_module_v1.cpp index 8dceddf..37f8bc5 100644 --- a/sdk/components/pipette_module/pipette_ctrl_module_v1.cpp +++ b/sdk/components/pipette_module/pipette_ctrl_module_v1.cpp @@ -1,798 +1,798 @@ -#include "pipette_ctrl_module_v1.hpp" - -#include "sdk\components\zcancmder\protocol_event_bus_sender.hpp" -using namespace iflytop; - -#define TAG "PipetteModule" - -#define DO(func) \ - { \ - int32_t ecode = func; \ - if (ecode != 0) { \ - ZLOGE(TAG, "[%d] do %s fail, error %s(%d)", __LINE__, #func, err::error2str(ecode), ecode); \ - return ecode; \ - } \ - } - -#define DO_IN_THREAD(func) \ - { \ - int32_t ecode = func; \ - if (ecode != 0) { \ - ZLOGE(TAG, "[%d]do %s fail, error %s(%d) ", __LINE__, #func, err::error2str(ecode), ecode); \ - creg.module_errorcode = ecode; \ - return; \ - } \ - } - -#define PIPETTE_PREPARE_POS 800 -#define LLF_DPOS 1000 - -void PipetteModule::initialize(int32_t id, config_t *config, StepMotorCtrlModule *zmotor, hardward_config_t *hardwaredcfg) { // - ZASSERT(config != nullptr); - ZASSERT(hardwaredcfg != nullptr); - m_id = id; - m_config = *config; - m_smtp2.initialize(hardwaredcfg->uart, 0, hardwaredcfg->hdma_rx, hardwaredcfg->hdma_tx); - m_thread.init("pipette_ctrl", 1024); - - m_config.limit_ul = 200; - m_config.pump_acc = 14; - m_config.pump_dec = 14; - m_config.pump_vstart = 0; - m_config.pump_vstop = 900; - m_config.aspirate_pump_vel = 500; // - m_config.pump_vmax = 1000; - m_config.lld_pump_vel = 100; // lld推荐使用速度为50->200,这里 - m_config.lld_motor_vel_rpm = 200; - m_config.lld_detect_period_ms = 10; // 100ms - m_config.lld_prepare_pos = PIPETTE_PREPARE_POS; - m_config.lld_prepare_pre_distribut_ul = 100; - - ZASSERT(zmotor); - m_zmotor = zmotor; -} - -int32_t PipetteModule::getid(int32_t *id) { - *id = m_id; - return 0; -} -/******************************************************************************* - * Module * - *******************************************************************************/ - -#define PROCESS_REG_CFG(cfgkey, cfg) PROCESS_REG(cfgkey, /* */ REG_GET(m_config.cfg), REG_SET(m_config.cfg)) - -int32_t PipetteModule::module_xxx_reg(int32_t param_id, bool read, int32_t &val) { - switch (param_id) { - MODULE_COMMON_PROCESS_REG_CB(); - PROCESS_REG(kreg_pipette_pos_ul, /* */ read_pos_ul(&val), ACTION_NONE); - PROCESS_REG(kreg_pipette_capactitance_val, /* */ read_capactitance(&val), ACTION_NONE); - PROCESS_REG(kreg_pipette_tip_state, /* */ read_tip_state(&val), ACTION_NONE); - - PROCESS_REG_CFG(kreg_pipette_limit_ul, limit_ul); - PROCESS_REG_CFG(kreg_pipette_pump_acc, pump_acc); - PROCESS_REG_CFG(kreg_pipette_pump_dec, pump_dec); - PROCESS_REG_CFG(kreg_pipette_pump_vstart, pump_vstart); - PROCESS_REG_CFG(kreg_pipette_pump_vstop, pump_vstop); - PROCESS_REG_CFG(kreg_pipette_pump_vmax, pump_vmax); - PROCESS_REG_CFG(kreg_pipette_aspirate_pump_vel, aspirate_pump_vel); - 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_pre_distribut_ul, lld_prepare_pre_distribut_ul); - - default: - return err::kmodule_not_find_reg; - break; - } - return 0; -} - -int32_t PipetteModule::module_stop() { - m_thread.stop(); - m_smtp2.pump_stop(); - m_zmotor->module_stop(); - return 0; -} -int32_t PipetteModule::module_active_cfg() { - DO(m_smtp2.pump_set_acc_and_dec(m_config.pump_acc, m_config.pump_dec)); - DO(m_smtp2.pump_set_vstart(m_config.pump_vstart)); - DO(m_smtp2.pump_set_vstop(m_config.pump_vstop)); - return 0; -} - -/*********************************************************************************************************************** - * EXTERN ACTION * - ***********************************************************************************************************************/ - -// 兼容旧程序 -int32_t PipetteModule::pipette_ctrl_move_to_ul(int32_t ul) { - ZLOGI(TAG, "pipette_ctrl_move_to_ul %d", ul); // not support - return err::kcmd_not_support; -}; - -int32_t PipetteModule::pipette_ctrl_init_device() { - ZLOGI(TAG, "pipette_ctrl_init_device"); - if (creg.module_status == 1) return err::kdevice_is_busy; - return do_pipette_ctrl_init_device(); -}; -int32_t PipetteModule::pipette_ctrl_put_tip() { - ZLOGI(TAG, "pipette_ctrl_put_tip"); - if (creg.module_status == 1) return err::kdevice_is_busy; - return do_pipette_ctrl_put_tip(); -}; -int32_t PipetteModule::pipette_lld_prepare() { - ZLOGI(TAG, "pipette_lld_prepare"); - if (creg.module_status == 1) return err::kdevice_is_busy; - return do_pipette_lld_prepare(); -}; - -int32_t PipetteModule::pipette_plld(int32_t zdpos, int32_t p_threshold) { - ZLOGI(TAG, "pipette_plld %d %d", zdpos, p_threshold); - if (creg.module_status == 1) return err::kdevice_is_busy; - return do_pipette_lld(kplld, zdpos, 0, p_threshold); -}; -int32_t PipetteModule::pipette_clld(int32_t zdpos, int32_t c_threshold) { - ZLOGI(TAG, "pipette_clld %d %d", zdpos, c_threshold); - if (creg.module_status == 1) return err::kdevice_is_busy; - return do_pipette_lld(kclld, zdpos, c_threshold, 0); -} -int32_t PipetteModule::pipette_mlld(int32_t zdpos, int32_t c_threshold, int32_t p_threshold) { - ZLOGI(TAG, "pipette_mlld %d %d %d", zdpos, c_threshold, p_threshold); - if (creg.module_status == 1) return err::kdevice_is_busy; - return do_pipette_lld(kmixlld, zdpos, c_threshold, p_threshold); -} - -int32_t PipetteModule::pipette_aspirate(int32_t ul) { - ZLOGI(TAG, "pipette_aspirate %d", ul); - if (creg.module_status == 1) return err::kdevice_is_busy; - return do_pipette_aspirate(ul, 0); -}; -int32_t PipetteModule::pipette_distribut(int32_t ul) { - ZLOGI(TAG, "pipette_distribut %d", ul); - if (creg.module_status == 1) return err::kdevice_is_busy; - return do_pipette_distribut(ul, 0); -}; -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 do_pipette_shake_up(ul, 0, times); -} - -int32_t PipetteModule::pipette_lld_is_detect_liquid(int32_t *detect_liquid) { // - *detect_liquid = m_state.detected_liquid; - return 0; -} - -int32_t PipetteModule::pipette_aspirate_llf(int32_t ul, int32_t zmotor_v) { // - ZLOGI(TAG, "pipette_aspirate_llf %d %d %d", ul, zmotor_v); - if (creg.module_status == 1) return err::kdevice_is_busy; - return do_pipette_aspirate(ul, zmotor_v); -} -int32_t PipetteModule::pipette_distribut_llf(int32_t ul, int32_t zmotor_v) { // - ZLOGI(TAG, "pipette_distribut_llf %d %d %d", ul, zmotor_v); - if (creg.module_status == 1) return err::kdevice_is_busy; - return do_pipette_distribut(ul, 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 do_pipette_shake_up(ul, zmotor_v, times); -} - -/*********************************************************************************************************************** - * ACTION * - ***********************************************************************************************************************/ -int32_t PipetteModule::do_pipette_ctrl_init_device() { - m_thread.stop(); - creg.module_status = 1; - m_thread.start( - [this]() { // - DO_IN_THREAD(befor_run()); - DO_IN_THREAD(m_smtp2.pump_init(m_config.pump_vmax)); - 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); - } - }, - [this]() { // exit fn - after_run(); - m_smtp2.pump_stop(); - 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); - - } // - ); - return 0; -} -int32_t PipetteModule::do_pipette_ctrl_put_tip() { - m_thread.stop(); - creg.module_status = 1; - creg.module_errorcode = 0; - m_thread.start( - [this]() { // - DO_IN_THREAD(do_put_tip_before_run()); - DO_IN_THREAD(m_smtp2.pump_put_tip()); - ZLOGI(TAG, "put tip"); - while (true) { - if (!check_when_run()) break; - if (m_thread.getExitFlag()) break; - - int32_t isbusy = 0; - - 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(50); - } - ZLOGI(TAG, "put tip end"); - - }, - [this]() { // exit fn - after_run(); - m_smtp2.pump_stop(); - - 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; -} -int32_t PipetteModule::do_pipette_ctrl_move_to_ul(int32_t ul) { - m_thread.stop(); - creg.module_status = 1; - m_thread.start( - [this, ul]() { // - DO_IN_THREAD(befor_run()); - 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; - - int32_t isbusy = 0; - DO_IN_THREAD(m_smtp2.pump_get_state(&isbusy)); - if (isbusy == 0) break; - - m_thread.sleep(10); - } - }, - [this]() { // exit fn - after_run(); - m_smtp2.pump_stop(); - - ZLOGI(TAG, "do_pipette_ctrl_move_to_ul finish,ecode:%d", creg.module_errorcode); - } // - ); - return 0; -} - -int32_t PipetteModule::do_pipette_lld_prepare() { - /** - * @brief - * lld_prepare - * - * 检查: - * 1.泵机上没有tip头（保证液体不会被反向吸到泵机中） - * 2.泵机是否初始化过 - * - * 动作: - * 1. 移动泵机到泵机中间位置 - * - * 状态: - * 1. 修改lld_prepared为true - * - */ - - // 检查泵机是否初始化过 - if (m_state.pipette_is_inited == 0) return err::kpipette_error_uninited; - - // 检查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(); - creg.module_status = 1; - m_thread.start( - [this]() { // - DO_IN_THREAD(befor_run()); - DO_IN_THREAD(m_smtp2.pump_move_to_ul(m_config.pump_vmax, m_config.lld_prepare_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; - - m_thread.sleep(10); - } - }, - [this]() { // exit fn - after_run(); - m_smtp2.pump_stop(); - - if (creg.module_errorcode == 0) { - m_state.lld_prepared = 1; - } - ZLOGI(TAG, "do_pipette_lld_prepare finish,ecode:%d", creg.module_errorcode); - - } // - ); - return 0; -} -int32_t PipetteModule::do_pipette_lld(lld_mode_t mode, int32_t zdpos, int32_t c_threshold, int32_t p_threshold) { - /** - * @brief - * - * 液面探测前检查: - * 1. 需要执行过prepare - * 2. 没有执行过lld - * - * - * lld模式: - * p模式 : 移动电机,泵机执行plld M100V100t50,0R - * c模式 : 移动电机 - * mix1模式 : 移动电机，执行mlld - * - */ - - if (m_state.lld_prepared == 0) { - ZLOGI(TAG, "do_pipette_lld fail, not prepare"); - return err::kpipette_error_not_lld_prepare; - } - - ZASSERT(mode == kclld || mode == kplld || mode == kmixlld); - - m_thread.stop(); - creg.module_status = 1; - m_state.detected_liquid = 0; - m_thread.start( - [this, mode, zdpos, c_threshold, p_threshold]() { // - DO_IN_THREAD(befor_run()); - auto submotor = m_zmotor->getMotor(); - - int32_t start_capacitance = 0; // 启动时电容数值 - int32_t start_pressure = 0; // 启动时压力数值 - int32_t start_motor_pos = 0; // 启动时电机位置 - - // 准备 - m_zmotor->module_active_cfg(); - m_zmotor->step_motor_enable(1); - capturedata_num = 0; - - // 读取初值 - DO_IN_THREAD(m_smtp2.pump_get_capacitance(&start_capacitance)); - 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_pre_distribut_ul)); - while (true) { - if (!check_when_run()) return; - if (m_thread.getExitFlag()) return; - - int32_t isbusy = 0; - DO_IN_THREAD(m_smtp2.pump_get_state(&isbusy)); - if (isbusy == 0) break; - - m_thread.sleep(10); - } - m_smtp2.pump_stop(); - ZLOGI(TAG, "lld before distribut ok"); - - ZLOGI(TAG, "start lld"); - submotor->moveBy(zdpos, m_config.lld_motor_vel_rpm); - osDelay(10); - if (mode == kclld) { - DO_IN_THREAD(m_smtp2.pump_clld(c_threshold)); - } else if (mode == kplld) { - DO_IN_THREAD(m_smtp2.pump_distribut_plld(m_config.lld_pump_vel, p_threshold)); - } else if (mode == kmixlld) { - DO_IN_THREAD(m_smtp2.pump_distribut_mlld(m_config.lld_pump_vel, c_threshold, p_threshold)); - } - - 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; - int32_t pressure = 0; - - // 获取状态 - DO_IN_THREAD(m_smtp2.pump_get_capacitance(&capacitance)); - DO_IN_THREAD(m_smtp2.pump_get_pressure(&pressure)); - motorpos = submotor->getXACTUAL() - start_motor_pos; - - // 液面探测检测 - int32_t detect_liquid = 0; - if (mode == kclld) { - DO_IN_THREAD(m_smtp2.pump_clld_get_state(&detect_liquid)); - } else if (mode == kplld) { - DO_IN_THREAD(m_smtp2.pump_distribut_plld_get_state(&detect_liquid)); - } else if (mode == kmixlld) { - DO_IN_THREAD(m_smtp2.pump_distribut_mlld_get_state(&detect_liquid)); - } - - // 探测到液面 - if (detect_liquid == 1) { - ZLOGI(TAG, "detect liquid"); - m_state.detected_liquid = 1; - submotor->stop(); - push_snesor_sample_data(motorpos, capacitance, pressure); - break; - } - - // Z轴电机是否停止运动 - submotor->readMotorState(&motorstoped, &motorerror); - if (motorerror != 0) { - creg.module_errorcode = motorerror; - ZLOGI(TAG, "motorerror %d", motorerror); - 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); - 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(); - submotor->stop(); - m_smtp2.pump_stop(); - after_run(); - ZLOGI(TAG, "do_pipette_clld_test finish,detect liquid:%d", m_state.detected_liquid); - } // - ); - return 0; -} -int32_t PipetteModule::do_pipette_aspirate(int32_t ul, int32_t zmotor_v) { - /** - * @brief - */ - ZLOGI(TAG, "do_pipette_aspirate %d", ul); - - int32_t target_load_ul = ul + m_state.load_val_ul; - if (target_load_ul > m_config.limit_ul) { - return err::kparam_out_of_range; - } - - m_thread.stop(); - creg.module_status = 1; - m_thread.start( - [this, ul, zmotor_v]() { // - auto submotor = m_zmotor->getMotor(); - pipette_enable_zmotor(1); - - DO_IN_THREAD(befor_run()); - if (zmotor_v != 0) { - submotor->moveBy(LLF_DPOS, zmotor_v); - } - DO_IN_THREAD(m_smtp2.pump_aspirate(m_config.aspirate_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); - } - }, - [this]() { // exit fn - after_run(); - m_smtp2.pump_stop(); - m_zmotor->getMotor()->stop(); - // ZLOGI(TAG, "do_pipette_aspirate finish, load_val_ul:%d,m_state.dul:%d before:%d after:%d", m_state.load_val_ul, m_state.dul, m_state.pump_before_pos_ul, m_state.pump_after_pos_ul); - 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; - } - m_state.aspirated = 1; - m_state.lld_prepared = 0; - ZLOGI(TAG, "do_pipette_aspirate finish, load_val_ul:%d", m_state.load_val_ul); - } // - ); - return 0; -} -int32_t PipetteModule::do_pipette_distribut(int32_t ul, int32_t zmotor_v) { - /** - * @brief - */ - - ZLOGI(TAG, "do_pipette_distribut %d", ul); - - m_thread.stop(); - creg.module_status = 1; - m_thread.start( - - [this, ul, zmotor_v]() { // - auto submotor = m_zmotor->getMotor(); - pipette_enable_zmotor(1); - DO_IN_THREAD(befor_run()); - - int32_t distribut_ul = ul; - if (distribut_ul == 0) distribut_ul = m_state.load_val_ul; - DO_IN_THREAD(m_smtp2.pump_distribut(m_config.aspirate_pump_vel, distribut_ul)); - if (zmotor_v != 0) { - submotor->moveBy(-LLF_DPOS, zmotor_v); - } - 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); - } - }, - [this, ul]() { // 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; - } - if (ul == 0 && m_state.load_val_ul == 1) { - m_state.load_val_ul = 0; - } - - m_state.lld_prepared = 0; - ZLOGI(TAG, "do_pipette_distribut finish, load_val_ul:%d", m_state.load_val_ul); - - } // - ); - 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; - } - - if (ul > m_config.limit_ul) { - return err::kparam_out_of_range; - } - - 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_pump_vel, ul)); - while (true) { - if (!check_when_run()) 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_pump_vel, ul)); - if (zmotor_v != 0) submotor->moveBy(-dpos, zmotor_v); - while (true) { - if (!check_when_run()) 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 || m_thread.getExitFlag()) { - 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_pump_vel, ul)); - while (true) { - if (!check_when_run()) 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", m_state.load_val_ul); - } // - ); - 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; - *motor_pos = capturedata_buf[index].motor_pos; - *cval = capturedata_buf[index].cap_val; - *pval = capturedata_buf[index].pressure_val; - return 0; -} -int32_t PipetteModule::pipette_get_sensor_sample_data_num(int32_t *num) { - *num = capturedata_num; - return 0; -} - -int32_t PipetteModule::pipette_write_cmd_direct(uint8_t *tx, int32_t len, uint8_t *rx, int32_t *rxlen) { - bool suc = m_smtp2.write_cmd((char *)tx); - if (!suc) { - return err::ksubdevice_overtime; - } - m_smtp2.getack((char *)rx, rxlen); - // ZLOGI(TAG, "rxlen = %d", *rxlen); - return 0; -} - -/*********************************************************************************************************************** - * 方法通用钩子 * - ***********************************************************************************************************************/ - -int32_t PipetteModule::befor_run() { - // - ZLOGI(TAG, "befor_run"); - - m_smtp2.pump_stop(); - creg.module_status = 1; - creg.module_errorcode = 0; - m_state.dul = 0; - DO(module_active_cfg()); - DO(m_smtp2.pump_get_motor_pos_ul(&m_state.pump_before_pos_ul)); - - return 0; -} - -int32_t PipetteModule::do_put_tip_before_run() { - creg.module_status = 1; - creg.module_errorcode = 0; - m_state.dul = 0; - return 0; -} - -int32_t PipetteModule::after_run() { - ZLOGI(TAG, "after_run"); - m_smtp2.pump_stop(); - - int32_t err = 0; - err = creg.module_errorcode; - if (err != 0) goto tag_err; - - err = m_smtp2.pump_get_motor_pos_ul(&m_state.pump_after_pos_ul); - if (err != 0) goto tag_err; - - m_state.dul = m_state.pump_after_pos_ul - m_state.pump_before_pos_ul; - - ZLOGI(TAG, "after_run before:%d after:%d dul:%d", m_state.pump_before_pos_ul, m_state.pump_after_pos_ul, m_state.dul); - - creg.module_status = 0; - return 0; - -tag_err: - ZLOGI(TAG, "after_run detecte, error %d", err); - creg.module_status = 2; - creg.module_errorcode = err; - return 0; -} -bool PipetteModule::check_when_run() { return true; } - -void PipetteModule::push_snesor_sample_data(int32_t motor_pos, int32_t cap_val, int32_t pressure_val) { - if (capturedata_num >= PIPETTEMODULE_SAMPLE_BUF_SIZE) { - ZLOGW(TAG, "capturedata_buf overflow"); - return; - } - - ZLOGI(TAG, "sample %2d mpos:%2d cap:%4d pressure_val:%4d", capturedata_num, motor_pos, cap_val, pressure_val); - capturedata_buf[capturedata_num].motor_pos = motor_pos; - capturedata_buf[capturedata_num].cap_val = cap_val; - capturedata_buf[capturedata_num].pressure_val = pressure_val; - capturedata_num++; -} -// 高试管800 - -/******************************************************************************* - * PRIVATE * - *******************************************************************************/ -int32_t PipetteModule::read_pos_ul(int32_t *val) { // - *val = m_state.load_val_ul; - return 0; -} -int32_t PipetteModule::read_capactitance(int32_t *val) { - m_smtp2.pump_get_capacitance(val); - return 0; -} -int32_t PipetteModule::read_tip_state(int32_t *val) { - *val = 0; - m_smtp2.pump_get_tip_state(val); - return 0; -} \ No newline at end of file +// #include "pipette_ctrl_module_v1.hpp" + +// #include "sdk\components\zcancmder\protocol_event_bus_sender.hpp" +// using namespace iflytop; + +// #define TAG "PipetteModule" + +// #define DO(func) \ +// { \ +// int32_t ecode = func; \ +// if (ecode != 0) { \ +// ZLOGE(TAG, "[%d] do %s fail, error %s(%d)", __LINE__, #func, err::error2str(ecode), ecode); \ +// return ecode; \ +// } \ +// } + +// #define DO_IN_THREAD(func) \ +// { \ +// int32_t ecode = func; \ +// if (ecode != 0) { \ +// ZLOGE(TAG, "[%d]do %s fail, error %s(%d) ", __LINE__, #func, err::error2str(ecode), ecode); \ +// creg.module_errorcode = ecode; \ +// return; \ +// } \ +// } + +// #define PIPETTE_PREPARE_POS 800 +// #define LLF_DPOS 1000 + +// void PipetteModule::initialize(int32_t id, config_t *config, StepMotorCtrlModule *zmotor, hardward_config_t *hardwaredcfg) { // +// ZASSERT(config != nullptr); +// ZASSERT(hardwaredcfg != nullptr); +// m_id = id; +// m_config = *config; +// m_smtp2.initialize(hardwaredcfg->uart, 0, hardwaredcfg->hdma_rx, hardwaredcfg->hdma_tx); +// m_thread.init("pipette_ctrl", 1024); + +// m_config.limit_ul = 200; +// m_config.pump_acc = 14; +// m_config.pump_dec = 14; +// m_config.pump_vstart = 0; +// m_config.pump_vstop = 900; +// m_config.aspirate_pump_vel = 500; // +// m_config.pump_vmax = 1000; +// m_config.lld_pump_vel = 100; // lld推荐使用速度为50->200,这里 +// m_config.lld_motor_vel_rpm = 200; +// m_config.lld_detect_period_ms = 10; // 100ms +// m_config.lld_prepare_pos = PIPETTE_PREPARE_POS; +// m_config.lld_prepare_pre_distribut_ul = 100; + +// ZASSERT(zmotor); +// m_zmotor = zmotor; +// } + +// int32_t PipetteModule::getid(int32_t *id) { +// *id = m_id; +// return 0; +// } +// /******************************************************************************* +// * Module * +// *******************************************************************************/ + +// #define PROCESS_REG_CFG(cfgkey, cfg) PROCESS_REG(cfgkey, /* */ REG_GET(m_config.cfg), REG_SET(m_config.cfg)) + +// int32_t PipetteModule::module_xxx_reg(int32_t param_id, bool read, int32_t &val) { +// switch (param_id) { +// MODULE_COMMON_PROCESS_REG_CB(); +// PROCESS_REG(kreg_pipette_pos_ul, /* */ read_pos_ul(&val), ACTION_NONE); +// PROCESS_REG(kreg_pipette_capactitance_val, /* */ read_capactitance(&val), ACTION_NONE); +// PROCESS_REG(kreg_pipette_tip_state, /* */ read_tip_state(&val), ACTION_NONE); + +// PROCESS_REG_CFG(kreg_pipette_limit_ul, limit_ul); +// PROCESS_REG_CFG(kreg_pipette_pump_acc, pump_acc); +// PROCESS_REG_CFG(kreg_pipette_pump_dec, pump_dec); +// PROCESS_REG_CFG(kreg_pipette_pump_vstart, pump_vstart); +// PROCESS_REG_CFG(kreg_pipette_pump_vstop, pump_vstop); +// PROCESS_REG_CFG(kreg_pipette_pump_vmax, pump_vmax); +// PROCESS_REG_CFG(kreg_pipette_aspirate_pump_vel, aspirate_pump_vel); +// 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_pre_distribut_ul, lld_prepare_pre_distribut_ul); + +// default: +// return err::kmodule_not_find_reg; +// break; +// } +// return 0; +// } + +// int32_t PipetteModule::module_stop() { +// m_thread.stop(); +// m_smtp2.pump_stop(); +// m_zmotor->module_stop(); +// return 0; +// } +// int32_t PipetteModule::module_active_cfg() { +// DO(m_smtp2.pump_set_acc_and_dec(m_config.pump_acc, m_config.pump_dec)); +// DO(m_smtp2.pump_set_vstart(m_config.pump_vstart)); +// DO(m_smtp2.pump_set_vstop(m_config.pump_vstop)); +// return 0; +// } + +// /*********************************************************************************************************************** +// * EXTERN ACTION * +// ***********************************************************************************************************************/ + +// // 兼容旧程序 +// int32_t PipetteModule::pipette_ctrl_move_to_ul(int32_t ul) { +// ZLOGI(TAG, "pipette_ctrl_move_to_ul %d", ul); // not support +// return err::kcmd_not_support; +// }; + +// int32_t PipetteModule::pipette_ctrl_init_device() { +// ZLOGI(TAG, "pipette_ctrl_init_device"); +// if (creg.module_status == 1) return err::kdevice_is_busy; +// return do_pipette_ctrl_init_device(); +// }; +// int32_t PipetteModule::pipette_ctrl_put_tip() { +// ZLOGI(TAG, "pipette_ctrl_put_tip"); +// if (creg.module_status == 1) return err::kdevice_is_busy; +// return do_pipette_ctrl_put_tip(); +// }; +// int32_t PipetteModule::pipette_lld_prepare() { +// ZLOGI(TAG, "pipette_lld_prepare"); +// if (creg.module_status == 1) return err::kdevice_is_busy; +// return do_pipette_lld_prepare(); +// }; + +// int32_t PipetteModule::pipette_plld(int32_t zdpos, int32_t p_threshold) { +// ZLOGI(TAG, "pipette_plld %d %d", zdpos, p_threshold); +// if (creg.module_status == 1) return err::kdevice_is_busy; +// return do_pipette_lld(kplld, zdpos, 0, p_threshold); +// }; +// int32_t PipetteModule::pipette_clld(int32_t zdpos, int32_t c_threshold) { +// ZLOGI(TAG, "pipette_clld %d %d", zdpos, c_threshold); +// if (creg.module_status == 1) return err::kdevice_is_busy; +// return do_pipette_lld(kclld, zdpos, c_threshold, 0); +// } +// int32_t PipetteModule::pipette_mlld(int32_t zdpos, int32_t c_threshold, int32_t p_threshold) { +// ZLOGI(TAG, "pipette_mlld %d %d %d", zdpos, c_threshold, p_threshold); +// if (creg.module_status == 1) return err::kdevice_is_busy; +// return do_pipette_lld(kmixlld, zdpos, c_threshold, p_threshold); +// } + +// int32_t PipetteModule::pipette_aspirate(int32_t ul) { +// ZLOGI(TAG, "pipette_aspirate %d", ul); +// if (creg.module_status == 1) return err::kdevice_is_busy; +// return do_pipette_aspirate(ul, 0); +// }; +// int32_t PipetteModule::pipette_distribut(int32_t ul) { +// ZLOGI(TAG, "pipette_distribut %d", ul); +// if (creg.module_status == 1) return err::kdevice_is_busy; +// return do_pipette_distribut(ul, 0); +// }; +// 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 do_pipette_shake_up(ul, 0, times); +// } + +// int32_t PipetteModule::pipette_lld_is_detect_liquid(int32_t *detect_liquid) { // +// *detect_liquid = m_state.detected_liquid; +// return 0; +// } + +// int32_t PipetteModule::pipette_aspirate_llf(int32_t ul, int32_t zmotor_v) { // +// ZLOGI(TAG, "pipette_aspirate_llf %d %d %d", ul, zmotor_v); +// if (creg.module_status == 1) return err::kdevice_is_busy; +// return do_pipette_aspirate(ul, zmotor_v); +// } +// int32_t PipetteModule::pipette_distribut_llf(int32_t ul, int32_t zmotor_v) { // +// ZLOGI(TAG, "pipette_distribut_llf %d %d %d", ul, zmotor_v); +// if (creg.module_status == 1) return err::kdevice_is_busy; +// return do_pipette_distribut(ul, 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 do_pipette_shake_up(ul, zmotor_v, times); +// } + +// /*********************************************************************************************************************** +// * ACTION * +// ***********************************************************************************************************************/ +// int32_t PipetteModule::do_pipette_ctrl_init_device() { +// m_thread.stop(); +// creg.module_status = 1; +// m_thread.start( +// [this]() { // +// DO_IN_THREAD(befor_run()); +// DO_IN_THREAD(m_smtp2.pump_init(m_config.pump_vmax)); +// 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); +// } +// }, +// [this]() { // exit fn +// after_run(); +// m_smtp2.pump_stop(); +// 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); + +// } // +// ); +// return 0; +// } +// int32_t PipetteModule::do_pipette_ctrl_put_tip() { +// m_thread.stop(); +// creg.module_status = 1; +// creg.module_errorcode = 0; +// m_thread.start( +// [this]() { // +// DO_IN_THREAD(do_put_tip_before_run()); +// DO_IN_THREAD(m_smtp2.pump_put_tip()); +// ZLOGI(TAG, "put tip"); +// while (true) { +// if (!check_when_run()) break; +// if (m_thread.getExitFlag()) break; + +// int32_t isbusy = 0; + +// 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(50); +// } +// ZLOGI(TAG, "put tip end"); + +// }, +// [this]() { // exit fn +// after_run(); +// m_smtp2.pump_stop(); + +// 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; +// } +// int32_t PipetteModule::do_pipette_ctrl_move_to_ul(int32_t ul) { +// m_thread.stop(); +// creg.module_status = 1; +// m_thread.start( +// [this, ul]() { // +// DO_IN_THREAD(befor_run()); +// 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; + +// int32_t isbusy = 0; +// DO_IN_THREAD(m_smtp2.pump_get_state(&isbusy)); +// if (isbusy == 0) break; + +// m_thread.sleep(10); +// } +// }, +// [this]() { // exit fn +// after_run(); +// m_smtp2.pump_stop(); + +// ZLOGI(TAG, "do_pipette_ctrl_move_to_ul finish,ecode:%d", creg.module_errorcode); +// } // +// ); +// return 0; +// } + +// int32_t PipetteModule::do_pipette_lld_prepare() { +// /** +// * @brief +// * lld_prepare +// * +// * 检查: +// * 1.泵机上没有tip头（保证液体不会被反向吸到泵机中） +// * 2.泵机是否初始化过 +// * +// * 动作: +// * 1. 移动泵机到泵机中间位置 +// * +// * 状态: +// * 1. 修改lld_prepared为true +// * +// */ + +// // 检查泵机是否初始化过 +// if (m_state.pipette_is_inited == 0) return err::kpipette_error_uninited; + +// // 检查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(); +// creg.module_status = 1; +// m_thread.start( +// [this]() { // +// DO_IN_THREAD(befor_run()); +// DO_IN_THREAD(m_smtp2.pump_move_to_ul(m_config.pump_vmax, m_config.lld_prepare_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; + +// m_thread.sleep(10); +// } +// }, +// [this]() { // exit fn +// after_run(); +// m_smtp2.pump_stop(); + +// if (creg.module_errorcode == 0) { +// m_state.lld_prepared = 1; +// } +// ZLOGI(TAG, "do_pipette_lld_prepare finish,ecode:%d", creg.module_errorcode); + +// } // +// ); +// return 0; +// } +// int32_t PipetteModule::do_pipette_lld(lld_mode_t mode, int32_t zdpos, int32_t c_threshold, int32_t p_threshold) { +// /** +// * @brief +// * +// * 液面探测前检查: +// * 1. 需要执行过prepare +// * 2. 没有执行过lld +// * +// * +// * lld模式: +// * p模式 : 移动电机,泵机执行plld M100V100t50,0R +// * c模式 : 移动电机 +// * mix1模式 : 移动电机，执行mlld +// * +// */ + +// if (m_state.lld_prepared == 0) { +// ZLOGI(TAG, "do_pipette_lld fail, not prepare"); +// return err::kpipette_error_not_lld_prepare; +// } + +// ZASSERT(mode == kclld || mode == kplld || mode == kmixlld); + +// m_thread.stop(); +// creg.module_status = 1; +// m_state.detected_liquid = 0; +// m_thread.start( +// [this, mode, zdpos, c_threshold, p_threshold]() { // +// DO_IN_THREAD(befor_run()); +// auto submotor = m_zmotor->getMotor(); + +// int32_t start_capacitance = 0; // 启动时电容数值 +// int32_t start_pressure = 0; // 启动时压力数值 +// int32_t start_motor_pos = 0; // 启动时电机位置 + +// // 准备 +// m_zmotor->module_active_cfg(); +// m_zmotor->step_motor_enable(1); +// capturedata_num = 0; + +// // 读取初值 +// DO_IN_THREAD(m_smtp2.pump_get_capacitance(&start_capacitance)); +// 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_pre_distribut_ul)); +// while (true) { +// if (!check_when_run()) return; +// if (m_thread.getExitFlag()) return; + +// int32_t isbusy = 0; +// DO_IN_THREAD(m_smtp2.pump_get_state(&isbusy)); +// if (isbusy == 0) break; + +// m_thread.sleep(10); +// } +// m_smtp2.pump_stop(); +// ZLOGI(TAG, "lld before distribut ok"); + +// ZLOGI(TAG, "start lld"); +// submotor->moveBy(zdpos, m_config.lld_motor_vel_rpm); +// osDelay(10); +// if (mode == kclld) { +// DO_IN_THREAD(m_smtp2.pump_clld(c_threshold)); +// } else if (mode == kplld) { +// DO_IN_THREAD(m_smtp2.pump_distribut_plld(m_config.lld_pump_vel, p_threshold)); +// } else if (mode == kmixlld) { +// DO_IN_THREAD(m_smtp2.pump_distribut_mlld(m_config.lld_pump_vel, c_threshold, p_threshold)); +// } + +// 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; +// int32_t pressure = 0; + +// // 获取状态 +// DO_IN_THREAD(m_smtp2.pump_get_capacitance(&capacitance)); +// DO_IN_THREAD(m_smtp2.pump_get_pressure(&pressure)); +// motorpos = submotor->getXACTUAL() - start_motor_pos; + +// // 液面探测检测 +// int32_t detect_liquid = 0; +// if (mode == kclld) { +// DO_IN_THREAD(m_smtp2.pump_clld_get_state(&detect_liquid)); +// } else if (mode == kplld) { +// DO_IN_THREAD(m_smtp2.pump_distribut_plld_get_state(&detect_liquid)); +// } else if (mode == kmixlld) { +// DO_IN_THREAD(m_smtp2.pump_distribut_mlld_get_state(&detect_liquid)); +// } + +// // 探测到液面 +// if (detect_liquid == 1) { +// ZLOGI(TAG, "detect liquid"); +// m_state.detected_liquid = 1; +// submotor->stop(); +// push_snesor_sample_data(motorpos, capacitance, pressure); +// break; +// } + +// // Z轴电机是否停止运动 +// submotor->readMotorState(&motorstoped, &motorerror); +// if (motorerror != 0) { +// creg.module_errorcode = motorerror; +// ZLOGI(TAG, "motorerror %d", motorerror); +// 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); +// 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(); +// submotor->stop(); +// m_smtp2.pump_stop(); +// after_run(); +// ZLOGI(TAG, "do_pipette_clld_test finish,detect liquid:%d", m_state.detected_liquid); +// } // +// ); +// return 0; +// } +// int32_t PipetteModule::do_pipette_aspirate(int32_t ul, int32_t zmotor_v) { +// /** +// * @brief +// */ +// ZLOGI(TAG, "do_pipette_aspirate %d", ul); + +// int32_t target_load_ul = ul + m_state.load_val_ul; +// if (target_load_ul > m_config.limit_ul) { +// return err::kparam_out_of_range; +// } + +// m_thread.stop(); +// creg.module_status = 1; +// m_thread.start( +// [this, ul, zmotor_v]() { // +// auto submotor = m_zmotor->getMotor(); +// pipette_enable_zmotor(1); + +// DO_IN_THREAD(befor_run()); +// if (zmotor_v != 0) { +// submotor->moveBy(LLF_DPOS, zmotor_v); +// } +// DO_IN_THREAD(m_smtp2.pump_aspirate(m_config.aspirate_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); +// } +// }, +// [this]() { // exit fn +// after_run(); +// m_smtp2.pump_stop(); +// m_zmotor->getMotor()->stop(); +// // ZLOGI(TAG, "do_pipette_aspirate finish, load_val_ul:%d,m_state.dul:%d before:%d after:%d", m_state.load_val_ul, m_state.dul, m_state.pump_before_pos_ul, m_state.pump_after_pos_ul); +// 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; +// } +// m_state.aspirated = 1; +// m_state.lld_prepared = 0; +// ZLOGI(TAG, "do_pipette_aspirate finish, load_val_ul:%d", m_state.load_val_ul); +// } // +// ); +// return 0; +// } +// int32_t PipetteModule::do_pipette_distribut(int32_t ul, int32_t zmotor_v) { +// /** +// * @brief +// */ + +// ZLOGI(TAG, "do_pipette_distribut %d", ul); + +// m_thread.stop(); +// creg.module_status = 1; +// m_thread.start( + +// [this, ul, zmotor_v]() { // +// auto submotor = m_zmotor->getMotor(); +// pipette_enable_zmotor(1); +// DO_IN_THREAD(befor_run()); + +// int32_t distribut_ul = ul; +// if (distribut_ul == 0) distribut_ul = m_state.load_val_ul; +// DO_IN_THREAD(m_smtp2.pump_distribut(m_config.aspirate_pump_vel, distribut_ul)); +// if (zmotor_v != 0) { +// submotor->moveBy(-LLF_DPOS, zmotor_v); +// } +// 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); +// } +// }, +// [this, ul]() { // 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; +// } +// if (ul == 0 && m_state.load_val_ul == 1) { +// m_state.load_val_ul = 0; +// } + +// m_state.lld_prepared = 0; +// ZLOGI(TAG, "do_pipette_distribut finish, load_val_ul:%d", m_state.load_val_ul); + +// } // +// ); +// 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; +// } + +// if (ul > m_config.limit_ul) { +// return err::kparam_out_of_range; +// } + +// 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_pump_vel, ul)); +// while (true) { +// if (!check_when_run()) 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_pump_vel, ul)); +// if (zmotor_v != 0) submotor->moveBy(-dpos, zmotor_v); +// while (true) { +// if (!check_when_run()) 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 || m_thread.getExitFlag()) { +// 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_pump_vel, ul)); +// while (true) { +// if (!check_when_run()) 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", m_state.load_val_ul); +// } // +// ); +// 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; +// *motor_pos = capturedata_buf[index].motor_pos; +// *cval = capturedata_buf[index].cap_val; +// *pval = capturedata_buf[index].pressure_val; +// return 0; +// } +// int32_t PipetteModule::pipette_get_sensor_sample_data_num(int32_t *num) { +// *num = capturedata_num; +// return 0; +// } + +// int32_t PipetteModule::pipette_write_cmd_direct(uint8_t *tx, int32_t len, uint8_t *rx, int32_t *rxlen) { +// bool suc = m_smtp2.write_cmd((char *)tx); +// if (!suc) { +// return err::ksubdevice_overtime; +// } +// m_smtp2.getack((char *)rx, rxlen); +// // ZLOGI(TAG, "rxlen = %d", *rxlen); +// return 0; +// } + +// /*********************************************************************************************************************** +// * 方法通用钩子 * +// ***********************************************************************************************************************/ + +// int32_t PipetteModule::befor_run() { +// // +// ZLOGI(TAG, "befor_run"); + +// m_smtp2.pump_stop(); +// creg.module_status = 1; +// creg.module_errorcode = 0; +// m_state.dul = 0; +// DO(module_active_cfg()); +// DO(m_smtp2.pump_get_motor_pos_ul(&m_state.pump_before_pos_ul)); + +// return 0; +// } + +// int32_t PipetteModule::do_put_tip_before_run() { +// creg.module_status = 1; +// creg.module_errorcode = 0; +// m_state.dul = 0; +// return 0; +// } + +// int32_t PipetteModule::after_run() { +// ZLOGI(TAG, "after_run"); +// m_smtp2.pump_stop(); + +// int32_t err = 0; +// err = creg.module_errorcode; +// if (err != 0) goto tag_err; + +// err = m_smtp2.pump_get_motor_pos_ul(&m_state.pump_after_pos_ul); +// if (err != 0) goto tag_err; + +// m_state.dul = m_state.pump_after_pos_ul - m_state.pump_before_pos_ul; + +// ZLOGI(TAG, "after_run before:%d after:%d dul:%d", m_state.pump_before_pos_ul, m_state.pump_after_pos_ul, m_state.dul); + +// creg.module_status = 0; +// return 0; + +// tag_err: +// ZLOGI(TAG, "after_run detecte, error %d", err); +// creg.module_status = 2; +// creg.module_errorcode = err; +// return 0; +// } +// bool PipetteModule::check_when_run() { return true; } + +// void PipetteModule::push_snesor_sample_data(int32_t motor_pos, int32_t cap_val, int32_t pressure_val) { +// if (capturedata_num >= PIPETTEMODULE_SAMPLE_BUF_SIZE) { +// ZLOGW(TAG, "capturedata_buf overflow"); +// return; +// } + +// ZLOGI(TAG, "sample %2d mpos:%2d cap:%4d pressure_val:%4d", capturedata_num, motor_pos, cap_val, pressure_val); +// capturedata_buf[capturedata_num].motor_pos = motor_pos; +// capturedata_buf[capturedata_num].cap_val = cap_val; +// capturedata_buf[capturedata_num].pressure_val = pressure_val; +// capturedata_num++; +// } +// // 高试管800 + +// /******************************************************************************* +// * PRIVATE * +// *******************************************************************************/ +// int32_t PipetteModule::read_pos_ul(int32_t *val) { // +// *val = m_state.load_val_ul; +// return 0; +// } +// int32_t PipetteModule::read_capactitance(int32_t *val) { +// m_smtp2.pump_get_capacitance(val); +// return 0; +// } +// int32_t PipetteModule::read_tip_state(int32_t *val) { +// *val = 0; +// m_smtp2.pump_get_tip_state(val); +// return 0; +// } \ No newline at end of file diff --git a/sdk/components/pipette_module/pipette_ctrl_module_v1.hpp b/sdk/components/pipette_module/pipette_ctrl_module_v1.hpp index affdfb1..e26ceba 100644 --- a/sdk/components/pipette_module/pipette_ctrl_module_v1.hpp +++ b/sdk/components/pipette_module/pipette_ctrl_module_v1.hpp @@ -1,185 +1,185 @@ #pragma once -// -#include "a8000_protocol\protocol.hpp" -#include "sdk/os/zos.hpp" -#include "sdk\components\sensors\smtp2_v2\smtp2_v2.hpp" -#include "sdk\components\step_motor_ctrl_module\step_motor_ctrl_module.hpp" -// #include "StepMotorCtrlModule" - -/** - * @brief - * - * 参考:https://iflytop1.feishu.cn/wiki/LTilwSyJwi6fJUkiIhzc2oRqnTb 对设备进行初始化 - * - */ -namespace iflytop { - -#define PIPETTEMODULE_SAMPLE_BUF_SIZE 300 - -class PipetteModule : public ZIModule, public ZIPipetteCtrlModule { - ENABLE_MODULE(PipetteModule, kpipette_ctrl_module, PC_VERSION); - - public: - typedef enum { - kclld = 0, - kplld = 1, - kmixlld = 2, - } lld_mode_t; - - typedef struct { - UART_HandleTypeDef *uart; - DMA_HandleTypeDef *hdma_rx; - DMA_HandleTypeDef *hdma_tx; - } hardward_config_t; - - typedef struct { - int32_t motor_pos; - int32_t cap_val; // 电容值 - int32_t pressure_val; // 压力值 - } snesor_sample_data_t; - - typedef struct { - int32_t limit_ul; - int32_t pump_acc; - int32_t pump_dec; - int32_t pump_vstart; - int32_t pump_vstop; - int32_t pump_vmax; - int32_t aspirate_pump_vel; // 吸吐泵机速度 - 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_pre_distribut_ul; // lld前，预分配位置 - } config_t; - - typedef struct { - int32_t load_val_ul; // 已装载量 - int32_t aspirated; // 已经吸入过液体 - int32_t lld_prepared; // 已经准备液面探测 - int32_t pipette_is_inited; // 泵机是否初始化 - - int32_t dul; // 吸吐变化量 - int32_t pump_before_pos_ul; // 执行指令前泵机位置 - int32_t pump_after_pos_ul; // 执行指令后泵机位置 - int32_t detected_liquid; // 检测到液面 - } state_t; - - private: - smtp2::SMTP2V2 m_smtp2; - StepMotorCtrlModule *m_zmotor = nullptr; - - ZThread m_thread; - - int32_t m_id = 0; - config_t m_config = {0}; - state_t m_state = {0}; - - snesor_sample_data_t capturedata_buf[PIPETTEMODULE_SAMPLE_BUF_SIZE]; - int32_t capturedata_num = 0; - - public: - void initialize(int32_t id, config_t *config, StepMotorCtrlModule *zmotor, hardward_config_t *hardwaredcfg); - - virtual int32_t getid(int32_t *id); - - /******************************************************************************* - * Module * - *******************************************************************************/ - - virtual int32_t module_stop() override; - virtual int32_t module_active_cfg() override; - virtual int32_t module_xxx_reg(int32_t param_id, bool read, int32_t &val) override; - - /******************************************************************************* - * pipette_ctrl * - *******************************************************************************/ - /** - * 参数: - * vstart - * vstop - * vmax - * acc - * dec - * - * 0.泵机复位 - * 0.Z轴失能 - * 1.移液枪初始化 - * 2.移液枪探测液面初始化 - * - * 3.移液枪-电容探测液面-测试(moterv,dpos)，动作:Z轴向下以motor_v移动dpos,使用电容模式 ,同时采集电容数值 - * 3.移液枪-压力探测液面-测试(moterv,dpos)，动作:Z轴向下以motor_v移动dpos,使用分配压力模式,压力阈值设置为最大, 同时采集电容数值，压力值 - * 4.读取压力和电容数值变化数据(电机位置，电容数值，压力数值，泵机位置) - * - * 5.吸液(ul) - * 6.排液(ul) - * - * - * - * - * 使用流程 - * - * pipette_ctrl_put_tip - * | - * pipette_lld_prepare - * | - * pipette_lld - * | - * pipette_aspirate - * pipette_distribut - * pipette_mix(吸吐混匀) - * - * - */ - - virtual int32_t pipette_ctrl_move_to_ul(int32_t ul) override; // 丢弃 - - virtual int32_t pipette_ctrl_init_device() override; - virtual int32_t pipette_ctrl_put_tip() override; - virtual int32_t pipette_lld_prepare() override; - virtual int32_t pipette_plld(int32_t zdpos, int32_t p_threshold) override; - virtual int32_t pipette_clld(int32_t zdpos, int32_t c_threshold) override; - virtual int32_t pipette_mlld(int32_t zdpos, int32_t c_threshold, int32_t p_threshold) override; - virtual int32_t pipette_lld_is_detect_liquid(int32_t *liquid) override; - - virtual int32_t pipette_aspirate(int32_t ul) override; - virtual int32_t pipette_distribut(int32_t ul) override; - virtual int32_t pipette_shake_up(int32_t ul, int32_t times) override; - - virtual int32_t pipette_aspirate_llf(int32_t ul, int32_t zmotor_v) override; - virtual int32_t pipette_distribut_llf(int32_t ul, int32_t zmotor_v) override; - virtual int32_t pipette_shake_up_llf(int32_t ul, int32_t zmotor_v, int32_t times) override; - - virtual int32_t pipette_enable_zmotor(int32_t enable); - virtual int32_t pipette_write_cmd_direct(uint8_t *tx, int32_t len, uint8_t *rx, int32_t *rxlen) override; - virtual int32_t pipette_get_sensor_sample_data(int32_t index, int32_t *motor_pos, int32_t *cval, int32_t *pval) override; // - virtual int32_t pipette_get_sensor_sample_data_num(int32_t *num) override; - - private: - int32_t do_pipette_ctrl_init_device(); - int32_t do_pipette_ctrl_put_tip(); - int32_t do_pipette_ctrl_move_to_ul(int32_t ul); - int32_t do_pipette_lld_prepare(); - int32_t do_pipette_lld(lld_mode_t mode, int32_t zdpos, int32_t c_threshold, int32_t p_threshold); - - 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(); - int32_t do_put_tip_before_run(); - int32_t after_run(); - bool check_when_run(); - - private: - int32_t read_pos_ul(int32_t *val); - int32_t read_capactitance(int32_t *val); - int32_t read_tip_state(int32_t *val); - - private: - - private: - void push_snesor_sample_data(int32_t motor_pos, int32_t cap_val, int32_t pressure_val); -}; -} // namespace iflytop +// // +// #include "a8000_protocol\protocol.hpp" +// #include "sdk/os/zos.hpp" +// #include "sdk\components\sensors\smtp2_v2\smtp2_v2.hpp" +// #include "sdk\components\step_motor_ctrl_module\step_motor_ctrl_module.hpp" +// // #include "StepMotorCtrlModule" + +// /** +// * @brief +// * +// * 参考:https://iflytop1.feishu.cn/wiki/LTilwSyJwi6fJUkiIhzc2oRqnTb 对设备进行初始化 +// * +// */ +// namespace iflytop { + +// #define PIPETTEMODULE_SAMPLE_BUF_SIZE 300 + +// class PipetteModule : public ZIModule, public ZIPipetteCtrlModule { +// ENABLE_MODULE(PipetteModule, kpipette_ctrl_module, PC_VERSION); + +// public: +// typedef enum { +// kclld = 0, +// kplld = 1, +// kmixlld = 2, +// } lld_mode_t; + +// typedef struct { +// UART_HandleTypeDef *uart; +// DMA_HandleTypeDef *hdma_rx; +// DMA_HandleTypeDef *hdma_tx; +// } hardward_config_t; + +// typedef struct { +// int32_t motor_pos; +// int32_t cap_val; // 电容值 +// int32_t pressure_val; // 压力值 +// } snesor_sample_data_t; + +// typedef struct { +// int32_t limit_ul; +// int32_t pump_acc; +// int32_t pump_dec; +// int32_t pump_vstart; +// int32_t pump_vstop; +// int32_t pump_vmax; +// int32_t aspirate_pump_vel; // 吸吐泵机速度 +// 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_pre_distribut_ul; // lld前，预分配位置 +// } config_t; + +// typedef struct { +// int32_t load_val_ul; // 已装载量 +// int32_t aspirated; // 已经吸入过液体 +// int32_t lld_prepared; // 已经准备液面探测 +// int32_t pipette_is_inited; // 泵机是否初始化 + +// int32_t dul; // 吸吐变化量 +// int32_t pump_before_pos_ul; // 执行指令前泵机位置 +// int32_t pump_after_pos_ul; // 执行指令后泵机位置 +// int32_t detected_liquid; // 检测到液面 +// } state_t; + +// private: +// smtp2::SMTP2V2 m_smtp2; +// StepMotorCtrlModule *m_zmotor = nullptr; + +// ZThread m_thread; + +// int32_t m_id = 0; +// config_t m_config = {0}; +// state_t m_state = {0}; + +// snesor_sample_data_t capturedata_buf[PIPETTEMODULE_SAMPLE_BUF_SIZE]; +// int32_t capturedata_num = 0; + +// public: +// void initialize(int32_t id, config_t *config, StepMotorCtrlModule *zmotor, hardward_config_t *hardwaredcfg); + +// virtual int32_t getid(int32_t *id); + +// /******************************************************************************* +// * Module * +// *******************************************************************************/ + +// virtual int32_t module_stop() override; +// virtual int32_t module_active_cfg() override; +// virtual int32_t module_xxx_reg(int32_t param_id, bool read, int32_t &val) override; + +// /******************************************************************************* +// * pipette_ctrl * +// *******************************************************************************/ +// /** +// * 参数: +// * vstart +// * vstop +// * vmax +// * acc +// * dec +// * +// * 0.泵机复位 +// * 0.Z轴失能 +// * 1.移液枪初始化 +// * 2.移液枪探测液面初始化 +// * +// * 3.移液枪-电容探测液面-测试(moterv,dpos)，动作:Z轴向下以motor_v移动dpos,使用电容模式 ,同时采集电容数值 +// * 3.移液枪-压力探测液面-测试(moterv,dpos)，动作:Z轴向下以motor_v移动dpos,使用分配压力模式,压力阈值设置为最大, 同时采集电容数值，压力值 +// * 4.读取压力和电容数值变化数据(电机位置，电容数值，压力数值，泵机位置) +// * +// * 5.吸液(ul) +// * 6.排液(ul) +// * +// * +// * +// * +// * 使用流程 +// * +// * pipette_ctrl_put_tip +// * | +// * pipette_lld_prepare +// * | +// * pipette_lld +// * | +// * pipette_aspirate +// * pipette_distribut +// * pipette_mix(吸吐混匀) +// * +// * +// */ + +// virtual int32_t pipette_ctrl_move_to_ul(int32_t ul) override; // 丢弃 + +// virtual int32_t pipette_ctrl_init_device() override; +// virtual int32_t pipette_ctrl_put_tip() override; +// virtual int32_t pipette_lld_prepare() override; +// virtual int32_t pipette_plld(int32_t zdpos, int32_t p_threshold) override; +// virtual int32_t pipette_clld(int32_t zdpos, int32_t c_threshold) override; +// virtual int32_t pipette_mlld(int32_t zdpos, int32_t c_threshold, int32_t p_threshold) override; +// virtual int32_t pipette_lld_is_detect_liquid(int32_t *liquid) override; + +// virtual int32_t pipette_aspirate(int32_t ul) override; +// virtual int32_t pipette_distribut(int32_t ul) override; +// virtual int32_t pipette_shake_up(int32_t ul, int32_t times) override; + +// virtual int32_t pipette_aspirate_llf(int32_t ul, int32_t zmotor_v) override; +// virtual int32_t pipette_distribut_llf(int32_t ul, int32_t zmotor_v) override; +// virtual int32_t pipette_shake_up_llf(int32_t ul, int32_t zmotor_v, int32_t times) override; + +// virtual int32_t pipette_enable_zmotor(int32_t enable); +// virtual int32_t pipette_write_cmd_direct(uint8_t *tx, int32_t len, uint8_t *rx, int32_t *rxlen) override; +// virtual int32_t pipette_get_sensor_sample_data(int32_t index, int32_t *motor_pos, int32_t *cval, int32_t *pval) override; // +// virtual int32_t pipette_get_sensor_sample_data_num(int32_t *num) override; + +// private: +// int32_t do_pipette_ctrl_init_device(); +// int32_t do_pipette_ctrl_put_tip(); +// int32_t do_pipette_ctrl_move_to_ul(int32_t ul); +// int32_t do_pipette_lld_prepare(); +// int32_t do_pipette_lld(lld_mode_t mode, int32_t zdpos, int32_t c_threshold, int32_t p_threshold); + +// 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(); +// int32_t do_put_tip_before_run(); +// int32_t after_run(); +// bool check_when_run(); + +// private: +// int32_t read_pos_ul(int32_t *val); +// int32_t read_capactitance(int32_t *val); +// int32_t read_tip_state(int32_t *val); + +// private: + +// private: +// void push_snesor_sample_data(int32_t motor_pos, int32_t cap_val, int32_t pressure_val); +// }; +// } // namespace iflytop diff --git a/sdk/components/pipette_module/pipette_ctrl_module_v2.cpp b/sdk/components/pipette_module/pipette_ctrl_module_v2.cpp index 0e2eeba..976a23b 100644 --- a/sdk/components/pipette_module/pipette_ctrl_module_v2.cpp +++ b/sdk/components/pipette_module/pipette_ctrl_module_v2.cpp @@ -33,7 +33,7 @@ #define THREAD_START_WORK(...) thread_start_work(__FUNCTION__, __VA_ARGS__) -#define PIPETTE_PREPARE_POS 800 +#define PIPETTE_PREPARE_POS 400 #define LLD_PREPARE_DISTRIBUT_POS 100 #define LLF_DPOS 1000 #define ASPIRATE_POS 100 @@ -340,6 +340,7 @@ int32_t PipetteModuleV2::pipette_lld_prepare() { DO_IN_THREAD(m_smtp2.pump_move_to_ul(m_cfg.pump_vmax, m_cfg.lld_prepare_pos)); // 移动到中点 pump_waitfor_stop(); m_state.lld_prepared = 1; + m_state.load_val_ul = 0; }); return 0; } @@ -363,14 +364,17 @@ int32_t PipetteModuleV2::pipette_aspirate_prepare() { m_state.aspirate_cfg_eigen_time = 0; m_state.aspirate_cfg_p_thre = 0; m_state.aspirate_cfg_llf_zm_rpm = 0; - - int32_t nowpos = zm_get_now_pos(); - int32_t bak = nowpos - 100; + m_state.load_val_ul = 0; + int32_t nowpos = zm_get_now_pos(); + int32_t bak = nowpos - 100; if (bak < 0) bak = 0; zm_move_to(bak, m_cfg.zm_default_velocity); zm_waitfor_stop(); + DO_IN_THREAD(m_smtp2.pump_move_to_ul(m_cfg.pump_vmax, 0)); + pump_waitfor_stop(); + DO_IN_THREAD(m_smtp2.pump_move_to_ul(m_cfg.pump_vmax, ASPIRATE_POS)); pump_waitfor_stop(); @@ -413,19 +417,20 @@ int32_t PipetteModuleV2::pipette_aspirate(int32_t ul) { THREAD_START_WORK([this, ul]() { m_state.lld_prepared = 0; - DO_IN_THREAD(m_smtp2.pump_move_to_ul(m_cfg.pump_vmax, ASPIRATE_POS)); + int dul = m_state.load_val_ul + ul <= 0 ? -m_state.load_val_ul : ul; + DO_IN_THREAD(m_smtp2.pump_aspirate(m_cfg.aspirate_pump_vel, dul)); + + int32_t dpos = ul > 0 ? m_cfg.aspirate_zmotor_max_move_by : -m_cfg.aspirate_zmotor_max_move_by; + if (m_state.aspirate_cfg_llf_zm_rpm != 0) zm_move_by(dpos, m_state.aspirate_cfg_llf_zm_rpm); + pump_waitfor_stop(); - if (ul > 0) { - // zmotor and pump move - if (m_state.aspirate_cfg_llf_zm_rpm != 0) zm_move_by(m_cfg.aspirate_zmotor_max_move_by, m_state.aspirate_cfg_llf_zm_rpm); - DO_IN_THREAD(m_smtp2.pump_aspirate(m_cfg.aspirate_pump_vel, ul)); - } else { - if (m_state.aspirate_cfg_llf_zm_rpm != 0) zm_move_by(-m_cfg.aspirate_zmotor_max_move_by, m_state.aspirate_cfg_llf_zm_rpm); - DO_IN_THREAD(m_smtp2.pump_distribut(m_cfg.aspirate_pump_vel, ul)); + // 全部排空时，多分配10ul去掉挂液 + if ((m_state.load_val_ul - dul) == 0) { + m_smtp2.pump_aspirate(m_cfg.aspirate_pump_vel, -10); + pump_waitfor_stop(); } - // wait... - pump_waitfor_stop(); + // stop m_zm->stop(); m_smtp2.pump_stop(); @@ -445,11 +450,15 @@ int32_t PipetteModuleV2::pipette_aspirate_and_verify(int32_t ul) { m_state.lld_prepared = 0; // zmotor and pump move - if (m_state.aspirate_cfg_llf_zm_rpm != 0) zm_move_by(m_cfg.aspirate_zmotor_max_move_by, m_state.aspirate_cfg_llf_zm_rpm); - DO_IN_THREAD(m_smtp2.pump_aspirate_and_verify(m_cfg.aspirate_pump_vel, ul, // + + int dul = m_state.load_val_ul + ul <= 0 ? -m_state.load_val_ul : ul; + DO_IN_THREAD(m_smtp2.pump_aspirate_and_verify(m_cfg.aspirate_pump_vel, dul, // m_state.aspirate_cfg_p_thre, // m_state.aspirate_cfg_eigen_time, // m_state.aspirate_cfg_tolerance)); + + int32_t dpos = ul > 0 ? m_cfg.aspirate_zmotor_max_move_by : -m_cfg.aspirate_zmotor_max_move_by; + if (m_state.aspirate_cfg_llf_zm_rpm != 0) zm_move_by(dpos, m_state.aspirate_cfg_llf_zm_rpm); // wait... pump_waitfor_stop(); // stop @@ -463,6 +472,16 @@ int32_t PipetteModuleV2::pipette_aspirate_and_verify(int32_t ul) { } int32_t PipetteModuleV2::pipette_shake_up(int32_t ul, int32_t times) { + if (m_state.load_val_ul != 0) { + ZLOGE(TAG, "pipette_shake_up fail, load_val_ul %d != 0", m_state.load_val_ul); + return err::kpipette_error_pump_load_val_is_not_empty; + } + + if (m_state.load_val_ul + ul > m_cfg.limit_ul) { + ZLOGE(TAG, "pipette_aspirate %d fail , final ul (%d + %d)> limit ul %d", ul, m_state.load_val_ul, ul, m_cfg.limit_ul); + return err::kparam_out_of_range; + } + THREAD_START_WORK([this, ul, times]() { // int32_t dpos = 0; @@ -481,7 +500,7 @@ int32_t PipetteModuleV2::pipette_shake_up(int32_t ul, int32_t times) { for (int i = 0; i < times - 1; i++) { // distribute if (m_state.aspirate_cfg_llf_zm_rpm) zm_move_to(startpos, m_state.aspirate_cfg_llf_zm_rpm); - DO_IN_THREAD(m_smtp2.pump_distribut(m_cfg.aspirate_pump_vel, ul)); + DO_IN_THREAD(m_smtp2.pump_aspirate(m_cfg.aspirate_pump_vel, -ul)); pump_waitfor_stop(); zm_waitfor_stop(); @@ -494,7 +513,7 @@ int32_t PipetteModuleV2::pipette_shake_up(int32_t ul, int32_t times) { // final distribute if (m_state.aspirate_cfg_llf_zm_rpm) zm_move_to(startpos, m_state.aspirate_cfg_llf_zm_rpm); - DO_IN_THREAD(m_smtp2.pump_distribut(m_cfg.aspirate_pump_vel, ul)); + DO_IN_THREAD(m_smtp2.pump_aspirate(m_cfg.aspirate_pump_vel, -ul)); pump_waitfor_stop(); zm_waitfor_stop(); }); @@ -621,7 +640,7 @@ int32_t PipetteModuleV2::do_pipette_lld(bool recorddata, int32_t lldtype /*kclld // 先分配一点体积，消除一些误差。 ZLOGI(TAG, "start lld before distribut %d ul", m_cfg.lld_prepare_pre_distribut_ul); - DO_IN_THREAD(m_smtp2.pump_distribut(m_cfg.pump_vmax, m_cfg.lld_prepare_pre_distribut_ul)); + DO_IN_THREAD(m_smtp2.pump_aspirate(m_cfg.pump_vmax, -m_cfg.lld_prepare_pre_distribut_ul)); pump_waitfor_stop(); ZLOGI(TAG, "lld before distribut ok"); @@ -949,8 +968,8 @@ void PipetteModuleV2::thread_start_work(const char *fnname, function fn) m_thread.start([this, fnname, fn]() { osDelay(1); ZLOGI(TAG, "start work -> %s", fnname); - int32_t pump_before_pos_ul; - int32_t pump_after_pos_ul; + // int32_t pump_before_pos_ul; + // int32_t pump_after_pos_ul; // clear some state m_state.dul = 0; @@ -959,7 +978,7 @@ void PipetteModuleV2::thread_start_work(const char *fnname, function fn) try { // before do int32_t startpos = zm_get_now_pos(); - DO_IN_THREAD(m_smtp2.pump_get_motor_pos_ul(&pump_before_pos_ul)); + // DO_IN_THREAD(m_smtp2.pump_get_motor_pos_ul(&pump_before_pos_ul)); // do fn if (fn) fn(); @@ -968,10 +987,10 @@ void PipetteModuleV2::thread_start_work(const char *fnname, function fn) m_smtp2.pump_stop(); // after do - DO_IN_THREAD(m_smtp2.pump_get_motor_pos_ul(&pump_after_pos_ul)); + // DO_IN_THREAD(m_smtp2.pump_get_motor_pos_ul(&pump_after_pos_ul)); int32_t endpos = zm_get_now_pos(); - m_state.dul = pump_after_pos_ul - pump_before_pos_ul; - m_state.dpos = endpos - startpos; + // m_state.dul = pump_after_pos_ul - pump_before_pos_ul; + m_state.dpos = endpos - startpos; } catch (const zapp_thread_stoped_exception &e) { // break by usr ZLOGI(TAG, "break by usr"); diff --git a/sdk/components/sensors/smtp2_v2/smtp2_v2.cpp b/sdk/components/sensors/smtp2_v2/smtp2_v2.cpp index ed36f65..ea67c23 100644 --- a/sdk/components/sensors/smtp2_v2/smtp2_v2.cpp +++ b/sdk/components/sensors/smtp2_v2/smtp2_v2.cpp @@ -48,8 +48,6 @@ int32_t SMTP2V2::pump_get_state_as_int(int32_t state_index, int32_t* val) { return err::ksucc; } - - int32_t SMTP2V2::pump_get_capacitance(int32_t* capacitance) { return pump_get_state_as_int(kstate_capacitance, capacitance); } int32_t SMTP2V2::pump_get_pressure(int32_t* pressure) { // return pump_get_state_as_int(kstate_pressure_ad, pressure); @@ -121,6 +119,18 @@ int32_t SMTP2V2::pump_aspirate(int32_t v, int32_t ul) { if (targetnl < 0) targetnl = 0; return pump_move_to_nl(v, targetnl); } +// int32_t SMTP2V2::pump_distribut(int32_t v, int32_t ul) { +// ZLOGI(TAG, "pump_distribut %d", ul); + +// int32_t nownl = 0; + +// int32_t ret = pump_get_state_as_int(kstate_pump_pos_nl, &nownl); +// if (ret != 0) return ret; + +// int targetnl = nownl - ul * 1000; +// if (targetnl < 0) targetnl = 0; +// return pump_move_to_nl(v, targetnl); +// } int32_t SMTP2V2::pump_aspirate_and_verify(int32_t v, int32_t ul, int32_t eigen_time, int32_t p_thre, int32_t tolerance) { /** @@ -223,19 +233,6 @@ int32_t SMTP2V2::pump_aspirate_and_verf(int32_t v, int32_t ul, int32_t p_thro, i } #endif -int32_t SMTP2V2::pump_distribut(int32_t v, int32_t ul) { - ZLOGI(TAG, "pump_distribut %d", ul); - - int32_t nownl = 0; - - int32_t ret = pump_get_state_as_int(kstate_pump_pos_nl, &nownl); - if (ret != 0) return ret; - - int targetnl = nownl - ul * 1000; - if (targetnl < 0) targetnl = 0; - return pump_move_to_nl(v, targetnl); -} - int32_t SMTP2V2::pump_distribut_plld(int32_t pumpv, int32_t pressure_threshold) { // M100V100t50,0R ZLOGI(TAG, "pump_distribut_plld %d %d", pumpv, pressure_threshold); diff --git a/sdk/components/sensors/smtp2_v2/smtp2_v2.hpp b/sdk/components/sensors/smtp2_v2/smtp2_v2.hpp index 70bd733..9455f57 100644 --- a/sdk/components/sensors/smtp2_v2/smtp2_v2.hpp +++ b/sdk/components/sensors/smtp2_v2/smtp2_v2.hpp @@ -199,7 +199,7 @@ class SMTP2V2 { int32_t pump_aspirate(int32_t v, int32_t ul); int32_t pump_aspirate_and_verify(int32_t v, int32_t ul, int32_t eigen_time, int32_t p_thre, int32_t tolerance); - int32_t pump_distribut(int32_t v, int32_t ul); + // int32_t pump_distribut(int32_t v, int32_t ul); int32_t pump_aspirate_infer_pressure(int32_t v, int32_t ul); int32_t pump_get_infer_pressure(int32_t* p_thre);