18 changed files with 1826 additions and 1265 deletions
-
93sdk/components/pipette_module/base/pipette_action_param.hpp
-
146sdk/components/pipette_module/base/pipette_cfg.hpp
-
34sdk/components/pipette_module/base/pipette_enum.hpp
-
1sdk/components/pipette_module/base/pipette_marco_utils.hpp
-
49sdk/components/pipette_module/base/pipette_state.hpp
-
1292sdk/components/pipette_module/pipette_ctrl_module.cpp
-
197sdk/components/pipette_module/pipette_ctrl_module.hpp
-
477sdk/components/pipette_module/pipette_ctrl_module_config_function_impl.cpp
-
124sdk/components/pipette_module/pipette_ctrl_module_pm_ctrl.cpp
-
116sdk/components/pipette_module/pipette_ctrl_module_test.cpp
-
194sdk/components/pipette_module/pipette_ctrl_module_utils.cpp
-
242sdk/components/pipette_module/pipette_ctrl_module_zm_ctrl.cpp
-
58sdk/components/zcan_protocol_parser/zcan_protocol_parser.cpp
-
11sdk/components/zcan_protocol_parser/zcan_protocol_parser.hpp
-
10usrc/a8000_protocol/protocol/cmdid.cpp
-
44usrc/a8000_protocol/protocol/cmdid.hpp
-
2usrc/a8000_protocol/protocol/errorcode.cpp
-
1usrc/a8000_protocol/protocol/errorcode.hpp
1292
sdk/components/pipette_module/pipette_ctrl_module.cpp
File diff suppressed because it is too large
View File
File diff suppressed because it is too large
View File
@ -0,0 +1,477 @@ |
|||
#include <stdarg.h>
|
|||
|
|||
#include "pipette_ctrl_module.hpp"
|
|||
#include "sdk\components\zcancmder\protocol_event_bus_sender.hpp"
|
|||
//
|
|||
#include "base/pipette_marco_utils.hpp"
|
|||
#include "sdk/chip/exhal/stm32_exhal_uart.hpp"
|
|||
#include "sdk\components\exception\zapp_exception.hpp"
|
|||
#include "sdk\components\exception\zapp_thread_stoped_exception.hpp"
|
|||
#define TAG "PipetteModule"
|
|||
|
|||
using namespace iflytop; |
|||
int32_t PipetteModule::pipette_set_pmbcfg(pm_bcfg_index_t index, int32_t val) { |
|||
switch (index) { |
|||
SET_CFG(kpm_bcfg_pad, pm_base_cfg.pad, val); |
|||
SET_CFG(kpm_bcfg_mark, pm_base_cfg.mark, val); |
|||
default: { |
|||
ZLOGE(TAG, "pipette_set_pmbcfg index %d out of range", index); |
|||
return err::kparam_out_of_range; |
|||
} |
|||
} |
|||
return 0; |
|||
} |
|||
int32_t PipetteModule::pipette_get_pmbcfg(pm_bcfg_index_t index, int32_t *val) { |
|||
switch (index) { |
|||
GET_CFG(kpm_bcfg_pad, pm_base_cfg.pad, val); |
|||
GET_CFG(kpm_bcfg_mark, pm_base_cfg.mark, val); |
|||
default: { |
|||
ZLOGE(TAG, "pipette_get_pmbcfg index %d out of range", index); |
|||
return err::kparam_out_of_range; |
|||
} |
|||
} |
|||
return 0; |
|||
} |
|||
|
|||
int32_t PipetteModule::pipette_set_liquid_info(int32_t cpyid, liquid_info_index_t index, int32_t val) { |
|||
if (cpyid < 0 || cpyid >= ARRARY_SIZE(m_liquid_info)) { |
|||
ZLOGE(TAG, "pipette_set_liquid_info cpyid %d out of range", cpyid); |
|||
return err::kparam_out_of_range; |
|||
} |
|||
|
|||
liquid_info_t *cfg = &m_liquid_info[cpyid]; |
|||
if (index < 0 || index > kliquid_info_mark) { |
|||
ZLOGE(TAG, "pipette_set_liquid_info index %d out of range", index); |
|||
return err::kparam_out_of_range; |
|||
} |
|||
|
|||
int32_t *settadd = ((int32_t *)cfg) + index; |
|||
*settadd = val; // 直接设置值
|
|||
ZLOGI(TAG, "pipette_set_liquid_info cpyid %d index %s val %d", cpyid, liquid_info_index_to_string(index), val); |
|||
return 0; |
|||
} |
|||
int32_t PipetteModule::pipette_get_liquid_info(int32_t cpyid, liquid_info_index_t index, int32_t *val) { |
|||
if (cpyid < 0 || cpyid >= ARRARY_SIZE(m_liquid_info)) { |
|||
ZLOGE(TAG, "pipette_get_liquid_info cpyid %d out of range", cpyid); |
|||
return err::kparam_out_of_range; |
|||
} |
|||
|
|||
if (index < 0 || index > kliquid_info_mark) { |
|||
ZLOGE(TAG, "pipette_get_liquid_info index %d out of range", index); |
|||
return err::kparam_out_of_range; |
|||
} |
|||
|
|||
liquid_info_t *cfg = &m_liquid_info[cpyid]; |
|||
int32_t *settadd = ((int32_t *)cfg) + index; |
|||
|
|||
*val = *settadd; // 直接获取值
|
|||
ZLOGI(TAG, "pipette_get_liquid_info cpyid %d index %s val %d", cpyid, liquid_info_index_to_string(index), *val); |
|||
|
|||
return 0; |
|||
} |
|||
|
|||
int32_t PipetteModule::pipette_set_zmbcfg(zm_bcfg_index_t index, int32_t val) { |
|||
if (index < 0 || index > kzm_bcfg_max) { |
|||
ZLOGE(TAG, "pipette_set_zmbcfg index %d out of range", index); |
|||
return err::kparam_out_of_range; |
|||
} |
|||
|
|||
int32_t *settadd = ((int32_t *)&zmbcfg) + index; |
|||
*settadd = val; // 直接设置值
|
|||
|
|||
ZLOGI(TAG, "pipette_set_zmbcfg index %s val %d", zm_bcfg_index_to_string(index), val); |
|||
|
|||
zm_sync_base_cfg(); |
|||
return 0; |
|||
} |
|||
int32_t PipetteModule::pipette_get_zmbcfg(zm_bcfg_index_t index, int32_t *val) { |
|||
if (index < 0 || index > kzm_bcfg_max) { |
|||
ZLOGE(TAG, "pipette_get_zmbcfg index %d out of range", index); |
|||
return err::kparam_out_of_range; |
|||
} |
|||
|
|||
int32_t *settadd = ((int32_t *)&zmbcfg) + index; |
|||
*val = *settadd; // 直接获取值
|
|||
ZLOGI(TAG, "pipette_get_zmbcfg index %s val %d", zm_bcfg_index_to_string(index), *val); |
|||
|
|||
return 0; |
|||
} |
|||
|
|||
int32_t PipetteModule::pipette_set_platinfo(int32_t cpyid, platinfo_index_t index, int32_t val) { |
|||
if (cpyid < 0 || cpyid >= ARRARY_SIZE(m_platinfo)) { |
|||
ZLOGE(TAG, "pipette_set_platinfo cpyid %d out of range", cpyid); |
|||
return err::kparam_out_of_range; |
|||
} |
|||
|
|||
if (index < 0 || index > kplatinfo_mark) { |
|||
ZLOGE(TAG, "pipette_set_platinfo index %d out of range", index); |
|||
return err::kparam_out_of_range; |
|||
} |
|||
|
|||
platinfo_t *platinfo = &m_platinfo[cpyid]; |
|||
int32_t *settadd = ((int32_t *)platinfo) + index; |
|||
*settadd = val; // 直接设置值
|
|||
|
|||
ZLOGI(TAG, "pipette_set_platinfo cpyid %d index %s val %d", cpyid, platinfo_index_to_string(index), val); |
|||
|
|||
return 0; |
|||
} |
|||
int32_t PipetteModule::pipette_get_platinfo(int32_t cpyid, platinfo_index_t index, int32_t *val) { |
|||
if (cpyid < 0 || cpyid >= ARRARY_SIZE(m_platinfo)) { |
|||
ZLOGE(TAG, "pipette_get_platinfo cpyid %d out of range", cpyid); |
|||
return err::kparam_out_of_range; |
|||
} |
|||
|
|||
if (index < 0 || index > kplatinfo_mark) { |
|||
ZLOGE(TAG, "pipette_get_platinfo index %d out of range", index); |
|||
return err::kparam_out_of_range; |
|||
} |
|||
|
|||
platinfo_t *platinfo = &m_platinfo[cpyid]; |
|||
int32_t *settadd = ((int32_t *)platinfo) + index; |
|||
*val = *settadd; // 直接获取值
|
|||
|
|||
ZLOGI(TAG, "pipette_get_platinfo cpyid %d index %s val %d", cpyid, platinfo_index_to_string(index), *val); |
|||
return 0; |
|||
} |
|||
|
|||
int32_t PipetteModule::pipette_set_zmvcfg(int32_t cpyid, zm_vcfg_index_t index, int32_t val) { |
|||
if (cpyid < 0 || cpyid >= ARRARY_SIZE(m_zm_vcfg)) { |
|||
ZLOGE(TAG, "pipette_set_zmvcfg cpyid %d out of range", cpyid); |
|||
return err::kparam_out_of_range; |
|||
} |
|||
if (index < 0 || index > kzm_vcfg_max) { |
|||
ZLOGE(TAG, "pipette_set_zmvcfg index %d out of range", index); |
|||
return err::kparam_out_of_range; |
|||
} |
|||
|
|||
zm_vcfg_t *cfg = &m_zm_vcfg[cpyid]; |
|||
int32_t *settadd = ((int32_t *)cfg) + index; |
|||
*settadd = val; // 直接设置值
|
|||
return 0; |
|||
} |
|||
int32_t PipetteModule::pipette_get_zmvcfg(int32_t cpyid, zm_vcfg_index_t index, int32_t *val) { |
|||
if (cpyid < 0 || cpyid >= ARRARY_SIZE(m_zm_vcfg)) { |
|||
ZLOGE(TAG, "pipette_get_zmvcfg cpyid %d out of range", cpyid); |
|||
return err::kparam_out_of_range; |
|||
} |
|||
|
|||
if (index < 0 || index > kzm_vcfg_max) { |
|||
ZLOGE(TAG, "pipette_get_zmvcfg index %d out of range", index); |
|||
return err::kparam_out_of_range; |
|||
} |
|||
|
|||
zm_vcfg_t *cfg = &m_zm_vcfg[cpyid]; |
|||
int32_t *settadd = ((int32_t *)cfg) + index; |
|||
*val = *settadd; // 直接获取值
|
|||
|
|||
ZLOGI(TAG, "pipette_get_zmvcfg cpyid %d index %s val %d", cpyid, zm_vcfg_index_to_string(index), *val); |
|||
|
|||
return 0; |
|||
} |
|||
|
|||
int32_t PipetteModule::pipette_set_pmvcfg(int32_t cpyid, pm_vcfg_index_t index, int32_t val) { |
|||
if (cpyid < 0 || cpyid >= ARRARY_SIZE(m_pm_vcfg)) { |
|||
ZLOGE(TAG, "pipette_set_pmvcfg cpyid %d out of range", cpyid); |
|||
return err::kparam_out_of_range; |
|||
} |
|||
|
|||
if (index < 0 || index > kpm_vcfg_max) { |
|||
ZLOGE(TAG, "pipette_set_pmvcfg index %d out of range", index); |
|||
return err::kparam_out_of_range; |
|||
} |
|||
|
|||
pm_vcfg_t *cfg = &m_pm_vcfg[cpyid]; |
|||
int32_t *settadd = ((int32_t *)cfg) + index; |
|||
*settadd = val; // 直接设置值
|
|||
|
|||
ZLOGI(TAG, "pipette_set_pmvcfg cpyid %d index %s val %d", cpyid, pm_vcfg_index_to_string(index), val); |
|||
|
|||
return 0; |
|||
} |
|||
int32_t PipetteModule::pipette_get_pmvcfg(int32_t cpyid, pm_vcfg_index_t index, int32_t *val) { |
|||
if (cpyid < 0 || cpyid >= ARRARY_SIZE(m_pm_vcfg)) { |
|||
ZLOGE(TAG, "pipette_get_pmvcfg cpyid %d out of range", cpyid); |
|||
return err::kparam_out_of_range; |
|||
} |
|||
|
|||
if (index < 0 || index > kpm_vcfg_max) { |
|||
ZLOGE(TAG, "pipette_get_pmvcfg index %d out of range", index); |
|||
return err::kparam_out_of_range; |
|||
} |
|||
|
|||
pm_vcfg_t *cfg = &m_pm_vcfg[cpyid]; |
|||
int32_t *settadd = ((int32_t *)cfg) + index; |
|||
*val = *settadd; // 直接获取值
|
|||
|
|||
ZLOGI(TAG, "pipette_get_pmvcfg cpyid %d index %s val %d", cpyid, pm_vcfg_index_to_string(index), *val); |
|||
return 0; |
|||
} |
|||
int32_t PipetteModule::pipette_set_container_info(int32_t cpyid, container_info_index_t index, int32_t val) { |
|||
if (cpyid < 0 || cpyid >= ARRARY_SIZE(m_container_info)) { |
|||
ZLOGE(TAG, "pipette_set_container_info cpyid %d out of range", cpyid); |
|||
return err::kparam_out_of_range; |
|||
} |
|||
|
|||
if (index < 0 || index > kcontainer_info_mark) { |
|||
ZLOGE(TAG, "pipette_set_container_info index %d out of range", index); |
|||
return err::kparam_out_of_range; |
|||
} |
|||
|
|||
container_info_t *cfg = &m_container_info[cpyid]; |
|||
int32_t *settadd = ((int32_t *)cfg) + index; |
|||
|
|||
*settadd = val; // 直接设置值
|
|||
ZLOGI(TAG, "pipette_set_container_info cpyid %d index %s val %d", cpyid, container_info_index_to_string(index), val); |
|||
|
|||
return 0; |
|||
} |
|||
int32_t PipetteModule::pipette_get_container_info(int32_t cpyid, container_info_index_t index, int32_t *val) { |
|||
if (cpyid < 0 || cpyid >= ARRARY_SIZE(m_container_info)) { |
|||
ZLOGE(TAG, "pipette_get_container_info cpyid %d out of range", cpyid); |
|||
return err::kparam_out_of_range; |
|||
} |
|||
|
|||
if (index < 0 || index > kcontainer_info_mark) { |
|||
ZLOGE(TAG, "pipette_get_container_info index %d out of range", index); |
|||
return err::kparam_out_of_range; |
|||
} |
|||
|
|||
container_info_t *cfg = &m_container_info[cpyid]; |
|||
int32_t *settadd = ((int32_t *)cfg) + index; |
|||
*val = *settadd; // 直接获取值
|
|||
|
|||
ZLOGI(TAG, "pipette_get_container_info cpyid %d index %s val %d", cpyid, container_info_index_to_string(index), *val); |
|||
|
|||
return 0; |
|||
} |
|||
|
|||
int32_t PipetteModule::pipette_read_state(pipette_state_index_t stateindex, int32_t *val) { |
|||
static_assert(kpipette_state_max == sizeof(m_state) / 4); |
|||
int32_t off = (int32_t)stateindex; |
|||
if (off < 0 || off >= (int32_t)kpipette_state_max) { |
|||
ZLOGE(TAG, "pipette_read_state index %d out of range", stateindex); |
|||
return err::kparam_out_of_range; |
|||
} |
|||
*val = *(((int32_t *)&m_state) + off); |
|||
ZLOGI(TAG, "pipette_read_state index %d val %d", stateindex, *val); |
|||
return 0; |
|||
} |
|||
|
|||
int32_t PipetteModule::pipette_get_platinfo_max_cpyid(int32_t *cpyid) { |
|||
*cpyid = ARRARY_SIZE(m_platinfo) - 1; |
|||
return 0; |
|||
} |
|||
int32_t PipetteModule::pipette_get_zmvcfg_max_cpyid(int32_t *cpyid) { |
|||
*cpyid = ARRARY_SIZE(m_zm_vcfg) - 1; |
|||
return 0; |
|||
} |
|||
|
|||
int32_t PipetteModule::pipette_get_pmvcfg_max_cpyid(int32_t *cpyid) { |
|||
*cpyid = ARRARY_SIZE(m_pm_vcfg) - 1; |
|||
return 0; |
|||
} |
|||
|
|||
int32_t PipetteModule::pipette_get_container_info_max_cpyid(int32_t *cpyid) { |
|||
*cpyid = ARRARY_SIZE(m_container_info) - 1; |
|||
return 0; |
|||
} |
|||
|
|||
int32_t PipetteModule::pipette_get_liquid_info_max_cpyid(int32_t *cpyid) { |
|||
*cpyid = ARRARY_SIZE(m_liquid_info) - 1; |
|||
return 0; |
|||
} |
|||
|
|||
zm_vcfg_t *PipetteModule::get_zm_vcfg(int32_t index) { |
|||
if (index < 0 || index >= ARRARY_SIZE(m_zm_vcfg)) { |
|||
ZLOGE(TAG, "get_zm_vcfg index %d out of range", index); |
|||
return &m_zm_vcfg[0]; |
|||
} |
|||
return &m_zm_vcfg[index]; |
|||
} |
|||
|
|||
pm_vcfg_t *PipetteModule::get_pm_vcfg(int32_t index) { |
|||
if (index < 0 || index >= ARRARY_SIZE(m_pm_vcfg)) { |
|||
ZLOGE(TAG, "get_pm_vcfg index %d out of range", index); |
|||
return &m_pm_vcfg[0]; |
|||
} |
|||
return &m_pm_vcfg[index]; |
|||
} |
|||
|
|||
container_info_t *PipetteModule::get_container_info(int32_t index) { |
|||
if (index < 0 || index >= ARRARY_SIZE(m_container_info)) { |
|||
ZLOGE(TAG, "get_container_info index %d out of range", index); |
|||
return &m_container_info[0]; |
|||
} |
|||
return &m_container_info[index]; |
|||
} |
|||
liquid_info_t *PipetteModule::get_liquid_info(int32_t index) { |
|||
if (index < 0 || index >= ARRARY_SIZE(m_liquid_info)) { |
|||
ZLOGE(TAG, "get_liquid_info index %d out of range", index); |
|||
return &m_liquid_info[0]; |
|||
} |
|||
return &m_liquid_info[index]; |
|||
} |
|||
platinfo_t *PipetteModule::get_platinfo(int32_t index) { |
|||
if (index < 0 || index >= ARRARY_SIZE(m_platinfo)) { |
|||
ZLOGE(TAG, "get_platinfo index %d out of range", index); |
|||
return &m_platinfo[0]; |
|||
} |
|||
return &m_platinfo[index]; |
|||
} |
|||
|
|||
void PipetteModule::get_cfg_smart(int32_t index, zm_vcfg_t *cfg) { //
|
|||
zm_vcfg_t *cfg0 = get_zm_vcfg(0); |
|||
zm_vcfg_t *cfgx = get_zm_vcfg(index); |
|||
static_assert(sizeof(zm_vcfg_t) % sizeof(int32_t) == 0, "zm_vcfg_t size is not multiple of int32_t"); |
|||
assign_cfg_smart((int32_t *)cfg, (int32_t *)cfgx, (int32_t *)cfg0, sizeof(zm_vcfg_t) / sizeof(int32_t)); |
|||
return; |
|||
} |
|||
void PipetteModule::get_cfg_smart(int32_t index, pm_vcfg_t *cfg) { |
|||
/**
|
|||
* @brief 当配置项的值为负数时，使用默认配置项的值 |
|||
*/ |
|||
pm_vcfg_t *cfg0 = get_pm_vcfg(0); |
|||
pm_vcfg_t *cfgx = get_pm_vcfg(index); |
|||
static_assert(sizeof(pm_vcfg_t) % sizeof(int32_t) == 0, "pm_vcfg_t size is not multiple of int32_t"); |
|||
assign_cfg_smart((int32_t *)cfg, (int32_t *)cfgx, (int32_t *)cfg0, sizeof(pm_vcfg_t) / sizeof(int32_t)); |
|||
return; |
|||
} |
|||
|
|||
container_info_t *PipetteModule::get_container_info_smart(int32_t index, container_info_t *val) { |
|||
container_info_t *cfg = get_container_info(index); |
|||
container_info_t *cfg0 = get_container_info(0); |
|||
static_assert(sizeof(container_info_t) % sizeof(int32_t) == 0, "container_info_t size is not multiple of int32_t"); |
|||
assign_cfg_smart((int32_t *)val, (int32_t *)cfg, (int32_t *)cfg0, sizeof(container_info_t) / sizeof(int32_t)); |
|||
return val; |
|||
} |
|||
platinfo_t *PipetteModule::get_platinfo_smart(int32_t index, platinfo_t *val) { |
|||
platinfo_t *cfg = get_platinfo(index); |
|||
platinfo_t *cfg0 = get_platinfo(0); |
|||
static_assert(sizeof(platinfo_t) % sizeof(int32_t) == 0, "platinfo_t size is not multiple of int32_t"); |
|||
assign_cfg_smart((int32_t *)val, (int32_t *)cfg, (int32_t *)cfg0, sizeof(platinfo_t) / sizeof(int32_t)); |
|||
return val; |
|||
} |
|||
|
|||
liquid_info_t *PipetteModule::get_liquid_info_smart(int32_t index, liquid_info_t *val) { |
|||
liquid_info_t *cfg = get_liquid_info(index); |
|||
liquid_info_t *cfg0 = get_liquid_info(0); |
|||
static_assert(sizeof(liquid_info_t) % sizeof(int32_t) == 0, "liquid_info_t size is not multiple of int32_t"); |
|||
assign_cfg_smart((int32_t *)val, (int32_t *)cfg, (int32_t *)cfg0, sizeof(liquid_info_t) / sizeof(int32_t)); |
|||
return val; |
|||
} |
|||
|
|||
void PipetteModule::parameter_init() { |
|||
/***********************************************************************************************************************
|
|||
* zmbcfg * |
|||
***********************************************************************************************************************/ |
|||
zmbcfg.shaft = 0; // Z轴电机运动方向调整
|
|||
zmbcfg.one_circle_pulse = 80; // 电子齿轮比-分子
|
|||
zmbcfg.one_circle_pulse_denominator = 1; // 电子齿轮比-分母
|
|||
zmbcfg.ihold = 7; //
|
|||
zmbcfg.irun = 7; //
|
|||
zmbcfg.iholddelay = 10; //
|
|||
zmbcfg.iglobalscaler = 0; //
|
|||
zmbcfg.min_d = 0; //
|
|||
zmbcfg.max_d = 0; //
|
|||
zmbcfg.tzerowait = 0; //
|
|||
zmbcfg.enc_resolution = 0; //
|
|||
zmbcfg.enable_enc = 0; //
|
|||
zmbcfg.dzero = 92; //
|
|||
zmbcfg.io_trigger_append_distance = 20; //
|
|||
zmbcfg.pos_devi_tolerance = 20; //
|
|||
zmbcfg.mres = 0; //
|
|||
zmbcfg.mark = 9973; //
|
|||
/***********************************************************************************************************************
|
|||
* pm_base_cfg * |
|||
***********************************************************************************************************************/ |
|||
pm_base_cfg.pad = 0; |
|||
pm_base_cfg.mark = 9973; |
|||
|
|||
/***********************************************************************************************************************
|
|||
* m_platinfo * |
|||
***********************************************************************************************************************/ |
|||
memset_int32_t((int32_t *)&m_platinfo[0], -1, sizeof(m_platinfo) / 4); |
|||
memset_int32_t((int32_t *)&m_platinfo[0], 0, sizeof(m_platinfo[0]) / 4); |
|||
m_platinfo[0].tip_type = smtp2::TS200UL; |
|||
m_platinfo[0].tip_length = 500; // 50mm
|
|||
m_platinfo[0].mark = 9973; //
|
|||
|
|||
/***********************************************************************************************************************
|
|||
* m_zm_vcfg * |
|||
***********************************************************************************************************************/ |
|||
memset_int32_t((int32_t *)&m_zm_vcfg[0], -1, sizeof(m_zm_vcfg) / 4); |
|||
memset_int32_t((int32_t *)&m_zm_vcfg[0], 0, sizeof(m_zm_vcfg[0]) / 4); |
|||
m_zm_vcfg[kzm_v_default].vstart = 100; |
|||
m_zm_vcfg[kzm_v_default].a1 = 50; |
|||
m_zm_vcfg[kzm_v_default].amax = 100; |
|||
m_zm_vcfg[kzm_v_default].v1 = 100; |
|||
m_zm_vcfg[kzm_v_default].dmax = 100; |
|||
m_zm_vcfg[kzm_v_default].d1 = 200; |
|||
m_zm_vcfg[kzm_v_default].vstop = 400; |
|||
m_zm_vcfg[kzm_v_default].vmax = 1200; |
|||
m_zm_vcfg[kzm_v_default].mark = 9973; |
|||
|
|||
m_zm_vcfg[kzm_v_move_to_zero].vmax = 600; |
|||
m_zm_vcfg[kzm_v_look_zero_edge].vmax = 100; |
|||
m_zm_vcfg[kzm_v_lld].vmax = 100; |
|||
m_zm_vcfg[kzm_v_swap_in].vmax = 300; |
|||
m_zm_vcfg[kzm_v_swap_out].vmax = 300; |
|||
m_zm_vcfg[kzm_v_llf].vmax = 500; |
|||
|
|||
/***********************************************************************************************************************
|
|||
* m_pm_vcfg * |
|||
***********************************************************************************************************************/ |
|||
|
|||
memset_int32_t((int32_t *)&m_pm_vcfg[0], -1, sizeof(m_pm_vcfg) / 4); |
|||
memset_int32_t((int32_t *)&m_pm_vcfg[0], 0, sizeof(m_pm_vcfg[0]) / 4); |
|||
m_pm_vcfg[kpm_v_default].acc = 1; |
|||
m_pm_vcfg[kpm_v_default].dec = 1; |
|||
m_pm_vcfg[kpm_v_default].vstart = 100; |
|||
m_pm_vcfg[kpm_v_default].vstop = 50; |
|||
m_pm_vcfg[kpm_v_default].mark = 9973; |
|||
|
|||
//
|
|||
m_pm_vcfg[kpm_v_default].vmax = 2500; |
|||
m_pm_vcfg[kpm_v_slow_lv1].vmax = 5; |
|||
m_pm_vcfg[kpm_v_slow_lv2].vmax = 10; |
|||
m_pm_vcfg[kpm_v_slow_lv3].vmax = 50; |
|||
m_pm_vcfg[kpm_v_mid].vmax = 1000; |
|||
m_pm_vcfg[kpm_v_quick].vmax = 2000; |
|||
m_pm_vcfg[kpm_v_max].vmax = 2500; |
|||
m_pm_vcfg[kpm_v_lld].vmax = 100; |
|||
|
|||
/***********************************************************************************************************************
|
|||
* m_container_info * |
|||
***********************************************************************************************************************/ |
|||
memset_int32_t((int32_t *)&m_container_info[0], -1, sizeof(m_container_info) / 4); |
|||
memset_int32_t((int32_t *)&m_container_info[0], 0, sizeof(m_container_info[0]) / 4); |
|||
m_container_info[0].leaving_height = 150; |
|||
m_container_info[0].mark = 9973; |
|||
|
|||
/***********************************************************************************************************************
|
|||
* m_liquid_info * |
|||
***********************************************************************************************************************/ |
|||
|
|||
memset_int32_t((int32_t *)&m_liquid_info[0], -1, sizeof(m_liquid_info) / 4); |
|||
memset_int32_t((int32_t *)&m_liquid_info[0], 0, sizeof(m_liquid_info[0]) / 4); |
|||
|
|||
m_liquid_info[0].plld_pm_vcpyid = kpm_v_lld; |
|||
m_liquid_info[0].plld_threshold = 30; |
|||
m_liquid_info[0].plld_zm_vel = 50; |
|||
m_liquid_info[0].empty_tip_pm_vcpyid = kpm_v_max; |
|||
m_liquid_info[0].blowout_air_volume = 0; |
|||
m_liquid_info[0].blowout_air_pm_vcpyid = kpm_v_quick; |
|||
m_liquid_info[0].over_aspirated_volume = 100; // 100(x100nl) == 10ul
|
|||
m_liquid_info[0].over_aspirated_pm_vcpyid = kpm_v_slow_lv2; |
|||
m_liquid_info[0].aspiration_pm_vindex_low = kpm_v_slow_lv1; |
|||
m_liquid_info[0].aspiration_pm_vindex_high = kpm_v_slow_lv3; |
|||
m_liquid_info[0].aspiration_volume_break_val = 500; // 500(x100nl) == 50ul
|
|||
m_liquid_info[0].volume_calibration_coefficient_b = 1.0f; |
|||
m_liquid_info[0].volume_calibration_coefficient_k = 1.0f; |
|||
m_liquid_info[0].settling_time = 1000; // ms
|
|||
m_liquid_info[0].transport_volume = 50; // 50(x100nl) == 5ul
|
|||
m_liquid_info[0].transport_volume_pm_vcpyid = kpm_v_slow_lv2; |
|||
m_liquid_info[0].mix_pm_vcpyid = kpm_v_max; |
|||
m_liquid_info[0].mark = 9973; |
|||
} |
|||
@ -0,0 +1,124 @@ |
|||
#include <stdarg.h>
|
|||
|
|||
#include "pipette_ctrl_module.hpp"
|
|||
#include "sdk\components\zcancmder\protocol_event_bus_sender.hpp"
|
|||
//
|
|||
#include "base/pipette_marco_utils.hpp"
|
|||
#include "sdk/chip/exhal/stm32_exhal_uart.hpp"
|
|||
#include "sdk\components\exception\zapp_exception.hpp"
|
|||
#include "sdk\components\exception\zapp_thread_stoped_exception.hpp"
|
|||
#define TAG "PipetteModule"
|
|||
|
|||
using namespace iflytop; |
|||
|
|||
void PipetteModule::pump_get_vcfg(int32_t vcfgcpyid, smtp2::VelCfg *cfg) { |
|||
pm_vcfg_t cfg2 = {0}; |
|||
get_cfg_smart(vcfgcpyid, &cfg2); |
|||
cfg->acc = cfg2.acc; |
|||
cfg->dec = cfg2.dec; |
|||
cfg->vstart = cfg2.vstart; |
|||
cfg->vstop = cfg2.vstop; |
|||
cfg->vmax = cfg2.vmax; |
|||
} |
|||
|
|||
void PipetteModule::pump_apply_vcfg(int32_t vcfgcpyid, bool force) { |
|||
ZLOGI(TAG, "pump_apply_vcfg %s", get_pm_vcpyid_name((pm_vcpyid_t)vcfgcpyid)); |
|||
|
|||
smtp2::VelCfg vcfg; |
|||
pump_get_vcfg(vcfgcpyid, &vcfg); |
|||
|
|||
DO_IN_THREAD(m_smtp2.pump_set_vcfg(&vcfg)); |
|||
} |
|||
void PipetteModule::_pump_move_to_x100nl(int32_t x100nl, int32_t vcfgcpyid) { |
|||
ZLOGI(TAG, "pump_move_to_x100nl %d, in %s", x100nl, get_pm_vcpyid_name((pm_vcpyid_t)vcfgcpyid)); |
|||
smtp2::VelCfg vcfg; |
|||
pump_get_vcfg(vcfgcpyid, &vcfg); |
|||
DO_IN_THREAD(m_smtp2.pump_move_to_nl(&vcfg, x100nl * 100)); |
|||
} |
|||
void PipetteModule::pump_do_lld(int32_t pressure_threshold, int32_t ___, int32_t vcfgidx) { |
|||
ZLOGI(TAG, "pump_do_lld pressure_threshold %d, vcfgidx %d", pressure_threshold, vcfgidx); |
|||
smtp2::VelCfg vcfg; |
|||
pump_get_vcfg(vcfgidx, &vcfg); |
|||
DO_IN_THREAD(m_smtp2.pump_aspirate_plld(pressure_threshold, &vcfg)); |
|||
} |
|||
|
|||
void PipetteModule::pump_move_to_x100nl_block(double x100nl, int32_t vcfgcpyid, int32_t *diff) { |
|||
ZLOGI(TAG, "pump_move_to_x100nl_block %f nl, in %s", x100nl, get_pm_vcpyid_name((pm_vcpyid_t)vcfgcpyid)); |
|||
|
|||
int32_t diff_pos = 0; |
|||
for (size_t i = 0; i < 3; i++) { |
|||
int32_t vcpyid = vcfgcpyid; |
|||
if (i != 0) { |
|||
vcpyid = kpm_v_slow_lv1; |
|||
} |
|||
|
|||
_pump_move_to_x100nl(x100nl, vcpyid); |
|||
pump_waitfor_stop(); |
|||
int32_t targetpos = x100nl * 100; |
|||
int32_t after_pos = pump_read_pos_nl(); |
|||
diff_pos = targetpos - after_pos; |
|||
if (diff) *diff = diff_pos; |
|||
ZLOGI(TAG, "pump target %d nl, now pos %d nl diff %d", targetpos, after_pos, diff_pos); |
|||
if (abs(diff_pos) > 20) { |
|||
ZLOGW(TAG, "pump move to x100nl %d failed, diff is too large retrying...", x100nl); |
|||
continue; |
|||
} |
|||
break; |
|||
} |
|||
if (abs(diff_pos) > 30) { |
|||
module_detail_errorcode = diff_pos; |
|||
throw zapp_exception(err::kpipette_pm_positioning_abnormality); |
|||
} |
|||
} |
|||
void PipetteModule::pump_move_by_x100nl_block(double x100nl, int32_t vcfgcpyid) { |
|||
int32_t startpos = pump_read_pos_nl(); |
|||
if (startpos > 1500000) { |
|||
// 参考 https://iflytop1.feishu.cn/wiki/IWn1waKCXiUvzfkOru5c0F3HnJf
|
|||
ZLOGW(TAG, "pump position is too large maybe overflow, startpos=%d", startpos); |
|||
startpos = 0; |
|||
} |
|||
double targetpos = startpos + x100nl * 100; |
|||
pump_move_to_x100nl_block(targetpos / 100, vcfgcpyid); |
|||
} |
|||
|
|||
void PipetteModule::pump_read_pos_nl(int32_t *val) { |
|||
//
|
|||
// *val = m_state.load_val_ul;
|
|||
// *val = pum
|
|||
// pump_read_pos_nl(val);
|
|||
DO_IN_THREAD(m_smtp2.pump_get_nl(val)); |
|||
} |
|||
int32_t PipetteModule::pump_read_pos_nl() { |
|||
int32_t val = 0; |
|||
DO_IN_THREAD(m_smtp2.pump_get_nl(&val)); |
|||
return val; |
|||
} |
|||
int32_t PipetteModule::pump_read_capactitance() { |
|||
int32_t val = 0; |
|||
DO_IN_THREAD(m_smtp2.pump_get_capacitance(&val)); |
|||
return val; |
|||
} |
|||
int32_t PipetteModule::pump_read_tip_state() { |
|||
int32_t val = 0; |
|||
DO_IN_THREAD(m_smtp2.pump_get_tip_state(&val)); |
|||
return val; |
|||
} |
|||
|
|||
|
|||
void PipetteModule::pump_waitfor_stop() { |
|||
while (true) { |
|||
int32_t isbusy = 0; |
|||
DO_IN_THREAD(m_smtp2.pump_get_state(&isbusy)); |
|||
if (isbusy == 0) break; |
|||
thread_delay(10); |
|||
} |
|||
} |
|||
|
|||
void PipetteModule::pump_waitfor_lld_is_ready(int32_t *zpos) { |
|||
m_smtp2.pump_set_io1_mode(0); // lld输入高
|
|||
while (true) { |
|||
int32_t isbusy = 0; |
|||
if (m_piette_gun_io1.getState()) { |
|||
} |
|||
} |
|||
} |
|||
@ -0,0 +1,116 @@ |
|||
#include "pipette_ctrl_module.hpp"
|
|||
|
|||
#include <stdarg.h>
|
|||
|
|||
#include "sdk\components\zcancmder\protocol_event_bus_sender.hpp"
|
|||
//
|
|||
#include "base/pipette_marco_utils.hpp"
|
|||
#include "sdk/chip/exhal/stm32_exhal_uart.hpp"
|
|||
#include "sdk\components\exception\zapp_exception.hpp"
|
|||
#include "sdk\components\exception\zapp_thread_stoped_exception.hpp"
|
|||
#define TAG "PipetteModule"
|
|||
|
|||
using namespace iflytop; |
|||
int32_t PipetteModule::pipette_test_pump_move_to_x100nl(int32_t x100nl, int32_t vcfgcpyid) { |
|||
thread_start_work(__FUNCTION__, [this, x100nl, vcfgcpyid]() { |
|||
ZLOGI(TAG, "pipette_pump_aspirate"); |
|||
if (pump_read_tip_state() == 0) { |
|||
throw zapp_exception(err::kpipette_error_TipDrop); |
|||
} |
|||
|
|||
// 移液枪部分参数初始化
|
|||
DO_IN_THREAD(m_smtp2.pump_enable_temp_compensation(0)); // 关闭温度补偿
|
|||
DO_IN_THREAD(m_smtp2.pump_set_back_clearance(0)); // 设置背隙为0
|
|||
DO_IN_THREAD(m_smtp2.pump_set_io1_mode(0)); // lld输入高
|
|||
|
|||
int32_t diff = 0; |
|||
pump_move_to_x100nl_block(x100nl, vcfgcpyid, &diff); //
|
|||
m_state.asynchronous_result0 = diff; |
|||
}); |
|||
return 0; |
|||
} |
|||
int32_t PipetteModule::pipette_test_lld(int32_t container_pos, int32_t container_cpyid, int32_t liquid_cpyid) { |
|||
thread_start_work(__FUNCTION__, [this, container_pos, container_cpyid, liquid_cpyid]() { |
|||
ZLOGI(TAG, "pipette_pump_aspirate"); |
|||
if (pump_read_tip_state() == 0) { |
|||
throw zapp_exception(err::kpipette_error_TipDrop); |
|||
} |
|||
|
|||
// 移液枪部分参数初始化
|
|||
DO_IN_THREAD(m_smtp2.pump_enable_temp_compensation(0)); // 关闭温度补偿
|
|||
DO_IN_THREAD(m_smtp2.pump_set_back_clearance(0)); // 设置背隙为0
|
|||
DO_IN_THREAD(m_smtp2.pump_set_io1_mode(0)); // lld输入高
|
|||
|
|||
platinfo_t *platform_info = get_platinfo_smart(m_state.platinfo_cpyid, &m_now_platinfo); |
|||
liquid_info_t *liquidinfo = get_liquid_info_smart(container_cpyid, &m_now_liquid_info); |
|||
container_info_t *container_cfg = get_container_info_smart(liquid_cpyid, &m_now_container_info); |
|||
|
|||
_do_lld(container_pos, platform_info, container_cfg, liquidinfo); |
|||
}); |
|||
return 0; |
|||
} |
|||
|
|||
int32_t PipetteModule::pipette_test_move_to_container_bottom(int32_t container_pos, int32_t container_cpyid) { |
|||
thread_start_work(__FUNCTION__, [this, container_pos, container_cpyid]() { |
|||
container_info_t *container_info = get_container_info_smart(container_cpyid, &m_now_container_info); |
|||
check_container_info_cpyid(container_cpyid); |
|||
zm_move_to_container_bottom_block(container_pos, container_info, kzm_v_default, 0); // 移动到容器底部
|
|||
}); |
|||
return 0; |
|||
} |
|||
int32_t PipetteModule::pipette_test_move_to_container_bottom_section_begin(int32_t container_pos, int32_t container_cpyid) { |
|||
thread_start_work(__FUNCTION__, [this, container_pos, container_cpyid]() { |
|||
container_info_t *container_info = get_container_info_smart(container_cpyid, &m_now_container_info); |
|||
check_container_info_cpyid(container_cpyid); |
|||
zm_move_to_container_bottom_section_begin_block(container_pos, container_info, kzm_v_default, 0); // 移动到容器底部
|
|||
}); |
|||
return 0; |
|||
} |
|||
int32_t PipetteModule::pipette_test_move_to_immersion_pos(int32_t container_pos, int32_t container_cpyid) { |
|||
thread_start_work(__FUNCTION__, [this, container_pos, container_cpyid]() { |
|||
container_info_t *container_info = get_container_info_smart(container_cpyid, &m_now_container_info); |
|||
check_container_info_cpyid(container_cpyid); |
|||
zm_move_to_immersion_pos_block(container_pos, -1, container_info, kzm_v_default, 0); // 移动到液面下
|
|||
}); |
|||
return 0; |
|||
} |
|||
int32_t PipetteModule::pipette_test_move_to_leaving_height_pos(int32_t container_pos, int32_t container_cpyid) { |
|||
thread_start_work(__FUNCTION__, [this, container_pos, container_cpyid]() { |
|||
container_info_t *container_info = get_container_info_smart(container_cpyid, &m_now_container_info); |
|||
check_container_info_cpyid(container_cpyid); |
|||
zm_move_to_leaving_height_pos_block(container_pos, -1, container_info, kzm_v_default, 0); // 移动到液面上方
|
|||
}); |
|||
return 0; |
|||
} |
|||
int32_t PipetteModule::pipette_test_move_to_jet_pos(int32_t container_pos, int32_t container_cpyid) { |
|||
thread_start_work(__FUNCTION__, [this, container_pos, container_cpyid]() { |
|||
container_info_t *container_info = get_container_info_smart(container_cpyid, &m_now_container_info); |
|||
check_container_info_cpyid(container_cpyid); |
|||
zm_move_to_jet_pos_block(container_pos, -1, container_info, kzm_v_default, 0); // 移动到jet位置
|
|||
}); |
|||
return 0; |
|||
} |
|||
int32_t PipetteModule::pipette_test_move_to_lld_start_search_pos(int32_t container_pos, int32_t container_cpyid) { |
|||
thread_start_work(__FUNCTION__, [this, container_pos, container_cpyid]() { |
|||
container_info_t *container_info = get_container_info_smart(container_cpyid, &m_now_container_info); |
|||
check_container_info_cpyid(container_cpyid); |
|||
zm_move_to_lld_start_search_pos_block(container_pos, container_info, kzm_v_default, 0); // 移动到lld开始搜索位置
|
|||
}); |
|||
return 0; |
|||
} |
|||
int32_t PipetteModule::pipette_test_move_to_fix_water_level_pos(int32_t container_pos, int32_t container_cpyid) { |
|||
thread_start_work(__FUNCTION__, [this, container_pos, container_cpyid]() { |
|||
container_info_t *container_info = get_container_info_smart(container_cpyid, &m_now_container_info); |
|||
check_container_info_cpyid(container_cpyid); |
|||
zm_move_to_fix_water_level_pos_block(container_pos, container_info, kzm_v_default, 0); // 移动到固定水位位置
|
|||
}); |
|||
return 0; |
|||
} |
|||
int32_t PipetteModule::pipette_test_move_to_pierce_pos(int32_t container_pos, int32_t container_cpyid) { |
|||
thread_start_work(__FUNCTION__, [this, container_pos, container_cpyid]() { |
|||
container_info_t *container_info = get_container_info_smart(container_cpyid, &m_now_container_info); |
|||
check_container_info_cpyid(container_cpyid); |
|||
zm_move_to_pierce_pos_block(container_pos, container_info, kzm_v_default, 0); // 移动到穿刺位置
|
|||
}); |
|||
return 0; |
|||
} |
|||
@ -0,0 +1,194 @@ |
|||
#include <stdarg.h>
|
|||
|
|||
#include "pipette_ctrl_module.hpp"
|
|||
#include "sdk\components\zcancmder\protocol_event_bus_sender.hpp"
|
|||
//
|
|||
#include "base/pipette_marco_utils.hpp"
|
|||
#include "sdk/chip/exhal/stm32_exhal_uart.hpp"
|
|||
#include "sdk\components\exception\zapp_exception.hpp"
|
|||
#include "sdk\components\exception\zapp_thread_stoped_exception.hpp"
|
|||
#define TAG "PipetteModule"
|
|||
|
|||
using namespace iflytop; |
|||
|
|||
void PipetteModule::assign_cfg_smart(int32_t *dest, int32_t *src, int32_t *defaultcfg, int32_t num) { |
|||
for (int i = 0; i < num; i++) { |
|||
if (src[i] >= 0) { |
|||
dest[i] = src[i]; |
|||
} else { |
|||
dest[i] = defaultcfg[i]; |
|||
} |
|||
} |
|||
} |
|||
void PipetteModule::memset_int32_t(int32_t *table, int32_t val, int32_t num) { |
|||
for (int i = 0; i < num; i++) { |
|||
table[i] = val; |
|||
} |
|||
} |
|||
|
|||
void PipetteModule::check_platinfo_cpyid(int32_t cpyid) { |
|||
if (cpyid < 0 || cpyid >= ARRARY_SIZE(m_platinfo)) { |
|||
ZLOGE(TAG, "pipette_get_container_info cpyid %d out of range", cpyid); |
|||
throw zapp_exception(err::kparam_out_of_range); |
|||
} |
|||
} |
|||
void PipetteModule::check_zmvcfg_cpyid(int32_t cpyid) { |
|||
if (cpyid < 0 || cpyid >= ARRARY_SIZE(m_zm_vcfg)) { |
|||
ZLOGE(TAG, "pipette_get_zmvcfg cpyid %d out of range", cpyid); |
|||
throw zapp_exception(err::kparam_out_of_range); |
|||
} |
|||
} |
|||
void PipetteModule::check_pmvcfg_cpyid(int32_t cpyid) { |
|||
if (cpyid < 0 || cpyid >= ARRARY_SIZE(m_pm_vcfg)) { |
|||
ZLOGE(TAG, "pipette_get_pmvcfg cpyid %d out of range", cpyid); |
|||
throw zapp_exception(err::kparam_out_of_range); |
|||
} |
|||
} |
|||
void PipetteModule::check_container_info_cpyid(int32_t cpyid) { |
|||
if (cpyid < 0 || cpyid >= ARRARY_SIZE(m_container_info)) { |
|||
ZLOGE(TAG, "pipette_get_container_info cpyid %d out of range", cpyid); |
|||
throw zapp_exception(err::kparam_out_of_range); |
|||
} |
|||
} |
|||
void PipetteModule::check_liquid_info_cpyid(int32_t cpyid) { |
|||
if (cpyid < 0 || cpyid >= ARRARY_SIZE(m_liquid_info)) { |
|||
ZLOGE(TAG, "pipette_get_liquid_info cpyid %d out of range", cpyid); |
|||
throw zapp_exception(err::kparam_out_of_range); |
|||
} |
|||
} |
|||
void PipetteModule::check_zmotor_is_enable() { |
|||
if (!m_zm->isMotorIcInitOk()) { |
|||
ZLOGE(TAG, "motor is not init"); |
|||
throw zapp_exception(err::kstep_motor_subic_init_fail); |
|||
} |
|||
if (!m_zm->isOnline()) { |
|||
ZLOGE(TAG, "motor is offline"); |
|||
throw zapp_exception(err::kstep_motor_subic_offline); |
|||
} |
|||
if (!m_state.enable) { |
|||
ZLOGE(TAG, "motor is not enable"); |
|||
throw zapp_exception(err::kstep_motor_not_enable); |
|||
} |
|||
} |
|||
|
|||
void PipetteModule::check_pipette_pump_aspirate_params() { |
|||
aspiration_param_t *acfg = &m_aspiration_param; |
|||
|
|||
if (acfg->volumeX100nl < 0) { |
|||
ZLOGE(TAG, "pipette_pump_aspirate_set_param x100nl %d out of range", acfg->volumeX100nl); |
|||
throw zapp_exception(err::kparam_out_of_range); |
|||
} |
|||
check_platinfo_cpyid(m_state.platinfo_cpyid); |
|||
check_container_info_cpyid(acfg->container_info_index); |
|||
check_liquid_info_cpyid(acfg->liquid_cfg_index); |
|||
} |
|||
|
|||
int32_t PipetteModule::compute_zm_llf_vel(int32_t pm_vindex, container_info_t *container_info) { |
|||
pm_vcfg_t pmv = {0}; |
|||
get_cfg_smart(pm_vindex, &pmv); |
|||
int32_t zmval = pmv.vmax * container_info->llf_vconvert_coneff * 0.001; |
|||
if (zmval == 0) zmval = 1; |
|||
return zmval; |
|||
} |
|||
|
|||
void PipetteModule::push_pressure_sample_data(int32_t motor_pos, int32_t pressure_val) { |
|||
if (capturedata_num >= PIPETTEMODULE_SAMPLE_BUF_SIZE) { |
|||
ZLOGW(TAG, "capturedata_buf overflow"); |
|||
return; |
|||
} |
|||
|
|||
ZLOGI(TAG, "sample %2d mpos:%2d pressure_val:%4d", capturedata_num, motor_pos, pressure_val); |
|||
capturedata_buf[capturedata_num].motor_pos = motor_pos; |
|||
capturedata_buf[capturedata_num].pressure_val = pressure_val; |
|||
capturedata_num++; |
|||
} |
|||
|
|||
void PipetteModule::dump(const char *title, platinfo_t *info) { |
|||
ZLOGI(TAG, "-------------------------- %s --------------------------", title); |
|||
ZLOGI(TAG, "work_ref_pos: %d", info->work_ref_pos); |
|||
ZLOGI(TAG, "tip_picking_pos: %d", info->tip_picking_pos); |
|||
ZLOGI(TAG, "tip_picking_search_range: %d", info->tip_picking_search_range); |
|||
ZLOGI(TAG, "tip_picking_append_distance: %d", info->tip_picking_append_distance); |
|||
ZLOGI(TAG, "tip_deposit_pos: %d", info->tip_deposit_pos); |
|||
ZLOGI(TAG, "_transform_pos: %d", info->_transform_pos); |
|||
ZLOGI(TAG, "tip_type: %d", info->tip_type); |
|||
ZLOGI(TAG, "tip_length: %d", info->tip_length); |
|||
ZLOGI(TAG, "mark: %d", info->mark); |
|||
ZLOGI(TAG, "-") |
|||
} |
|||
void PipetteModule::dump(const char *title, liquid_info_t *liquidinfo) { |
|||
ZLOGI(TAG, "-------------------------- %s --------------------------", title); |
|||
ZLOGI(TAG, "- plld_pm_vindex :%d", liquidinfo->plld_pm_vcpyid); |
|||
ZLOGI(TAG, "- plld_threshold :%d", liquidinfo->plld_threshold); |
|||
ZLOGI(TAG, "- plld_zm_vel :%d", liquidinfo->plld_zm_vel); |
|||
ZLOGI(TAG, "- empty_tip_pm_vindex :%d", liquidinfo->empty_tip_pm_vcpyid); |
|||
ZLOGI(TAG, "- blowout_air_volume :%d", liquidinfo->blowout_air_volume); |
|||
ZLOGI(TAG, "- blowout_air_pm_vindex :%d", liquidinfo->blowout_air_pm_vcpyid); |
|||
ZLOGI(TAG, "- over_aspirated_volume :%d", liquidinfo->over_aspirated_volume); |
|||
ZLOGI(TAG, "- over_aspirated_pm_vindex :%d", liquidinfo->over_aspirated_pm_vcpyid); |
|||
ZLOGI(TAG, "- aspiration_pm_vindex_low :%d", liquidinfo->aspiration_pm_vindex_low); |
|||
ZLOGI(TAG, "- aspiration_pm_vindex_high :%d", liquidinfo->aspiration_pm_vindex_high); |
|||
ZLOGI(TAG, "- aspiration_volume_break_val :%d", liquidinfo->aspiration_volume_break_val); |
|||
ZLOGI(TAG, "- volume_calibration_coefficient_b :%d", liquidinfo->volume_calibration_coefficient_b); |
|||
ZLOGI(TAG, "- volume_calibration_coefficient_k :%d", liquidinfo->volume_calibration_coefficient_k); |
|||
ZLOGI(TAG, "- settling_time :%d", liquidinfo->settling_time); |
|||
ZLOGI(TAG, "- transport_volume :%d", liquidinfo->transport_volume); |
|||
ZLOGI(TAG, "- transport_volume_pm_vindex :%d", liquidinfo->transport_volume_pm_vcpyid); |
|||
ZLOGI(TAG, "- mix_pm_vindex :%d", liquidinfo->mix_pm_vcpyid); |
|||
ZLOGI(TAG, "- mark :%d", liquidinfo->mark); |
|||
ZLOGI(TAG, "-"); |
|||
} |
|||
void PipetteModule::dump(const char *title, container_info_t *container_cfg) { |
|||
ZLOGI(TAG, "-------------------------- %s --------------------------", title); |
|||
ZLOGI(TAG, "- container_type :%d", container_cfg->container_type); |
|||
ZLOGI(TAG, "- container_neck_pos :%d", container_cfg->_container_neck_pos); |
|||
ZLOGI(TAG, "- container_depth :%d", container_cfg->container_depth); |
|||
ZLOGI(TAG, "- container_round :%d", container_cfg->container_round); |
|||
ZLOGI(TAG, "- container_bottom_section_height :%d", container_cfg->container_bottom_section_height); |
|||
ZLOGI(TAG, "- immersion_depth :%d", container_cfg->immersion_depth); |
|||
ZLOGI(TAG, "- leaving_height :%d", container_cfg->leaving_height); |
|||
ZLOGI(TAG, "- jet_height :%d", container_cfg->jet_height); |
|||
ZLOGI(TAG, "- lld_start_search_depth :%d", container_cfg->lld_start_search_depth); |
|||
ZLOGI(TAG, "- fix_water_level_depth :%d", container_cfg->fix_water_level_depth); |
|||
ZLOGI(TAG, "- llf_vconvert_coneff :%d", container_cfg->llf_vconvert_coneff); |
|||
ZLOGI(TAG, "- pierce_depth :%d", container_cfg->pierce_depth); |
|||
ZLOGI(TAG, "- mark :%d", container_cfg->mark); |
|||
ZLOGI(TAG, "-"); |
|||
} |
|||
|
|||
void PipetteModule::dump(const char *title, aspiration_param_t *param) { |
|||
ZLOGI(TAG, "-------------------------- %s --------------------------", title); |
|||
ZLOGI(TAG, "- volumeX100nl: %d", param->volumeX100nl); |
|||
ZLOGI(TAG, "- container_pos: %d", param->container_pos); |
|||
ZLOGI(TAG, "- container_info_index: %d", param->container_info_index); |
|||
ZLOGI(TAG, "- liquid_cfg_index: %d", param->liquid_cfg_index); |
|||
ZLOGI(TAG, "- aspiration_mode: %d", param->aspiration_mode); |
|||
ZLOGI(TAG, "- lld_enable: %d", param->lld_enable); |
|||
ZLOGI(TAG, "- lld_type: %d", param->lld_type); |
|||
ZLOGI(TAG, "- lld_enable_protect: %d", param->lld_enable_protect); |
|||
ZLOGI(TAG, "- mix_llf_enable: %d", param->mix_llf_enable); |
|||
ZLOGI(TAG, "- llf_enable: %d", param->llf_enable); |
|||
ZLOGI(TAG, "- mix_volume: %d", param->mix_volume); |
|||
ZLOGI(TAG, "- mix_times: %d", param->mix_times); |
|||
ZLOGI(TAG, "- max: %d", param->max); |
|||
ZLOGI(TAG, "-"); |
|||
} |
|||
void PipetteModule::dump(const char *title, distribu_all_param_t *param) { |
|||
ZLOGI(TAG, "-------------------------- %s --------------------------", title); |
|||
ZLOGI(TAG, "- container_pos: %d", param->container_pos); |
|||
ZLOGI(TAG, "- dest_container_cpyid: %d", param->dest_container_cpyid); |
|||
ZLOGI(TAG, "- dest_container_is_empty: %d", param->dest_container_is_empty); |
|||
ZLOGI(TAG, "- dest_liquid_cfg_index: %d", param->dest_liquid_cfg_index); |
|||
ZLOGI(TAG, "- distribu_type: %d", param->distribu_type); |
|||
ZLOGI(TAG, "- mix_volume: %d", param->mix_volume); |
|||
ZLOGI(TAG, "- mix_times: %d", param->mix_times); |
|||
ZLOGI(TAG, "- zm_auto_move_to_zero: %d", param->zm_auto_move_to_zero); |
|||
ZLOGI(TAG, "- max: %d", param->max); |
|||
ZLOGI(TAG, "-"); |
|||
} |
|||
|
|||
int32_t PipetteModule::calibrate_liquid_volume(liquid_info_t *liquidinfo, int32_t volumex100nl) { |
|||
int32_t x100nl = volumex100nl * liquidinfo->volume_calibration_coefficient_k * 0.001 + //
|
|||
liquidinfo->volume_calibration_coefficient_b * 0.001; |
|||
return x100nl; |
|||
} |
|||
@ -0,0 +1,242 @@ |
|||
#include <stdarg.h>
|
|||
|
|||
#include "pipette_ctrl_module.hpp"
|
|||
#include "sdk\components\zcancmder\protocol_event_bus_sender.hpp"
|
|||
//
|
|||
#include "base/pipette_marco_utils.hpp"
|
|||
#include "sdk/chip/exhal/stm32_exhal_uart.hpp"
|
|||
#include "sdk\components\exception\zapp_exception.hpp"
|
|||
#include "sdk\components\exception\zapp_thread_stoped_exception.hpp"
|
|||
#define TAG "PipetteModule"
|
|||
|
|||
using namespace iflytop; |
|||
|
|||
void PipetteModule::zm_move_to_container_bottom_block(int32_t container_pos, container_info_t *containInfo, int32_t vbcpyid, int32_t vel) { //
|
|||
zm_move_to_block(container_pos + containInfo->container_depth, vbcpyid, vel); |
|||
} |
|||
void PipetteModule::zm_move_to_container_bottom_section_begin_block(int32_t container_pos, container_info_t *containInfo, int32_t vbcpyid, int32_t vel) { //
|
|||
zm_move_to_block(container_pos + containInfo->container_depth - containInfo->container_bottom_section_height, vbcpyid, vel); |
|||
} |
|||
void PipetteModule::zm_move_to_immersion_pos_block(int32_t container_pos, int32_t waterlevel, container_info_t *containInfo, int32_t vbcpyid, int32_t vel) { //
|
|||
if (waterlevel == -1) |
|||
zm_move_to_block(container_pos + containInfo->fix_water_level_depth + containInfo->immersion_depth, vbcpyid, vel); |
|||
else |
|||
zm_move_to_block(waterlevel + containInfo->immersion_depth, vbcpyid, vel); |
|||
} |
|||
void PipetteModule::zm_move_to_leaving_height_pos_block(int32_t container_pos, int32_t waterlevel, container_info_t *containInfo, int32_t vbcpyid, int32_t vel) { //
|
|||
if (waterlevel == -1) |
|||
zm_move_to_block(container_pos + containInfo->fix_water_level_depth - containInfo->leaving_height, vbcpyid, vel); |
|||
else |
|||
zm_move_to_block(waterlevel - containInfo->leaving_height, vbcpyid, vel); |
|||
} |
|||
void PipetteModule::zm_move_to_jet_pos_block(int32_t container_pos, int32_t waterlevel, container_info_t *containInfo, int32_t vbcpyid, int32_t vel) { //
|
|||
if (waterlevel == -1) |
|||
zm_move_to_block(container_pos + containInfo->fix_water_level_depth - containInfo->jet_height, vbcpyid, vel); |
|||
else |
|||
zm_move_to_block(waterlevel - containInfo->jet_height, vbcpyid, vel); |
|||
} |
|||
void PipetteModule::zm_move_to_lld_start_search_pos_block(int32_t container_pos, container_info_t *containInfo, int32_t vbcpyid, int32_t vel) { //
|
|||
zm_move_to_block(container_pos + containInfo->lld_start_search_depth, vbcpyid, vel); |
|||
} |
|||
void PipetteModule::zm_move_to_fix_water_level_pos_block(int32_t container_pos, container_info_t *containInfo, int32_t vbcpyid, int32_t vel) { //
|
|||
zm_move_to_block(container_pos + containInfo->fix_water_level_depth, vbcpyid, vel); |
|||
} |
|||
void PipetteModule::zm_move_to_pierce_pos_block(int32_t container_pos, container_info_t *containInfo, int32_t vbcpyid, int32_t vel) { //
|
|||
zm_move_to_block(container_pos + containInfo->pierce_depth, vbcpyid, vel); |
|||
} |
|||
|
|||
void PipetteModule::do_zm_move_0p() { |
|||
bool is_trigger = false; |
|||
if (!zm0p_is_trigger()) { |
|||
ZLOGI(TAG, "move to zero"); |
|||
zm_move_to_end(-1, kzm_v_move_to_zero); |
|||
zm_waitfor_zm0p_trigger_and_stop_motor(&is_trigger); |
|||
if (!is_trigger) throw zapp_exception(err::kstep_motor_not_found_point_edge); |
|||
} |
|||
|
|||
// leave from zero
|
|||
if (zm0p_is_trigger()) { |
|||
ZLOGI(TAG, "leave from zero"); |
|||
zm_move_by(500 /*50mm */, kzm_v_look_zero_edge, 0); |
|||
zm_waitfor_zm0p_not_trigger_and_stop_motor(&is_trigger); |
|||
if (!is_trigger) throw zapp_exception(err::kstep_motor_not_found_point_edge); |
|||
} |
|||
|
|||
if (!zm0p_is_trigger()) { |
|||
ZLOGI(TAG, "move to zero edge again"); |
|||
zm_move_by(-100 /*10mm */, kzm_v_look_zero_edge, 0); |
|||
zm_waitfor_zm0p_trigger_and_stop_motor(&is_trigger); |
|||
if (!is_trigger) throw zapp_exception(err::kstep_motor_not_found_point_edge); |
|||
} |
|||
zm_waitfor_stop(); |
|||
} |
|||
|
|||
int32_t PipetteModule::zm_get_now_pos() { |
|||
if (zmbcfg.enable_enc == 0) { |
|||
return m_zm->getXACTUAL(); |
|||
} |
|||
return m_zm->read_enc_val(); |
|||
} |
|||
void PipetteModule::zm_set_now_pos(int32_t x) { |
|||
m_zm->setXACTUAL(x); |
|||
m_zm->set_enc_val(x); |
|||
} |
|||
void PipetteModule::zm_move_to_zero_quick_block() { |
|||
// 期望偏差
|
|||
int32_t expectation_dpos = -zmbcfg.io_trigger_append_distance + 0 + zmbcfg.dzero - zm_get_now_pos(); |
|||
int32_t startpos = zm_get_now_pos(); |
|||
if (!zm0p_is_trigger()) { |
|||
// 快速移动到零点
|
|||
// moveTo(0 + zmbcfg.dzero, zmbcfg.default_velocity);
|
|||
zm_move_to(0 + zmbcfg.dzero, kzm_v_default, 0); |
|||
zm_waitfor_zm0p_trigger_and_stop_motor(NULL); |
|||
} else { |
|||
// 快速移动到零点
|
|||
zm_move_to(0 + zmbcfg.dzero, kzm_v_default, 0); |
|||
zm_waitfor_zm0p_not_trigger_and_stop_motor(NULL); |
|||
} |
|||
// 移动到零点
|
|||
do_zm_move_0p(); |
|||
|
|||
// 校验偏差
|
|||
int32_t dpos = zm_get_now_pos() - startpos; |
|||
// 设置零点
|
|||
zm_set_now_pos(0 + zmbcfg.dzero - zmbcfg.io_trigger_append_distance); |
|||
m_state.zm_has_move_zero = 1; |
|||
|
|||
if (zmbcfg.pos_devi_tolerance != 0 && (abs(expectation_dpos - dpos) > zmbcfg.pos_devi_tolerance)) { |
|||
throw zapp_exception(err::kstep_motor_lost_step); |
|||
} |
|||
} |
|||
|
|||
void PipetteModule::zm_move_to(int32_t x, int32_t vbaseindex, int32_t vel) { |
|||
/**
|
|||
* @brief 以电机的参考位置为基准，向下为正向，向上为负向 |
|||
*/ |
|||
ZLOGI(TAG, "zm_move_to %d", x); |
|||
if (zmbcfg.enable_enc != 0) { |
|||
m_zm->setXACTUAL(m_zm->read_enc_val()); |
|||
} |
|||
|
|||
zm_apply_vcfg(vbaseindex, vel); |
|||
m_zm->moveTo(x); |
|||
} |
|||
void PipetteModule::zm_move_to_block(int32_t x, int32_t vbaseindex, int32_t vel) { |
|||
zm_move_to(x, vbaseindex, vel); |
|||
zm_waitfor_stop(); |
|||
} |
|||
void PipetteModule::zm_move_by(int32_t dx, int32_t vindex, int32_t vel) { |
|||
ZLOGI(TAG, "zm_move_by %d %d", dx); |
|||
if (zmbcfg.enable_enc != 0) { |
|||
m_zm->setXACTUAL(m_zm->read_enc_val()); |
|||
} |
|||
zm_apply_vcfg(vindex, vel); |
|||
m_zm->moveBy(dx); |
|||
} |
|||
void PipetteModule::zm_move_by_block(int32_t dx, int32_t vindex, int32_t vel) { |
|||
zm_move_by(dx, vindex, vel); |
|||
zm_waitfor_stop(); |
|||
} |
|||
|
|||
void PipetteModule::zm_move_to_end(int32_t direction, int32_t vindex) { |
|||
if (zmbcfg.enable_enc != 0) { |
|||
m_zm->setXACTUAL(m_zm->read_enc_val()); |
|||
} |
|||
zm_apply_vcfg(vindex, 0); |
|||
m_zm->moveToEnd(direction); |
|||
} |
|||
|
|||
void PipetteModule::zm_update_dzero(int32_t dzero) { |
|||
int32_t nowabs = zm_get_now_pos() - zmbcfg.dzero; |
|||
zmbcfg.dzero = dzero; |
|||
zm_set_now_pos(nowabs + zmbcfg.dzero); |
|||
} |
|||
bool PipetteModule::zm0p_is_trigger() { return m_zm0p->getState(); } |
|||
|
|||
void PipetteModule::zm_apply_vcfg(int32_t vbasecfgindex, int32_t vel) { |
|||
ZLOGI(TAG, "zm_apply_vcfg %s", get_zm_vcpyid_name((zm_vcpyid_t)vbasecfgindex)); |
|||
/**
|
|||
* @brief 当配置项的值为负数时，使用默认配置项的值 |
|||
*/ |
|||
|
|||
zm_vcfg_t cfg = {0}; |
|||
get_cfg_smart(vbasecfgindex, &cfg); |
|||
if (vel > 0) { |
|||
cfg.vmax = vel; |
|||
} |
|||
TMC51X0::VCfg_t tcmvcfg; |
|||
tcmvcfg.vstart = cfg.vstart; |
|||
tcmvcfg.a1 = cfg.a1; |
|||
tcmvcfg.amax = cfg.amax; |
|||
tcmvcfg.v1 = cfg.v1; |
|||
tcmvcfg.dmax = cfg.dmax; |
|||
tcmvcfg.d1 = cfg.d1; |
|||
tcmvcfg.vstop = cfg.vstop; |
|||
tcmvcfg.vmax = cfg.vmax; |
|||
m_zm->set_vcfg(&tcmvcfg); |
|||
|
|||
ZLOGI(TAG, "zm set vstart %d,a1 %d,amax %d,v1 %d,dmax %d,d1 %d,vstop %d,vmax %d", tcmvcfg.vstart, tcmvcfg.a1, tcmvcfg.amax, tcmvcfg.v1, tcmvcfg.dmax, tcmvcfg.d1, tcmvcfg.vstop, tcmvcfg.vmax); |
|||
} |
|||
|
|||
void PipetteModule::zm_sync_base_cfg() { |
|||
m_zm->enable(false); |
|||
m_zm->setMRES((mres_type_t)zmbcfg.mres); |
|||
m_zm->setIHOLD_IRUN(zmbcfg.ihold, zmbcfg.irun, zmbcfg.iholddelay); |
|||
m_zm->setScale(zmbcfg.one_circle_pulse); |
|||
m_zm->setScaleDenominator(zmbcfg.one_circle_pulse_denominator); |
|||
m_zm->setMotorShaft(zmbcfg.shaft); |
|||
m_zm->setGlobalScale(zmbcfg.iglobalscaler); |
|||
m_zm->set_tzerowait(zmbcfg.tzerowait); |
|||
m_zm->set_enc_resolution(zmbcfg.enc_resolution); |
|||
zm_apply_vcfg(kzm_v_default, 0); |
|||
m_zm->stop(); |
|||
if (m_state.enable) { |
|||
m_zm->enable(true); |
|||
} |
|||
} |
|||
|
|||
void PipetteModule::zm_waitfor_zm0p_trigger_and_stop_motor(bool *triggerEdge) { |
|||
if (triggerEdge) *triggerEdge = true; |
|||
while (!zm0p_is_trigger()) { |
|||
if (m_zm->isStoped()) { |
|||
ZLOGI(TAG, "motor stop first....."); |
|||
if (triggerEdge) *triggerEdge = false; |
|||
break; |
|||
} |
|||
thread_delay(1); |
|||
} |
|||
|
|||
// 停止电机
|
|||
m_zm->stop(); |
|||
// 等待电机停止
|
|||
while (!m_zm->isStoped()) { |
|||
thread_delay(10); |
|||
} |
|||
} |
|||
|
|||
void PipetteModule::zm_waitfor_zm0p_not_trigger_and_stop_motor(bool *triggerEdge) { |
|||
// 等待触发
|
|||
if (triggerEdge) *triggerEdge = true; |
|||
while (zm0p_is_trigger()) { |
|||
if (m_zm->isStoped()) { |
|||
ZLOGI(TAG, "motor stop first....."); |
|||
if (triggerEdge) *triggerEdge = false; |
|||
} |
|||
thread_delay(1); |
|||
} |
|||
|
|||
// 停止电机
|
|||
m_zm->stop(); |
|||
// 等待电机停止
|
|||
while (!m_zm->isStoped()) thread_delay(2); |
|||
} |
|||
void PipetteModule::zm_stop() { |
|||
// 停止电机
|
|||
m_zm->stop(); |
|||
// 等待电机停止
|
|||
while (!m_zm->isStoped()) thread_delay(2); |
|||
} |
|||
|
|||
void PipetteModule::zm_waitfor_stop() { |
|||
while (!m_zm->isStoped()) thread_delay(1); |
|||
} |
|||
Write
Preview
Loading…
Cancel
Save
Reference in new issue