|
|
|
@ -389,6 +389,7 @@ int32_t PipetteModule::pipette_pump_aspirate() { |
|
|
|
dump("liquid_info", liquidinfo); |
|
|
|
|
|
|
|
clearPipetteGunState(); |
|
|
|
zm_move_to_zero_quick_block(); |
|
|
|
|
|
|
|
if (acfg->lld_enable) { |
|
|
|
_do_lld(acfg->container_pos, platform_info, container_cfg, liquidinfo); // 液面探测
|
|
|
|
@ -416,15 +417,16 @@ int32_t PipetteModule::pipette_pump_aspirate() { |
|
|
|
} |
|
|
|
|
|
|
|
void PipetteModule::_do_lld(int32_t container_pos, platinfo_t *platform_info, container_info_t *container_cfg, liquid_info_t *liquidinfo) { |
|
|
|
int32_t ecode = 0; |
|
|
|
int32_t ecode = 0; |
|
|
|
int32_t start_pressure = 0; |
|
|
|
|
|
|
|
// 移动到转移位置
|
|
|
|
ZLOGI(TAG, "-->zm move to transform pos"); |
|
|
|
zm_move_to_block(0, kzm_v_default, 0); |
|
|
|
zm_move_to_block(0, kpm_v_max, 0); |
|
|
|
// 清空tip
|
|
|
|
ZLOGI(TAG, "-->empty tip before lld"); |
|
|
|
pump_move_to_x100nl_block(0, liquidinfo->empty_tip_pm_vcpyid); |
|
|
|
pump_move_to_x100nl_block(10, kpm_v_default); // 回转一下，消除齿轮间隙
|
|
|
|
pump_move_to_x100nl_block(300 /*30ul*/, kpm_v_max); // 为lld清空tip，提供一定空气柱
|
|
|
|
|
|
|
|
// 移动到瓶口
|
|
|
|
ZLOGI(TAG, "-->zm move to container neck pos"); |
|
|
|
@ -434,15 +436,24 @@ void PipetteModule::_do_lld(int32_t container_pos, platinfo_t *platform_info, co |
|
|
|
ZLOGI(TAG, "-->start lld, lld_pm_vindex %d plld_threshold %d", liquidinfo->plld_pm_vcpyid, liquidinfo->plld_threshold); |
|
|
|
DO_IN_THREAD(m_smtp2.pump_set_io1_mode(1)); // lld输入高
|
|
|
|
//! WARNING: 这里强制设置tip类型为200ul
|
|
|
|
DO_IN_THREAD(m_smtp2.pump_set_tip_type(smtp2::TS200UL)); // 设置tip大小
|
|
|
|
DO_IN_THREAD(m_smtp2.pump_set_tip_type(smtp2::TS200UL)); // 设置tip大小
|
|
|
|
DO_IN_THREAD(m_smtp2.pump_set_plld_start_delay(100)); |
|
|
|
m_smtp2.pump_get_pressure(&start_pressure); |
|
|
|
ZLOGI(TAG, "-->start lld , start_pressure is %d", start_pressure); |
|
|
|
|
|
|
|
#if 1
|
|
|
|
pump_do_lld(liquidinfo->plld_threshold, 200, liquidinfo->plld_pm_vcpyid); // lld延时200ms，速度为plld_zm_vel
|
|
|
|
osDelay(110); // 等待lld启动
|
|
|
|
#else
|
|
|
|
pump_start_lld(2400, liquidinfo->plld_pm_vcpyid); |
|
|
|
#endif
|
|
|
|
|
|
|
|
// 快速移动到瓶口
|
|
|
|
ZLOGI(TAG, "-->zm move quick to lld search start pos"); |
|
|
|
zm_move_to_lld_start_search_pos_block(container_pos, container_cfg, kzm_v_default, 0); |
|
|
|
// lld,zm移动到瓶底
|
|
|
|
ZLOGI(TAG, "-->zm move slow to container bottom pos"); |
|
|
|
zm_move_to(container_pos + container_cfg->container_depth, kzm_v_lld, liquidinfo->plld_zm_vel); |
|
|
|
zm_move_to(container_pos + container_cfg->container_depth - 50, kzm_v_lld, liquidinfo->plld_zm_vel); |
|
|
|
|
|
|
|
/**
|
|
|
|
* @brief 等待压力大于阈值 |
|
|
|
@ -450,6 +461,7 @@ void PipetteModule::_do_lld(int32_t container_pos, platinfo_t *platform_info, co |
|
|
|
int32_t water_level = 0; |
|
|
|
bool detect_liquid = false; |
|
|
|
while (true) { |
|
|
|
#if 1
|
|
|
|
if (m_zm->isStoped()) { |
|
|
|
detect_liquid = false; |
|
|
|
ZLOGI(TAG, "-->zm is stoped"); |
|
|
|
@ -458,13 +470,52 @@ void PipetteModule::_do_lld(int32_t container_pos, platinfo_t *platform_info, co |
|
|
|
|
|
|
|
// lld触发
|
|
|
|
if (m_piette_gun_io1.getState()) { |
|
|
|
int32_t water_level = zm_get_now_pos(); |
|
|
|
detect_liquid = true; |
|
|
|
water_level = zm_get_now_pos() - liquidinfo->plld_invasion_depth; |
|
|
|
detect_liquid = true; |
|
|
|
m_zm->stop(); |
|
|
|
ZLOGI(TAG, "-->detect liquid, water_level %d", water_level); |
|
|
|
break; |
|
|
|
} |
|
|
|
ZLOGI(TAG, "...wait for lld trigger, now zm pos %d", zm_get_now_pos()); |
|
|
|
if (!m_common_cfg.pressure_record_enable) { |
|
|
|
ZLOGI(TAG, "...wait for lld trigger, now zm pos %d", zm_get_now_pos()); |
|
|
|
} else { |
|
|
|
int32_t pressure = 0; |
|
|
|
m_smtp2.pump_get_pressure(&pressure); |
|
|
|
ZLOGI(TAG, "...wait for lld trigger, now zm pos %d, pressure %d dpressure %d", zm_get_now_pos(), pressure, pressure - start_pressure); |
|
|
|
} |
|
|
|
#else
|
|
|
|
if (m_zm->isStoped()) { |
|
|
|
detect_liquid = false; |
|
|
|
ZLOGI(TAG, "-->zm is stoped"); |
|
|
|
break; |
|
|
|
} |
|
|
|
|
|
|
|
int32_t pressure = 0; |
|
|
|
int32_t dpressure0, dpressure1 = 0; |
|
|
|
|
|
|
|
m_smtp2.pump_get_pressure(&pressure); |
|
|
|
dpressure0 = pressure - start_pressure; |
|
|
|
|
|
|
|
// lld触发
|
|
|
|
if (abs(dpressure0) > liquidinfo->plld_threshold) { |
|
|
|
water_level = zm_get_now_pos() - liquidinfo->plld_invasion_depth; |
|
|
|
detect_liquid = true; |
|
|
|
|
|
|
|
m_smtp2.pump_get_pressure(&pressure); |
|
|
|
dpressure1 = pressure - start_pressure; |
|
|
|
if (abs(dpressure1) < abs(dpressure0) + 5) { |
|
|
|
detect_liquid = false; |
|
|
|
ZLOGW(TAG, "!!!!!!!!!!!!!!!!!!!!!!!!!!!! %d", dpressure1); |
|
|
|
continue; |
|
|
|
} |
|
|
|
|
|
|
|
m_zm->stop(); |
|
|
|
ZLOGI(TAG, "-->detect liquid, water_level %d,dpressure0 %d,dpressure1 %d", water_level, dpressure0, dpressure1); |
|
|
|
break; |
|
|
|
} |
|
|
|
|
|
|
|
ZLOGI(TAG, "...wait for lld trigger, now zm pos %d, pressure %d dpressure %d", zm_get_now_pos(), pressure, dpressure0); |
|
|
|
#endif
|
|
|
|
thread_delay(1); |
|
|
|
} |
|
|
|
ecode = m_smtp2.pump_get_state(nullptr); // 获取泵状态，在此之前不能执行任何泵机指令，否则会丢失错误状态
|
|
|
|
@ -489,8 +540,11 @@ void PipetteModule::_do_lld(int32_t container_pos, platinfo_t *platform_info, co |
|
|
|
DO_IN_THREAD(m_smtp2.pump_stop()); |
|
|
|
m_zm->stop(); |
|
|
|
|
|
|
|
zm_move_to_block(water_level, kzm_v_swap_out, 0); // 回到默认位置
|
|
|
|
|
|
|
|
if (!detect_liquid) { |
|
|
|
ZLOGE(TAG, "detect liquid fail"); |
|
|
|
zm_move_to_block(0, kzm_v_default, 0); |
|
|
|
throw zapp_exception(err::kpipette_lld_error_no_liquid_detected); |
|
|
|
} |
|
|
|
m_state.detected_liquid = 1; |
|
|
|
@ -533,7 +587,7 @@ void PipetteModule::_do_sapirate(platinfo_t *platform_info, container_info_t *co |
|
|
|
|
|
|
|
// Z轴移动到液面之下
|
|
|
|
ZLOGI(TAG, "--> move below the liquid surface"); |
|
|
|
zm_move_to_immersion_pos_block(-1, m_state.water_level, container_cfg, kzm_v_swap_in, 0); // 移动到液面下
|
|
|
|
zm_move_to_immersion_pos_block(acfg->container_pos, m_state.water_level, container_cfg, kzm_v_swap_in, 0); // 移动到液面下
|
|
|
|
|
|
|
|
// 吸入润湿TIP的溶液
|
|
|
|
if (liquidinfo->over_aspirated_volume > 0) { |
|
|
|
@ -549,7 +603,7 @@ void PipetteModule::_do_sapirate(platinfo_t *platform_info, container_info_t *co |
|
|
|
for (size_t i = 0; i < acfg->mix_times; i++) { |
|
|
|
// 移动到瓶底
|
|
|
|
if (acfg->mix_llf_enable > 0) { |
|
|
|
zm_move_to(acfg->container_pos + container_cfg->container_depth, kzm_v_llf, compute_zm_llf_vel(liquidinfo->mix_pm_vcpyid, container_cfg)); |
|
|
|
zm_move_to(acfg->container_pos + container_cfg->container_depth - 15, kzm_v_llf, compute_zm_llf_vel(liquidinfo->mix_pm_vcpyid, container_cfg)); |
|
|
|
} |
|
|
|
pump_move_by_x100nl_block(acfg->mix_volume, liquidinfo->mix_pm_vcpyid); |
|
|
|
if (acfg->mix_llf_enable > 0) zm_stop(); |
|
|
|
@ -669,10 +723,11 @@ int32_t PipetteModule::pipette_pump_distribu_all() { |
|
|
|
// 移动到swap位置
|
|
|
|
ZLOGI(TAG, "-->move to swap position"); |
|
|
|
zm_move_to_leaving_height_pos_block(container_pos, -1, container_cfg, kzm_v_default, 0); // 移动到swap位置
|
|
|
|
|
|
|
|
// 清空transport volume
|
|
|
|
if (liquidInGun_info->transport_volume != 0) { |
|
|
|
ZLOGI(TAG, "-->clean transport volume"); |
|
|
|
pump_move_to_x100nl_block(liquidInGun_info->transport_volume, liquidInGun_info->transport_volume_pm_vcpyid); // 清空transport volume
|
|
|
|
pump_move_by_x100nl_block(-liquidInGun_info->transport_volume, liquidInGun_info->transport_volume_pm_vcpyid); // 清空transport volume
|
|
|
|
} else { |
|
|
|
ZLOGI(TAG, "-->clean transport volume,skip"); |
|
|
|
} |
|
|
|
@ -684,7 +739,7 @@ int32_t PipetteModule::pipette_pump_distribu_all() { |
|
|
|
// 分液
|
|
|
|
ZLOGI(TAG, "-->distribu liquid"); |
|
|
|
int32_t distribu_volume = calibrate_liquid_volume(liquidInGun_info, m_pump_gun_state.liquid_volume); |
|
|
|
pump_move_by_x100nl_block(distribu_volume, liquidInGun_info->distribu_pm_vpyid); // 分液
|
|
|
|
pump_move_by_x100nl_block(-distribu_volume, liquidInGun_info->distribu_pm_vpyid); // 分液
|
|
|
|
|
|
|
|
// 移动到swap位置，快速清空tip,使用(emptyTip速度)
|
|
|
|
ZLOGI(TAG, "-->move to swap position and clean tip"); |
|
|
|
|
