From 7a482bcf4a0869462fc4e8872dda90d877b810a1 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Wed, 18 Sep 2024 23:40:11 +0800 Subject: [PATCH] update --- .settings/language.settings.xml | 4 +- a8000_protocol | 2 +- .../pipette_module/pipette_ctrl_module_v2.cpp | 320 ++++++++++++++------- .../pipette_module/pipette_ctrl_module_v2.hpp | 62 ++-- sdk/components/zcancmder/zcan_protocol_parser.cpp | 6 +- sdk/components/zcancmder/zcan_protocol_parser.hpp | 2 + .../subboard80_cliptip/subboard80_cliptip.cpp | 55 ++-- 7 files changed, 286 insertions(+), 165 deletions(-) diff --git a/.settings/language.settings.xml b/.settings/language.settings.xml index f889596..810d72c 100644 --- a/.settings/language.settings.xml +++ b/.settings/language.settings.xml @@ -5,7 +5,7 @@ - + @@ -16,7 +16,7 @@ - + diff --git a/a8000_protocol b/a8000_protocol index c025f34..48fe715 160000 --- a/a8000_protocol +++ b/a8000_protocol @@ -1 +1 @@ -Subproject commit c025f34164cd3334f78d8fbcbd42b54e17987153 +Subproject commit 48fe71561ae851deea3de371ef04276fa8d777f9 diff --git a/sdk/components/pipette_module/pipette_ctrl_module_v2.cpp b/sdk/components/pipette_module/pipette_ctrl_module_v2.cpp index 8b012aa..0e25952 100644 --- a/sdk/components/pipette_module/pipette_ctrl_module_v2.cpp +++ b/sdk/components/pipette_module/pipette_ctrl_module_v2.cpp @@ -9,6 +9,9 @@ #define TAG "PipetteModuleV2" +/*********************************************************************************************************************** + * MARCO * + ***********************************************************************************************************************/ #define ZSLICE(...) ZSlice slice(this, TAG, __FUNCTION__, __VA_ARGS__) #define DO_IN_THREAD(func) \ { \ @@ -28,12 +31,18 @@ } \ } -using namespace iflytop; -// using namespace pipette_module_v3; - #define PIPETTE_PREPARE_POS 800 #define LLD_PREPARE_DISTRIBUT_POS 100 #define LLF_DPOS 1000 +#define ASPIRATE_POS 0 + +using namespace iflytop; +// using namespace pipette_module_v3; + +static int32_t assign_int32(int32_t *toval, int32_t val) { + *toval = val; + return 0; +} void PipetteModuleV2::initialize(int32_t id, config_t *config, hardward_config_t *hardwaredcfg) { // ZASSERT(config != nullptr); @@ -67,15 +76,14 @@ void PipetteModuleV2::create_default_cfg(config_t *defaultcfg) { cfg.lld_prepare_pos = PIPETTE_PREPARE_POS; cfg.lld_prepare_pre_distribut_ul = LLD_PREPARE_DISTRIBUT_POS; - cfg.motor_one_circle_pulse = 10000; - cfg.motor_one_circle_pulse_denominator = 1; - cfg.motor_ihold = 1; - cfg.motor_irun = 3; - cfg.motor_iholddelay = 100; - cfg.motor_default_velocity = 500; - cfg.motor_run_to_zero_speed = 100; - cfg.motor_look_zero_edge_speed = 100; - + cfg.zm_one_circle_pulse = 10000; + cfg.zm_one_circle_pulse_denominator = 1; + cfg.zm_ihold = 1; + cfg.zm_irun = 3; + cfg.zm_iholddelay = 100; + cfg.zm_default_velocity = 500; + cfg.zm_run_to_zero_speed = 100; + cfg.zm_look_zero_edge_speed = 100; } int32_t PipetteModuleV2::getid(int32_t *id) { *id = m_id; @@ -91,21 +99,21 @@ int32_t PipetteModuleV2::module_active_cfg() { DO(m_smtp2.pump_set_vstop(m_cfg.pump_vstop)); m_zm->enable(false); - m_zm->setIHOLD_IRUN(m_cfg.motor_ihold, m_cfg.motor_irun, m_cfg.motor_iholddelay); - m_zm->setScale(m_cfg.motor_one_circle_pulse); - m_zm->setScaleDenominator(m_cfg.motor_one_circle_pulse_denominator); - m_zm->setMotorShaft(m_cfg.motor_shaft); - m_zm->setGlobalScale(m_cfg.motor_iglobalscaler); - - m_zm->set_vstart(m_cfg.motor_vstart); - m_zm->set_a1(m_cfg.motor_a1); - m_zm->set_amax(m_cfg.motor_amax); - m_zm->set_v1(m_cfg.motor_v1); - m_zm->set_dmax(m_cfg.motor_dmax); - m_zm->set_d1(m_cfg.motor_d1); - m_zm->set_vstop(m_cfg.motor_vstop); - m_zm->set_tzerowait(m_cfg.motor_tzerowait); - m_zm->set_enc_resolution(m_cfg.motor_enc_resolution); + m_zm->setIHOLD_IRUN(m_cfg.zm_ihold, m_cfg.zm_irun, m_cfg.zm_iholddelay); + m_zm->setScale(m_cfg.zm_one_circle_pulse); + m_zm->setScaleDenominator(m_cfg.zm_one_circle_pulse_denominator); + m_zm->setMotorShaft(m_cfg.zm_shaft); + m_zm->setGlobalScale(m_cfg.zm_iglobalscaler); + + m_zm->set_vstart(m_cfg.zm_vstart); + m_zm->set_a1(m_cfg.zm_a1); + m_zm->set_amax(m_cfg.zm_amax); + m_zm->set_v1(m_cfg.zm_v1); + m_zm->set_dmax(m_cfg.zm_dmax); + m_zm->set_d1(m_cfg.zm_d1); + m_zm->set_vstop(m_cfg.zm_vstop); + m_zm->set_tzerowait(m_cfg.zm_tzerowait); + m_zm->set_enc_resolution(m_cfg.zm_enc_resolution); if (m_state.enable) { m_zm->enable(true); @@ -114,8 +122,8 @@ int32_t PipetteModuleV2::module_active_cfg() { return 0; } -#define XXX_CFG_REG(cfgkey, cfg) \ - case cfgkey: { \ +#define XXX_CFG_REG(cfg) \ + case kreg_pipette_##cfg: { \ if (read) { \ return REG_GET(m_cfg.cfg); \ } else { \ @@ -127,52 +135,58 @@ int32_t PipetteModuleV2::module_active_cfg() { #define XXX_STATE_REG(statekey, action) PROCESS_REG(statekey, action, ACTION_NONE) int32_t PipetteModuleV2::_module_xxx_reg(int32_t param_id, bool read, int32_t &val) { switch (param_id) { - // XXX_STATE_REG(kreg_pipette_pos_ul, /* */ read_pos_ul(&val)); - // XXX_STATE_REG(kreg_pipette_capactitance_val, /* */ read_capactitance(&val)); - // XXX_STATE_REG(kreg_pipette_tip_state, /* */ read_tip_state(&val)); - // XXX_STATE_REG(kreg_step_motor_pos, getnowpos(val), ACTION_NONE); - // XXX_STATE_REG(kreg_step_motor_is_enable, REG_GET(m_state.enable), ACTION_NONE); - // XXX_STATE_REG(kreg_step_motor_has_move_zero, REG_GET(m_state.has_move_to_zero), ACTION_NONE); - // XXX_CFG_REG(kreg_step_motor_dpos, REG_GET(m_state.dpos), ACTION_NONE); - - XXX_CFG_REG(kreg_pipette_limit_ul, limit_ul); - XXX_CFG_REG(kreg_pipette_pump_acc, pump_acc); - XXX_CFG_REG(kreg_pipette_pump_dec, pump_dec); - XXX_CFG_REG(kreg_pipette_pump_vstart, pump_vstart); - XXX_CFG_REG(kreg_pipette_pump_vstop, pump_vstop); - XXX_CFG_REG(kreg_pipette_pump_vmax, pump_vmax); - XXX_CFG_REG(kreg_pipette_aspirate_pump_vel, aspirate_pump_vel); - XXX_CFG_REG(kreg_pipette_lld_pump_vel, lld_pump_vel); - XXX_CFG_REG(kreg_pipette_lld_motor_vel_rpm, lld_motor_vel_rpm); - XXX_CFG_REG(kreg_pipette_lld_detect_period_ms, lld_detect_period_ms); - XXX_CFG_REG(kreg_pipette_lld_prepare_pos, lld_prepare_pos); - XXX_CFG_REG(kreg_pipette_lld_prepare_pre_distribut_ul, lld_prepare_pre_distribut_ul); - - XXX_CFG_REG(kreg_step_motor_shaft, motor_shaft); - XXX_CFG_REG(kreg_step_motor_one_circle_pulse, motor_one_circle_pulse); - XXX_CFG_REG(kreg_step_motor_one_circle_pulse_denominator, motor_one_circle_pulse_denominator); - XXX_CFG_REG(kreg_step_motor_default_velocity, motor_default_velocity); - XXX_CFG_REG(kreg_step_motor_run_to_zero_speed, motor_run_to_zero_speed); - XXX_CFG_REG(kreg_step_motor_look_zero_edge_speed, motor_look_zero_edge_speed); - XXX_CFG_REG(kreg_step_motor_ihold, motor_ihold); - XXX_CFG_REG(kreg_step_motor_irun, motor_irun); - XXX_CFG_REG(kreg_step_motor_iholddelay, motor_iholddelay); - XXX_CFG_REG(kreg_step_motor_max_d, motor_max_d); - XXX_CFG_REG(kreg_step_motor_min_d, motor_min_d); - XXX_CFG_REG(kreg_step_motor_iglobalscaler, motor_iglobalscaler); - - XXX_CFG_REG(kreg_step_motor_vstart, motor_vstart); - XXX_CFG_REG(kreg_step_motor_a1, motor_a1); - XXX_CFG_REG(kreg_step_motor_amax, motor_amax); - XXX_CFG_REG(kreg_step_motor_v1, motor_v1); - XXX_CFG_REG(kreg_step_motor_dmax, motor_dmax); - XXX_CFG_REG(kreg_step_motor_d1, motor_d1); - XXX_CFG_REG(kreg_step_motor_vstop, motor_vstop); - XXX_CFG_REG(kreg_step_motor_tzerowait, motor_tzerowait); - XXX_CFG_REG(kreg_step_motor_enc_resolution, motor_enc_resolution); - XXX_CFG_REG(kreg_step_motor_enable_enc, motor_enable_enc); - XXX_CFG_REG(kreg_step_motor_dzero_pos, motor_dzero); - XXX_CFG_REG(kreg_step_motor_pos_devi_tolerance, pos_devi_tolerance); + /*********************************************************************************************************************** + * STATE * + ***********************************************************************************************************************/ + XXX_STATE_REG(kreg_pipette_pos_ul, /* */ pump_read_pos_ul(&val)); + XXX_STATE_REG(kreg_pipette_capactitance_val, /* */ pump_read_capactitance(&val)); + XXX_STATE_REG(kreg_pipette_tip_state, /* */ pump_read_tip_state(&val)); + XXX_STATE_REG(kreg_pipette_zm_pos, assign_int32(&val, zm_get_now_pos())); + XXX_STATE_REG(kreg_pipette_zm_is_enable, REG_GET(m_state.enable)); + XXX_STATE_REG(kreg_pipette_zm_dpos, REG_GET(m_state.dpos)); + + /*********************************************************************************************************************** + * CFG * + ***********************************************************************************************************************/ + XXX_CFG_REG(limit_ul); + XXX_CFG_REG(pump_acc); + XXX_CFG_REG(pump_dec); + XXX_CFG_REG(pump_vstart); + XXX_CFG_REG(pump_vstop); + XXX_CFG_REG(pump_vmax); + XXX_CFG_REG(aspirate_pump_vel); + XXX_CFG_REG(lld_pump_vel); + XXX_CFG_REG(lld_motor_vel_rpm); + XXX_CFG_REG(lld_detect_period_ms); + XXX_CFG_REG(lld_prepare_pos); + XXX_CFG_REG(lld_prepare_pre_distribut_ul); + + XXX_CFG_REG(zm_shaft); + XXX_CFG_REG(zm_one_circle_pulse); + XXX_CFG_REG(zm_one_circle_pulse_denominator); + XXX_CFG_REG(zm_default_velocity); + XXX_CFG_REG(zm_run_to_zero_speed); + XXX_CFG_REG(zm_look_zero_edge_speed); + XXX_CFG_REG(zm_ihold); + XXX_CFG_REG(zm_irun); + XXX_CFG_REG(zm_iholddelay); + XXX_CFG_REG(zm_max_d); + XXX_CFG_REG(zm_min_d); + XXX_CFG_REG(zm_iglobalscaler); + + XXX_CFG_REG(zm_vstart); + XXX_CFG_REG(zm_a1); + XXX_CFG_REG(zm_amax); + XXX_CFG_REG(zm_v1); + XXX_CFG_REG(zm_dmax); + XXX_CFG_REG(zm_d1); + XXX_CFG_REG(zm_vstop); + XXX_CFG_REG(zm_tzerowait); + XXX_CFG_REG(zm_enc_resolution); + XXX_CFG_REG(zm_enable_enc); + XXX_CFG_REG(zm_dzero); + XXX_CFG_REG(zm_pos_devi_tolerance); + XXX_CFG_REG(zm_io_trigger_append_distance); default: return err::kmodule_not_find_reg; @@ -194,6 +208,17 @@ int32_t PipetteModuleV2::module_xxx_reg(int32_t param_id, bool read, int32_t &va * EXT_API * ***********************************************************************************************************************/ +int32_t PipetteModuleV2::pipette_zmotor_enable(int32_t enable) { + ZLOGI(TAG, "m%d pipette_zmotor_enable %ld", m_id, enable); + m_thread.stop(); + m_zm->enable(enable); + m_state.enable = enable; + if (enable == 0 && m_cfg.zm_enable_enc == 0) { + m_state.has_move_to_zero = 0; + } + return 0; +} + /** * @brief 移动到零点 * @@ -203,7 +228,8 @@ int32_t PipetteModuleV2::pipette_zmotor_move_zero() { // thread_start_work([this]() { do_zm_move_0p(); // 移动到零点 if (creg.module_errorcode != 0) { // 计算并更新零点坐标 - zm_set_now_pos(0 + m_cfg.motor_dzero - m_cfg.io_trigger_append_distance); + zm_set_now_pos(0 + m_cfg.zm_dzero - m_cfg.zm_io_trigger_append_distance); + m_state.has_move_to_zero = 1; } }); return 0; @@ -220,9 +246,10 @@ int32_t PipetteModuleV2::pipette_zmotor_measure_distance() { do_zm_move_0p(); // 移动到零点 int32_t dpos = zm_get_now_pos() - startpos; if (creg.module_errorcode != 0) { // 计算并更新零点坐标 - zm_set_now_pos(0 + m_cfg.motor_dzero - m_cfg.io_trigger_append_distance); + zm_set_now_pos(0 + m_cfg.zm_dzero - m_cfg.zm_io_trigger_append_distance); + m_state.has_move_to_zero = 1; } - m_asynchronous_result.result0 = 0 + m_cfg.motor_dzero - m_cfg.io_trigger_append_distance - dpos; + m_asynchronous_result.result0 = 0 + m_cfg.zm_dzero - m_cfg.zm_io_trigger_append_distance - dpos; }); return 0; } @@ -237,7 +264,6 @@ int32_t PipetteModuleV2::pipette_zmotor_read_measure_distance_result(int32_t *re *result0 = m_asynchronous_result.result0; return 0; } - /** * @brief 相对位移 * @@ -246,7 +272,7 @@ int32_t PipetteModuleV2::pipette_zmotor_read_measure_distance_result(int32_t *re */ int32_t PipetteModuleV2::pipette_zmotor_move_by(int32_t distance) { thread_start_work([this, distance]() { - zm_move_by(distance, m_cfg.motor_default_velocity); + zm_move_by(distance, m_cfg.zm_default_velocity); zm_waitfor_stop(); }); return 0; @@ -257,11 +283,16 @@ int32_t PipetteModuleV2::pipette_zmotor_move_by(int32_t distance) { * @param position * @return int32_t */ -int32_t PipetteModuleV2::pipette_zmotor_move_to(int32_t position) { - thread_start_work([this, position]() { - zm_move_to(position, m_cfg.motor_default_velocity); +int32_t PipetteModuleV2::pipette_zmotor_move_to(int32_t tox) { + if (m_state.has_move_to_zero == 0) return err::kstep_motor_not_move_to_zero; + if (m_cfg.zm_min_d != 0 && tox < m_cfg.zm_min_d) tox = m_cfg.zm_min_d; + if (m_cfg.zm_max_d != 0 && tox > m_cfg.zm_max_d) tox = m_cfg.zm_max_d; + + thread_start_work([this, tox]() { + zm_move_to(tox, m_cfg.zm_default_velocity); zm_waitfor_stop(); }); + return 0; } /** @@ -270,6 +301,7 @@ int32_t PipetteModuleV2::pipette_zmotor_move_to(int32_t position) { * @return int32_t */ int32_t PipetteModuleV2::pipette_zmotor_move_to_zero_point_quick() { + if (m_state.has_move_to_zero == 0) return err::kstep_motor_not_move_to_zero; thread_start_work([this]() { do_pipette_zmotor_move_to_zero_point_quick(); }); return 0; } @@ -280,7 +312,6 @@ int32_t PipetteModuleV2::pipette_init_device() { pump_waitfor_stop(); // m_state.load_val_ul = 0; - m_state.aspirated = 0; m_state.lld_prepared = 0; m_state.pipette_is_inited = 1; }); @@ -293,6 +324,8 @@ int32_t PipetteModuleV2::pipette_put_tip() { } int32_t PipetteModuleV2::pipette_lld_prepare() { + if (m_state.pipette_is_inited == 0) return err::kpipette_error_uninited; + thread_start_work([this]() { DO_IN_THREAD(m_smtp2.pump_move_to_ul(m_cfg.pump_vmax, m_cfg.lld_prepare_pos)); // 移动到中点 pump_waitfor_stop(); @@ -301,6 +334,14 @@ int32_t PipetteModuleV2::pipette_lld_prepare() { return 0; } int32_t PipetteModuleV2::pipette_lld(int32_t lldtype, int32_t zdpos, int32_t c_threshold, int32_t p_threshold) { + // 检查泵机是否初始化过 + if (m_state.pipette_is_inited == 0) return err::kpipette_error_uninited; + + // 检查tip头状态 + int32_t tipstate = 0; + DO(m_smtp2.pump_get_tip_state(&tipstate)); + if (tipstate != 0) return err::kpipette_error_tipisload_when_lld_prepare; + thread_start_work([this, lldtype, zdpos, c_threshold, p_threshold]() { // m_state.lld_prepared = 0; // 失效m_state.lld_prepared @@ -391,12 +432,14 @@ int32_t PipetteModuleV2::pipette_lld(int32_t lldtype, int32_t zdpos, int32_t c_t } int32_t PipetteModuleV2::pipette_aspirate_prepare() { + if (m_state.pipette_is_inited == 0) return err::kpipette_error_uninited; + thread_start_work([this]() { // ZLOGI(TAG, "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; - DO_IN_THREAD(m_smtp2.pump_move_to_ul(m_cfg.pump_vmax, 0)); + DO_IN_THREAD(m_smtp2.pump_move_to_ul(m_cfg.pump_vmax, ASPIRATE_POS)); pump_waitfor_stop(); ZLOGI(TAG, "pipette_aspirate_prepare end..."); }); @@ -426,10 +469,14 @@ int32_t PipetteModuleV2::pipette_aspirate_set_operation_verifi_tolerance(int32_t } int32_t PipetteModuleV2::pipette_aspirate(int32_t ul) { + if (m_state.pipette_is_inited == 0) return err::kpipette_error_uninited; + thread_start_work([this, ul]() { - m_state.aspirated = 1; m_state.lld_prepared = 0; + DO_IN_THREAD(m_smtp2.pump_move_to_ul(m_cfg.pump_vmax, ASPIRATE_POS)); + 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); @@ -450,8 +497,9 @@ int32_t PipetteModuleV2::pipette_aspirate(int32_t ul) { return 0; } int32_t PipetteModuleV2::pipette_aspirate_and_verify(int32_t ul) { + if (m_state.pipette_is_inited == 0) return err::kpipette_error_uninited; + thread_start_work([this, ul]() { - m_state.aspirated = 1; m_state.lld_prepared = 0; // zmotor and pump move @@ -473,6 +521,8 @@ 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.pipette_is_inited == 0) return err::kpipette_error_uninited; + thread_start_work([this, ul, times]() { // int32_t dpos = 0; @@ -512,6 +562,8 @@ int32_t PipetteModuleV2::pipette_shake_up(int32_t ul, int32_t times) { } int32_t PipetteModuleV2::pipette_aspirate_infer_pressure(int32_t ul) { + if (m_state.pipette_is_inited == 0) return err::kpipette_error_uninited; + thread_start_work([this, ul]() { // ZLOGI(TAG, "pipette_aspirate_infer_pressure %d", ul); @@ -528,6 +580,8 @@ int32_t PipetteModuleV2::pipette_get_aspirate_infer_pressure_result(int32_t *res return 0; } int32_t PipetteModuleV2::pipette_aspirate_infer_eigen_time(int32_t ul) { + if (m_state.pipette_is_inited == 0) return err::kpipette_error_uninited; + // thread_start_work([this, ul]() { // @@ -560,39 +614,39 @@ int32_t PipetteModuleV2::pipette_get_aspirate_infer_eigen_time_result(int32_t *r ***********************************************************************************************************************/ void PipetteModuleV2::do_zm_move_0p() { if (!zm0p_is_trigger()) { - zm_move_to_end(-1, m_cfg.motor_run_to_zero_speed); + zm_move_to_end(-1, m_cfg.zm_run_to_zero_speed); zm_waitfor_zm0p_trigger_and_stop_motor(); } // leave from zero if (zm0p_is_trigger()) { - zm_move_to_end(1, m_cfg.motor_look_zero_edge_speed); + zm_move_to_end(1, m_cfg.zm_look_zero_edge_speed); zm_waitfor_zm0p_not_trigger_and_stop_motor(); } // move to zero again if (!zm0p_is_trigger()) { - zm_move_to_end(-1, m_cfg.motor_run_to_zero_speed); + zm_move_to_end(-1, m_cfg.zm_run_to_zero_speed); zm_waitfor_zm0p_trigger_and_stop_motor(); } // move to zero - zm_move_by(m_cfg.io_trigger_append_distance, m_cfg.motor_run_to_zero_speed); + zm_move_by(m_cfg.zm_io_trigger_append_distance, m_cfg.zm_run_to_zero_speed); zm_waitfor_stop(); } void PipetteModuleV2::do_pipette_zmotor_move_to_zero_point_quick() { // 期望偏差 - int32_t expectation_dpos = -m_cfg.io_trigger_append_distance + 0 + m_cfg.motor_dzero - zm_get_now_pos(); + int32_t expectation_dpos = -m_cfg.zm_io_trigger_append_distance + 0 + m_cfg.zm_dzero - zm_get_now_pos(); int32_t startpos = zm_get_now_pos(); if (!zm0p_is_trigger()) { // 快速移动到零点 - // moveTo(0 + m_cfg.motor_dzero, m_cfg.motor_default_velocity); - zm_move_to(0 + m_cfg.motor_dzero, m_cfg.motor_default_velocity); + // moveTo(0 + m_cfg.zm_dzero, m_cfg.zm_default_velocity); + zm_move_to(0 + m_cfg.zm_dzero, m_cfg.zm_default_velocity); zm_waitfor_zm0p_trigger_and_stop_motor(); } else { // 快速移动到零点 - zm_move_to(0 + m_cfg.motor_dzero, m_cfg.motor_default_velocity); + zm_move_to(0 + m_cfg.zm_dzero, m_cfg.zm_default_velocity); zm_waitfor_zm0p_not_trigger_and_stop_motor(); } // 移动到零点 @@ -600,7 +654,7 @@ void PipetteModuleV2::do_pipette_zmotor_move_to_zero_point_quick() { // 校验偏差 int32_t dpos = zm_get_now_pos() - startpos; - if (m_cfg.pos_devi_tolerance != 0 && (abs(expectation_dpos - dpos) > m_cfg.pos_devi_tolerance)) { + if (m_cfg.zm_pos_devi_tolerance != 0 && (abs(expectation_dpos - dpos) > m_cfg.zm_pos_devi_tolerance)) { throw zapp_exception(err::kstep_motor_lost_step); } } @@ -620,7 +674,7 @@ int32_t PipetteModuleV2::pipette_zmotor_read_dev_status_cache(int32_t *devStatus * ZM_UTILS * ***********************************************************************************************************************/ int32_t PipetteModuleV2::zm_get_now_pos() { - if (m_cfg.motor_enable_enc == 0) { + if (m_cfg.zm_enable_enc == 0) { return m_zm->getXACTUAL(); } return m_zm->read_enc_val(); @@ -630,29 +684,29 @@ void PipetteModuleV2::zm_set_now_pos(int32_t x) { m_zm->set_enc_val(x); } void PipetteModuleV2::zm_move_to(int32_t x, int32_t v) { - if (m_cfg.motor_enable_enc != 0) { + if (m_cfg.zm_enable_enc != 0) { m_zm->setXACTUAL(m_zm->read_enc_val()); } m_zm->moveTo(x, v); } void PipetteModuleV2::zm_move_by(int32_t dx, int32_t v) { - if (m_cfg.motor_enable_enc != 0) { + if (m_cfg.zm_enable_enc != 0) { m_zm->setXACTUAL(m_zm->read_enc_val()); } m_zm->moveBy(dx, v); } void PipetteModuleV2::zm_move_to_end(int32_t direction, int32_t v) { - if (m_cfg.motor_enable_enc != 0) { + if (m_cfg.zm_enable_enc != 0) { m_zm->setXACTUAL(m_zm->read_enc_val()); } m_zm->moveToEnd(direction, v); } void PipetteModuleV2::zm_update_dzero(int32_t dzero) { - int32_t nowabs = zm_get_now_pos() - m_cfg.motor_dzero; - m_cfg.motor_dzero = dzero; - zm_set_now_pos(nowabs + m_cfg.motor_dzero); + int32_t nowabs = zm_get_now_pos() - m_cfg.zm_dzero; + m_cfg.zm_dzero = dzero; + zm_set_now_pos(nowabs + m_cfg.zm_dzero); } bool PipetteModuleV2::zm0p_is_trigger() { return m_zm0p->getState(); } @@ -674,6 +728,21 @@ void PipetteModuleV2::pump_waitfor_stop() { } } +int32_t PipetteModuleV2::pump_read_pos_ul(int32_t *val) { + // + *val = m_state.load_val_ul; + return 0; +} +int32_t PipetteModuleV2::pump_read_capactitance(int32_t *val) { + m_smtp2.pump_get_capacitance(val); + return 0; +} +int32_t PipetteModuleV2::pump_read_tip_state(int32_t *val) { + *val = 0; + m_smtp2.pump_get_tip_state(val); + return 0; +} + void PipetteModuleV2::zm_waitfor_zm0p_not_trigger_and_stop_motor() { // 等待触发 while (zm0p_is_trigger()) thread_delay(2); @@ -708,6 +777,45 @@ void PipetteModuleV2::push_snesor_sample_data(int32_t motor_pos, int32_t cap_val */ int32_t PipetteModuleV2::bfcall(int32_t cmdid, uint8_t *param, int32_t len) { ZLOGI(TAG, "bfcall %s", cmdid2str(cmdid)); + + if (cmdid == kpipette_zmotor_move_zero) goto check; + if (cmdid == kpipette_zmotor_move_to_zero_point_quick) goto check; + if (cmdid == kpipette_zmotor_measure_distance) goto check; + if (cmdid == kpipette_zmotor_read_measure_distance_result) goto check; + if (cmdid == kpipette_zmotor_move_by) goto check; + if (cmdid == kpipette_zmotor_move_to) goto check; + if (cmdid == kpipette_init_device) goto check; + if (cmdid == kpipette_put_tip) goto check; + if (cmdid == kpipette_lld_prepare) goto check; + if (cmdid == kpipette_lld) goto check; + if (cmdid == kpipette_aspirate_prepare) goto check; + if (cmdid == kpipette_aspirate_set_llf_velocity) goto check; + if (cmdid == kpipette_aspirate_set_operation_verifi_p_thre) goto check; + if (cmdid == kpipette_aspirate_set_operation_verifi_eigen_time) goto check; + if (cmdid == kpipette_aspirate_set_operation_verifi_tolerance) goto check; + if (cmdid == kpipette_aspirate) goto check; + if (cmdid == kpipette_aspirate_and_verify) goto check; + if (cmdid == kpipette_shake_up) goto check; + if (cmdid == kpipette_aspirate_infer_pressure) goto check; + if (cmdid == kpipette_aspirate_infer_eigen_time) goto check; + +check: + if (!m_state.enable) { + return err::kstep_motor_not_enable; + } + if (creg.module_errorcode == err::kstep_motor_subic_reset) { + return creg.module_errorcode; + } else if (creg.module_errorcode == err::kstep_motor_drv_err) { + return creg.module_errorcode; + } else if (creg.module_errorcode == err::kstep_motor_uv_cp) { + return creg.module_errorcode; + } else { + /** + * @brief 其他错误允许设备继续运行 + */ + return 0; + } + return 0; } void PipetteModuleV2::aftercall(int32_t val) { @@ -719,7 +827,7 @@ void PipetteModuleV2::aftercall(int32_t val) { } void PipetteModuleV2::runingcheck() { // 检查是否丢步 - if (m_cfg.motor_enable_enc != 0) { + if (m_cfg.zm_enable_enc != 0) { vPortEnterCritical(); int32_t m1enc = m_zm->read_enc_val(); @@ -729,8 +837,8 @@ void PipetteModuleV2::runingcheck() { int32_t dm1 = abs(m1enc - m1pos); - if (m_cfg.pos_devi_tolerance != 0) { - if (dm1 > m_cfg.pos_devi_tolerance) { + if (m_cfg.zm_pos_devi_tolerance != 0) { + if (dm1 > m_cfg.zm_pos_devi_tolerance) { ZLOGE(TAG, "motor pos devi %d", dm1); creg.module_errorcode = err::kstep_motor_lost_step; } diff --git a/sdk/components/pipette_module/pipette_ctrl_module_v2.hpp b/sdk/components/pipette_module/pipette_ctrl_module_v2.hpp index f586fb3..25e1f7c 100644 --- a/sdk/components/pipette_module/pipette_ctrl_module_v2.hpp +++ b/sdk/components/pipette_module/pipette_ctrl_module_v2.hpp @@ -65,37 +65,36 @@ class PipetteModuleV2 : public ZIModule { int32_t lld_prepare_pre_distribut_ul; // lld前，预分配位置 int32_t llf_area2rpm; // 液面跟随，电机运动速度和容器截面积比例关系 - int32_t motor_shaft; - int32_t motor_one_circle_pulse; // - int32_t motor_one_circle_pulse_denominator; // - int32_t motor_ihold; - int32_t motor_irun; - int32_t motor_iholddelay; - int32_t motor_iglobalscaler; - int32_t motor_default_velocity; - int32_t motor_min_d; - int32_t motor_max_d; - int32_t motor_run_to_zero_speed; - int32_t motor_look_zero_edge_speed; - int32_t motor_vstart; - int32_t motor_a1; - int32_t motor_amax; - int32_t motor_v1; - int32_t motor_dmax; - int32_t motor_d1; - int32_t motor_vstop; - int32_t motor_tzerowait; - int32_t motor_enc_resolution; - int32_t motor_enable_enc; - int32_t motor_dzero; // 零点偏移 - int32_t io_trigger_append_distance; - int32_t pos_devi_tolerance; + int32_t zm_shaft; + int32_t zm_one_circle_pulse; // + int32_t zm_one_circle_pulse_denominator; // + int32_t zm_ihold; + int32_t zm_irun; + int32_t zm_iholddelay; + int32_t zm_iglobalscaler; + int32_t zm_default_velocity; + int32_t zm_min_d; + int32_t zm_max_d; + int32_t zm_run_to_zero_speed; + int32_t zm_look_zero_edge_speed; + int32_t zm_vstart; + int32_t zm_a1; + int32_t zm_amax; + int32_t zm_v1; + int32_t zm_dmax; + int32_t zm_d1; + int32_t zm_vstop; + int32_t zm_tzerowait; + int32_t zm_enc_resolution; + int32_t zm_enable_enc; + int32_t zm_dzero; // 零点偏移 + int32_t zm_io_trigger_append_distance; + int32_t zm_pos_devi_tolerance; } config_t; typedef struct { int32_t enable; // 电机使能 int32_t load_val_ul; // 已装载量 - int32_t aspirated; // 已经吸入过液体 int32_t lld_prepared; // 已经准备液面探测 int32_t pipette_is_inited; // 泵机是否初始化 int32_t dul; // 吸吐变化量 @@ -107,6 +106,8 @@ class PipetteModuleV2 : public ZIModule { int32_t aspirate_cfg_eigen_time; // 吸液操作验证，特征时间，用来检测气泡，吸空检测 int32_t aspirate_cfg_tolerance; // 容差 int32_t zmax_move_by; // 吸液Z轴最大移动距离 + + int32_t has_move_to_zero; } state_t; struct { @@ -156,12 +157,14 @@ class PipetteModuleV2 : public ZIModule { /*********************************************************************************************************************** * Z轴控制 * ***********************************************************************************************************************/ + virtual int32_t pipette_zmotor_enable(int32_t enable); virtual int32_t pipette_zmotor_move_zero(); virtual int32_t pipette_zmotor_move_to_zero_point_quick(); virtual int32_t pipette_zmotor_measure_distance(); virtual int32_t pipette_zmotor_read_measure_distance_result(int32_t *result0); virtual int32_t pipette_zmotor_move_by(int32_t distance); virtual int32_t pipette_zmotor_move_to(int32_t position); + virtual int32_t pipette_init_device(); virtual int32_t pipette_put_tip(); virtual int32_t pipette_lld_prepare(); @@ -211,7 +214,12 @@ class PipetteModuleV2 : public ZIModule { void zm_waitfor_zm0p_trigger_and_stop_motor(); void zm_waitfor_zm0p_not_trigger_and_stop_motor(); void zm_waitfor_stop(); - void pump_waitfor_stop(); + + int32_t pump_read_pos_ul(int32_t *val); + int32_t pump_read_capactitance(int32_t *val); + int32_t pump_read_tip_state(int32_t *val); + void pump_waitfor_stop(); + // bool zm_wait_for_stop(int32_t checkperiod); private: diff --git a/sdk/components/zcancmder/zcan_protocol_parser.cpp b/sdk/components/zcancmder/zcan_protocol_parser.cpp index 2993fd4..bac0970 100644 --- a/sdk/components/zcancmder/zcan_protocol_parser.cpp +++ b/sdk/components/zcancmder/zcan_protocol_parser.cpp @@ -151,6 +151,7 @@ void ZCanProtocolParser::initialize(IZCanReceiver* cancmder) { REGFN(plate_code_scaner_open_laser); REGFN(plate_code_scaner_close_laser); + REGFN(pipette_zmotor_enable); REGFN(pipette_zmotor_move_zero); REGFN(pipette_zmotor_move_to_zero_point_quick); REGFN(pipette_zmotor_measure_distance); @@ -940,7 +941,10 @@ int32_t ZCanProtocolParser::plate_code_scaner_close_laser(cmdcontxt_t* cxt) { * PipetteModuleV2 * ***********************************************************************************************************************/ #define MODULE_CLASS PipetteModuleV2 - +int32_t ZCanProtocolParser::pipette_zmotor_enable(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(1); + return module->pipette_zmotor_enable(cxt->params[0]); +} int32_t ZCanProtocolParser::pipette_zmotor_move_zero(cmdcontxt_t* cxt) { CHECK_AND_GET_MODULE(0); return module->pipette_zmotor_move_zero(); diff --git a/sdk/components/zcancmder/zcan_protocol_parser.hpp b/sdk/components/zcancmder/zcan_protocol_parser.hpp index 41ca065..caee362 100644 --- a/sdk/components/zcancmder/zcan_protocol_parser.hpp +++ b/sdk/components/zcancmder/zcan_protocol_parser.hpp @@ -184,6 +184,8 @@ class ZCanProtocolParser : public IZCanReceiverListener { CMDFN(plate_code_scaner_open_laser); CMDFN(plate_code_scaner_close_laser); // + + CMDFN(pipette_zmotor_enable); CMDFN(pipette_zmotor_move_zero); CMDFN(pipette_zmotor_move_to_zero_point_quick); CMDFN(pipette_zmotor_measure_distance); diff --git a/usrc/subboards/subboard80_cliptip/subboard80_cliptip.cpp b/usrc/subboards/subboard80_cliptip/subboard80_cliptip.cpp index 0b4728b..4e909c5 100644 --- a/usrc/subboards/subboard80_cliptip/subboard80_cliptip.cpp +++ b/usrc/subboards/subboard80_cliptip/subboard80_cliptip.cpp @@ -53,41 +53,41 @@ void Subboard80Cliptip::initialize() { { static TMC51X0 motor; static PipetteModuleV2 module; - int subid = 1; + int subid = 2; TMC51X0::cfg_t tmc5130cfg = { .spi = &hspi1, - .csgpio = PA4, - .ennPin = PD0, + .csgpio = MOTOR1_CSN, + .ennPin = MOTOR1_ENN, }; motor.initialize(&tmc5130cfg); motor.setMotorShaft(false); static ZGPIO input[2]; - input[0].initAsInput(PD2, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true); - input[1].initAsInput(PD1, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, true); + input[0].initAsInput(MOTOR1_REFL, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, MOTOR1_REFL_MIRROR); + input[1].initAsInput(MOTOR1_REFR, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, MOTOR1_REFR_MIRROR); PipetteModuleV2::config_t cfg = {0}; PipetteModuleV2::create_default_cfg(&cfg); - cfg.motor_shaft = false; - cfg.motor_one_circle_pulse = 80; - cfg.motor_one_circle_pulse_denominator = 1; - cfg.motor_ihold = 1; - cfg.motor_irun = 8; - cfg.motor_iholddelay = 10; - cfg.motor_iglobalscaler = 0; - cfg.motor_default_velocity = 500; - cfg.motor_min_d = 0; - cfg.motor_max_d = 0; - cfg.motor_run_to_zero_speed = 200; - cfg.motor_look_zero_edge_speed = 100; - cfg.motor_vstart = 100; - cfg.motor_a1 = 50; - cfg.motor_amax = 300; - cfg.motor_v1 = 300; - cfg.motor_dmax = 300; - cfg.motor_d1 = 50; - cfg.motor_vstop = 100; - cfg.motor_tzerowait = 0; - cfg.motor_enc_resolution = 0; - cfg.motor_enable_enc = 0; + cfg.motor_shaft = MOTOR1_MOTOR_SHAFT; + cfg.motor_one_circle_pulse = MOTOR1_MOTOR_ONE_CIRCLE_PULSE; + cfg.motor_one_circle_pulse_denominator = MOTOR1_MOTOR_ONE_CIRCLE_PULSE_DENOMINATOR; + cfg.motor_ihold = MOTOR1_STEPMOTOR_IHOLD; + cfg.motor_irun = MOTOR1_STEPMOTOR_IRUN; + cfg.motor_iholddelay = MOTOR1_STEPMOTOR_IHOLDDELAY; + cfg.motor_iglobalscaler = MOTOR1_STEPMOTOR_IGLOBALSCALER; + cfg.motor_default_velocity = MOTOR1_MOTOR_DEFAULT_VELOCITY; + cfg.motor_min_d = MOTOR1_MIN_D; + cfg.motor_max_d = MOTOR1_MAX_D; + cfg.motor_run_to_zero_speed = MOTOR1_MOTOR_RUN_TO_ZERO_SPEED; + cfg.motor_look_zero_edge_speed = MOTOR1_MOTOR_LOOK_ZERO_EDGE_SPEED; + cfg.motor_vstart = MOTOR1_MOTOR_VSTART; + cfg.motor_a1 = MOTOR1_MOTOR_A1; + cfg.motor_amax = MOTOR1_MOTOR_AMAX; + cfg.motor_v1 = MOTOR1_MOTOR_V1; + cfg.motor_dmax = MOTOR1_MOTOR_DMAX; + cfg.motor_d1 = MOTOR1_MOTOR_D1; + cfg.motor_vstop = MOTOR1_MOTOR_VSTOP; + cfg.motor_tzerowait = MOTOR1_MOTOR_TZEROWAIT; + cfg.motor_enc_resolution = MOTOR1_ENC_RESOLUTION; + cfg.motor_enable_enc = MOTOR1_ENABLE_ENC_RESOLUTION; // cfg.limit_ul = 30; @@ -103,7 +103,6 @@ void Subboard80Cliptip::initialize() { module.initialize(getmoduleId(subid), &cfg, &hardwarecfg); GService::inst()->getZCanProtocolParser()->registerModule(&module); ZLOGI(TAG, "subboard80_cliptip pipette module initialized"); - } {