5 changed files with 204 additions and 180 deletions
-
6sdk/components/pipette_module/base/pipette_action_param.hpp
-
175sdk/components/pipette_module/pipette_ctrl_module.cpp
-
201sdk/components/pipette_module/pipette_ctrl_slice_impl.cpp
-
1usrc/a8000_protocol/protocol/cmdid.cpp
-
1usrc/a8000_protocol/protocol/cmdid.hpp
@ -0,0 +1,201 @@ |
|||
#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"
|
|||
/**
|
|||
* @brief 参数检查 |
|||
*/ |
|||
using namespace iflytop; |
|||
|
|||
int32_t PipetteModule::bfcall(int32_t cmdid, uint8_t *param, int32_t len) { |
|||
int32_t paramNum = len / 4; |
|||
int32_t *param32 = (int32_t *)param; |
|||
if (cmdid != kmodule_get_status) { |
|||
MODULE_BFCALL_DUMP_FN_CALL(cmdid, param32, paramNum); |
|||
} |
|||
|
|||
// 检查Z轴是否使能
|
|||
switch (cmdid) { |
|||
case kpipette_zmotor_move_zero: |
|||
case kpipette_zmotor_move_to_zero_point_quick: |
|||
case kpipette_zmotor_measure_distance: |
|||
case kpipette_zmotor_move_by: |
|||
case kpipette_zmotor_move_to: |
|||
case kpipette_pump_take_tip: |
|||
case kpipette_pump_deposit_tip: |
|||
case kpipette_pump_pierce_through: |
|||
case kpipette_pump_aspirate: |
|||
case kpipette_pump_distribu_all: |
|||
case kpipette_test_move_to_container_bottom: |
|||
case kpipette_test_move_to_container_bottom_section_begin: |
|||
case kpipette_test_move_to_immersion_pos: |
|||
case kpipette_test_move_to_leaving_height_pos: |
|||
case kpipette_test_move_to_jet_pos: |
|||
case kpipette_test_move_to_lld_start_search_pos: |
|||
case kpipette_test_move_to_fix_water_level_pos: |
|||
case kpipette_test_move_to_pierce_pos: |
|||
|
|||
if (!m_zm->isMotorIcInitOk()) { |
|||
ZLOGE(TAG, "motor is not init"); |
|||
return err::kstep_motor_subic_init_fail; |
|||
} |
|||
if (!m_zm->isOnline()) { |
|||
ZLOGE(TAG, "motor is offline"); |
|||
return err::kstep_motor_subic_offline; |
|||
} |
|||
if (!m_state.enable) { |
|||
ZLOGE(TAG, "motor is not enable"); |
|||
return err::kstep_motor_not_enable; |
|||
} |
|||
break; |
|||
default: |
|||
break; |
|||
} |
|||
// 检查是否归零
|
|||
switch (cmdid) { |
|||
case kpipette_zmotor_move_to_zero_point_quick: |
|||
case kpipette_zmotor_move_to: |
|||
case kpipette_pump_take_tip: |
|||
case kpipette_pump_deposit_tip: |
|||
case kpipette_pump_pierce_through: |
|||
case kpipette_pump_aspirate: |
|||
case kpipette_pump_distribu_all: |
|||
case kpipette_test_move_to_container_bottom: |
|||
case kpipette_test_move_to_container_bottom_section_begin: |
|||
case kpipette_test_move_to_immersion_pos: |
|||
case kpipette_test_move_to_leaving_height_pos: |
|||
case kpipette_test_move_to_jet_pos: |
|||
case kpipette_test_move_to_lld_start_search_pos: |
|||
case kpipette_test_move_to_fix_water_level_pos: |
|||
case kpipette_test_move_to_pierce_pos: |
|||
|
|||
if (!m_state.zm_has_move_zero) { |
|||
ZLOGE(TAG, "motor has not moved to zero"); |
|||
return err::kstep_motor_not_move_to_zero; |
|||
} |
|||
break; |
|||
default: |
|||
break; |
|||
} |
|||
|
|||
return 0; |
|||
} |
|||
void PipetteModule::aftercall(int32_t cmdid, uint8_t *param, int32_t len, uint8_t *ack, int32_t acklen, int32_t ret) { |
|||
if (cmdid == kmodule_get_status) return; |
|||
|
|||
if (ret == 0) { |
|||
ZLOGI(TAG, "<- call success", module_id); |
|||
} else { |
|||
ZLOGI(TAG, "<- call fail,%s(%d)", err::error2str(ret), ret); |
|||
} |
|||
} |
|||
|
|||
void PipetteModule::runingcheck() { |
|||
// 检查是否丢步
|
|||
if (zmbcfg.enable_enc != 0) { |
|||
vPortEnterCritical(); |
|||
|
|||
int32_t m1enc = m_zm->read_enc_val(); |
|||
int32_t m1pos = m_zm->getXACTUAL(); |
|||
|
|||
vPortExitCritical(); |
|||
|
|||
int32_t dm1 = abs(m1enc - m1pos); |
|||
|
|||
if (zmbcfg.pos_devi_tolerance != 0) { |
|||
if (dm1 > zmbcfg.pos_devi_tolerance) { |
|||
ZLOGE(TAG, "motor pos devi %d", dm1); |
|||
module_errorcode = err::kstep_motor_lost_step; |
|||
} |
|||
} |
|||
} |
|||
|
|||
// 检查电机状态
|
|||
{ |
|||
auto state = m_zm->getGState(); |
|||
if (state.reset) { |
|||
ZLOGE(TAG, "motor reset when run"); |
|||
throw zapp_exception(err::kstep_motor_subic_reset); |
|||
} else if (state.uv_cp) { |
|||
ZLOGE(TAG, "motor uv_cp when run"); |
|||
throw zapp_exception(err::kstep_motor_uv_cp); |
|||
} else if (state.drv_err) { |
|||
ZLOGE(TAG, "motor drv_err when run"); |
|||
throw zapp_exception(err::kstep_motor_drv_err); |
|||
} |
|||
} |
|||
} |
|||
|
|||
void PipetteModule::thread_delay(int32_t delayms) { //
|
|||
runingcheck(); |
|||
m_thread.sleep(delayms); |
|||
if (m_thread.getExitFlag()) { |
|||
throw zapp_thread_stoped_exception(); |
|||
} |
|||
} |
|||
void PipetteModule::thread_start_work(const char *fnname, function<void()> fn) { |
|||
m_thread.stop(); |
|||
module_status = 1; |
|||
module_errorcode = 0; |
|||
module_detail_errorcode = 0; |
|||
|
|||
m_state.asynchronous_result0 = 0; |
|||
m_state.asynchronous_result1 = 0; |
|||
|
|||
m_state.aspiration_ok = 0; |
|||
m_state.pump_lld_reach_limit = 0; |
|||
m_state.detected_liquid = 0; |
|||
m_state.water_level = 0; |
|||
m_state.tipblock = 0; |
|||
m_state.airsuction = 0; |
|||
m_state.bubble = 0; |
|||
|
|||
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;
|
|||
|
|||
// clear some state
|
|||
m_state.dul = 0; |
|||
m_state.dpos = 0; |
|||
|
|||
try { |
|||
// before do
|
|||
int32_t startpos = zm_get_now_pos(); |
|||
// DO_IN_THREAD(m_smtp2.pump_get_ul(&pump_before_pos_ul));
|
|||
|
|||
// do fn
|
|||
if (fn) fn(); |
|||
|
|||
m_zm->stop(); |
|||
m_smtp2.pump_stop(); |
|||
|
|||
// after do
|
|||
// DO_IN_THREAD(m_smtp2.pump_get_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; |
|||
module_status = 0; |
|||
} catch (const zapp_thread_stoped_exception &e) { // break by usr
|
|||
ZLOGI(TAG, "break by usr"); |
|||
m_zm->stop(); |
|||
m_smtp2.pump_stop(); |
|||
module_status = 0; |
|||
} catch (const zapp_exception &e) { |
|||
ZLOGI(TAG, "catch exception %d", e.ecode()); |
|||
m_zm->stop(); |
|||
m_smtp2.pump_stop(); |
|||
module_errorcode = e.ecode(); |
|||
module_status = 2; |
|||
} |
|||
|
|||
ZLOGI(TAG, "<- end work"); |
|||
}); |
|||
} |
|||
Write
Preview
Loading…
Cancel
Save
Reference in new issue