From 8d9925fc92063a8c295adf3fea45545638a9e234 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Thu, 5 Oct 2023 09:52:33 +0800 Subject: [PATCH] update --- components/flash/znvs.cpp | 2 + components/flash/znvs.hpp | 3 +- components/pipette_module/pipette_ctrl_module.cpp | 115 ++++++------- components/pipette_module/pipette_ctrl_module.hpp | 37 ++++- components/sensors/smtp2/smtp2.cpp | 25 ++- components/sensors/smtp2/smtp2.hpp | 6 +- .../step_motor_ctrl_module.cpp | 177 ++++++++++----------- .../step_motor_ctrl_module.hpp | 47 +++--- .../zcan_step_motor_ctrl_module.cpp | 12 +- components/zprotocols/zcancmder | 2 +- os/zos.cpp | 3 + os/zthread.cpp | 49 ++++++ os/zthread.hpp | 19 ++- 13 files changed, 299 insertions(+), 198 deletions(-) diff --git a/components/flash/znvs.cpp b/components/flash/znvs.cpp index 47b60bf..9a4fba5 100644 --- a/components/flash/znvs.cpp +++ b/components/flash/znvs.cpp @@ -71,12 +71,14 @@ void ZNVS::allocate_cfg(const char* key, type_t type, uint8_t* default_val, uint cfg->is_initialed = true; cfg->type = (uint8_t)type; memcpy(cfg->val, default_val, len); +#if 0 /** * @brief 刷新flash * * TODO:优化这里的代码，不要每次都刷新flash */ zsimple_flash_write((uint8_t*)&m_cfg, sizeof(m_cfg)); +#endif break; } } diff --git a/components/flash/znvs.hpp b/components/flash/znvs.hpp index fd4d75f..e7807cc 100644 --- a/components/flash/znvs.hpp +++ b/components/flash/znvs.hpp @@ -41,6 +41,8 @@ class ZNVS { void factory_reset(); void dumpcfg(); + void flush(); + int8_t get_config_int8(const char* key, int8_t default_val); void set_config_int8(const char* key, int8_t val); @@ -71,7 +73,6 @@ class ZNVS { cfg_t* get_cfg(const char* key); void allocate_cfg(const char* key, type_t type, uint8_t* default_val, uint8_t len); cfg_t* get_and_create_cfg(const char* key, type_t type, uint8_t* default_val, uint8_t len); - void flush(); }; } // namespace iflytop \ No newline at end of file diff --git a/components/pipette_module/pipette_ctrl_module.cpp b/components/pipette_module/pipette_ctrl_module.cpp index 325861b..b74d046 100644 --- a/components/pipette_module/pipette_ctrl_module.cpp +++ b/components/pipette_module/pipette_ctrl_module.cpp @@ -2,6 +2,19 @@ using namespace iflytop; using namespace std; #define TAG "PipetteModule" + +#define DO(ACTION) \ + { \ + int exec_ret = ACTION; \ + if (exec_ret != 0) { \ + ZLOGE(TAG, "do " #ACTION " fail, ret = %d", exec_ret); \ + m_lastexecstatus = exec_ret; \ + if (status_cb) status_cb(exec_ret); \ + return; \ + } \ + ZLOGI(TAG, "do " #ACTION " complete"); \ + } + void PipetteModule::initialize(SMTP2 *smtp2, // StepMotorCtrlModule *stepMotor) { m_smtp2 = smtp2; @@ -20,73 +33,67 @@ int32_t PipetteModule::stop(u8 stop_type) { } int32_t PipetteModule::zero_pos_calibrate(action_cb_status_t status_cb) { + ZLOGI(TAG, "zero_pos_calibrate"); m_thread.stop(); - m_thread.start([this, status_cb]() { - int32_t exec_ret = do_motor_action_block([this]() { // - return m_stepMotor->move_to_zero_with_calibrate(0, nullptr); - }); - if (exec_ret != 0) { - ZLOGE(TAG, "move_to_zero_with_calibrate fail, exec_ret = %d", exec_ret); - m_lastexecstatus = exec_ret; - if (status_cb) status_cb(exec_ret); - return; - } - ZLOGI(TAG, "zero_pos_calibrate complete"); + m_thread.start([this, status_cb]() { // + DO(m_stepMotor->move_to_zero_with_calibrate_block(0)); + + m_lastexecstatus = 0; + if (status_cb) status_cb(m_lastexecstatus); + return; }); - return 0; } int32_t PipetteModule::reset_device(action_cb_status_t status_cb) { + ZLOGI(TAG, "reset_device"); m_thread.stop(); m_thread.start([this, status_cb]() { - /******************************************************************************* - * m_smtp2 * - *******************************************************************************/ // 移液枪复位 - int ret = m_smtp2->doaction_block(&m_thread, [this]() { return m_smtp2->init_device(); }); - if (ret != 0) { - ZLOGE(TAG, "init_device fail,%d", ret); - m_lastexecstatus = ret; - if (status_cb) status_cb(ret); - return; - } - ZLOGI(TAG, "SMTP2 init_device complete"); - - /******************************************************************************* - * m_stepMotor * - *******************************************************************************/ - ret = do_motor_action_block( // - [this]() { return m_stepMotor->move_to_zero(nullptr); }); - - if (ret != 0) { - ZLOGE(TAG, "move_to_zero fail, ret = %d", ret); - m_lastexecstatus = ret; - if (status_cb) status_cb(m_lastexecstatus); - return; - } - ZLOGI(TAG, "Z axis reset complete"); - m_lastexecstatus = ret; + DO(m_smtp2->init_device_block()) + // Z轴复位 + DO(m_stepMotor->move_to_zero_block()); + // + m_lastexecstatus = 0; if (status_cb) status_cb(m_lastexecstatus); }); return 0; } -int32_t PipetteModule::do_motor_action_block(function action) { - if (action == nullptr) return -1; - int32_t ret = action(); - if (ret != 0) { - m_stepMotor->stop(0); - return ret; - } +int32_t PipetteModule::take_tip(int take_tip_high, action_cb_status_t status_cb) { + ZLOGI(TAG, "take_tip %d", tipid); - while (!m_thread.getExitFlag()) { - if (!m_stepMotor->isbusy()) break; - m_thread.sleep(1000); - } + m_thread.stop(); + m_thread.start([this, status_cb]() { + // 移动移液枪到取Tip位置 + DO(m_stepMotor->move_to(m_platformpara.take_tip_height_mm, nullptr)); + // 移液枪返回零点 + DO(m_stepMotor->move_to(m_platformpara.transfer_height_mm, nullptr)); - if (m_thread.getExitFlag()) { - m_stepMotor->stop(0); - return err::kcommon_error_break_by_user; - } + m_lastexecstatus = 0; + if (status_cb) status_cb(m_lastexecstatus); + }); +} + +int32_t PipetteModule::remove_tip(int remove_tip_high, action_cb_status_t status_cb) { + ZLOGI(TAG, "rmove_tip "); + + m_thread.stop(); + m_thread.start([this, status_cb]() { + // 移动移液枪到取Tip位置 + DO(m_stepMotor->move_to(m_platformpara.remove_tip_height_mm, nullptr)); + // 丢弃tip头 + DO(m_smtp2->put_tip_block()); + // 移液枪返回零点 + DO(m_stepMotor->move_to(m_platformpara.transfer_height_mm, nullptr)); - return m_stepMotor->get_last_exec_status(); + m_lastexecstatus = 0; + if (status_cb) status_cb(m_lastexecstatus); + }); } + +int32_t PipetteModule::take_and_split_liquid(u8 tube_id, s16 liquid_volume, s16 zhight, s16 abs_zhight, s16 shake_times, s16 shake_volume, // + action_cb_status_t status_cb) { + /** + * @brief + */ + return 0; +} \ No newline at end of file diff --git a/components/pipette_module/pipette_ctrl_module.hpp b/components/pipette_module/pipette_ctrl_module.hpp index 9448074..a566d3b 100644 --- a/components/pipette_module/pipette_ctrl_module.hpp +++ b/components/pipette_module/pipette_ctrl_module.hpp @@ -22,19 +22,46 @@ class PipetteModule : public I_PipetteModule { public: void initialize(SMTP2 *smtp2, StepMotorCtrlModule *stepMotor); + // void set_platform_para(platformpara_t *platformpara); + + int32_t set_z_motor_para(const I_StepMotorCtrlModule::base_param_t &z_motor_param); + int32_t get_z_motor_para(I_StepMotorCtrlModule::base_param_t &z_motor_param); + virtual int32_t enable(u8 enable); virtual int32_t stop(u8 stop_type); virtual int32_t zero_pos_calibrate(action_cb_status_t status_cb); virtual int32_t reset_device(action_cb_status_t status_cb); - virtual int32_t take_tip(int tipid, action_cb_status_t status_cb); - virtual int32_t remove_tip(action_cb_status_t status_cb); - // 取液(平台参数，试管参数, 取样体积，取样高度，摇匀次数，摇匀体积) - virtual int32_t take_and_split_liquid(u8 tube_id, s16 liquid_volume, s16 zhight, s16 abs_zhight, s16 shake_times, s16 shake_volume, // + virtual int32_t take_tip(int take_tip_high, action_cb_status_t status_cb); + virtual int32_t remove_tip(int remove_tip_high, action_cb_status_t status_cb); + + /** + * @brief 取液摇匀（自动探查液面） + * + * @param max_deep_height_mm 最大深度 + * @param relative_liquid_height_mm 相对液面高度 + * @param shake_times 摇匀次数 + * @param shake_volume 摇匀体积 + * @param take_volume 取样体积 + * @param status_cb + * @return int32_t + */ + virtual int32_t take_and_split_liquid(s16 max_deep_height_mm, // + s16 relative_liquid_height_mm, // + s16 shake_times, // + s16 shake_volume, // + s16 take_volume, // action_cb_status_t status_cb); + /** + * @brief 移动到指定高度 + * + * @param z_height + * @param status_cb + * @return int32_t + */ + virtual int32_t z_move_to(s16 z_height_mm, action_cb_status_t status_cb); public: - int32_t do_motor_action_block(function action); }; } // namespace iflytop \ No newline at end of file diff --git a/components/sensors/smtp2/smtp2.cpp b/components/sensors/smtp2/smtp2.cpp index 12ceb9d..ee8accc 100644 --- a/components/sensors/smtp2/smtp2.cpp +++ b/components/sensors/smtp2/smtp2.cpp @@ -110,15 +110,27 @@ int SMTP2::init_device() { ZLOGI(TAG, "init_device"); return doaction(fmt("/1ZR\r")); } +int SMTP2::init_device_block() { + return doaction_block([this]() { return init_device(); }); +} + int SMTP2::put_tip() { // ZLOGI(TAG, "put_tip:%s", "/1E1R"); ZLOGI(TAG, "put_tip"); return doaction(fmt("/1E1R\r")); } +int SMTP2::put_tip_block() { + return doaction_block([this]() { return put_tip(); }); +} int SMTP2::move_to(int pos) { ZLOGI(TAG, "move_to %d", pos); return doaction(fmt("/1N%dA%dR\r", 0, pos)); } + +int SMTP2::move_to_block(int pos) { + return doaction_block([this, pos]() { return move_to(pos); }); +} + int SMTP2::move_to_ul(int ul) { /** * @brief 分辨率在0.319ul进行操作 @@ -128,6 +140,10 @@ int SMTP2::move_to_ul(int ul) { int stepNum = Get1fromfloat(stepNumfloat); return doaction(fmt("/1N%dA%dR\r", 0, stepNum)); } +int SMTP2::move_to_ul_block(int ul) { + return doaction_block([this, ul]() { return move_to_ul(ul); }); +} + int SMTP2::set_resolution(int resolution) { ZLOGI(TAG, "set_resolution %d", resolution); return doaction(fmt("/1N%dR\r", resolution)); @@ -334,9 +350,10 @@ int SMTP2::sendcmd_block(const char* cmd, size_t txlen, char* rxbuf, size_t rxbu return 0; } -int32_t SMTP2::doaction_block(ZThread* thread, function action) { +int32_t SMTP2::doaction_block(function action) { // 移液枪复位 - int ret = action(); + ThisThread thisThread; + int ret = action(); if (ret != 0) { return ret; } @@ -352,10 +369,10 @@ int32_t SMTP2::doaction_block(ZThread* thread, function action) { return ret; } } - if (thread->getExitFlag()) { + if (thisThread.getExitFlag()) { stop(); return err::kcommon_error_break_by_user; } - thread->sleep(1000); + thisThread.sleep(1000); } } diff --git a/components/sensors/smtp2/smtp2.hpp b/components/sensors/smtp2/smtp2.hpp index 50f0e6b..c574de0 100644 --- a/components/sensors/smtp2/smtp2.hpp +++ b/components/sensors/smtp2/smtp2.hpp @@ -67,12 +67,14 @@ class SMTP2 { * @return int */ int init_device(); + int init_device_block(); /** * @brief 放Tip * * @return int */ int put_tip(); + int put_tip_block(); /** * @brief 移动到指定位置 @@ -81,6 +83,7 @@ class SMTP2 { * @return int */ int move_to(int pos); + int move_to_block(int pos); /** * @brief 移动到指定位置 * @@ -88,6 +91,7 @@ class SMTP2 { * @return int */ int move_to_ul(int ul); + int move_to_ul_block(int ul); /** * @brief 读取电容值 @@ -119,7 +123,7 @@ class SMTP2 { */ void stop(); - int32_t doaction_block(ZThread* thread, function action); + int32_t doaction_block(function action); private: int32_t doaction(char* cmd); diff --git a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp index 3720149..4137bad 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp @@ -14,6 +14,7 @@ void StepMotorCtrlModule::initialize(int id, IStepperMotor* stepM, ZGPIO* zero_g m_statu_lock.init(); m_thread.init("XYRobotCtrlModule", 1024, osPriorityNormal); +#if 0 run_param_t run_param; run_to_zero_param_t run_to_zero_param; warning_limit_param_t warning_limit_param; @@ -25,6 +26,7 @@ void StepMotorCtrlModule::initialize(int id, IStepperMotor* stepM, ZGPIO* zero_g set_run_param(run_param); set_run_to_zero_param(run_to_zero_param); set_warning_limit_param(warning_limit_param); +#endif } bool StepMotorCtrlModule::isbusy() { return m_thread.isworking(); } @@ -35,16 +37,16 @@ int32_t StepMotorCtrlModule::move_to(int32_t tox, action_cb_status_t status_cb) ZLOGI(TAG, "m%d move_to %d", m_id, tox); m_thread.stop(); - if (m_cfg_min_x != 0 && tox < m_cfg_min_x) { - tox = m_cfg_min_x; + if (m_param.min_x != 0 && tox < m_param.min_x) { + tox = m_param.min_x; } - if (m_cfg_max_x != 0 && tox > m_cfg_max_x) { - tox = m_cfg_max_x; + if (m_param.max_x != 0 && tox > m_param.max_x) { + tox = m_param.max_x; } m_thread.start( [this, tox, status_cb]() { - _motor_move_to(tox, m_cfg_velocity, m_cfg_acc, m_cfg_dec); + _motor_move_to(tox, m_param.maxspeed, m_param.acc, m_param.dec); while (!_motor_is_reach_target()) { if (m_thread.getExitFlag()) break; vTaskDelay(10); @@ -112,8 +114,8 @@ int32_t StepMotorCtrlModule::move_to_zero_with_calibrate(int32_t nowx, action_cb } int32_t calibrate_x, calibrate_y; - calibrate_x = dx + nowx; - m_zero_shift_x = calibrate_x; + calibrate_x = dx + nowx; + m_param.shift_x = calibrate_x; m_stepM1->setXACTUAL(0); m_lastexecstatus = 0; @@ -146,21 +148,21 @@ int32_t StepMotorCtrlModule::force_change_current_pos(int32_t xpos) { } int32_t StepMotorCtrlModule::rotate(int32_t speed, int32_t lastforms, action_cb_status_t status_cb) { // zlock_guard l(m_lock); - ZLOGI(TAG, "rotate speed:%d lastforms:%d acc:%d dec:%d", speed, lastforms, m_cfg_acc, m_cfg_dec); + ZLOGI(TAG, "rotate speed:%d lastforms:%d acc:%d dec:%d", speed, lastforms, m_param.acc, m_param.dec); if (lastforms < 0) { ZLOGW(TAG, "lastforms:%d < 0", lastforms); return err::kcommon_error_param_out_of_range; } - if (abs(speed) > m_cfg_max_speed) { - ZLOGW(TAG, "speed:%d > m_cfg_max_speed:%d", speed, m_cfg_max_speed); - speed = m_cfg_max_speed; + if (abs(speed) > m_param.maxspeed) { + ZLOGW(TAG, "speed:%d > m_cfg_max_speed:%d", speed, m_param.maxspeed); + speed = m_param.maxspeed; } m_thread.stop(); m_thread.start([this, lastforms, speed, status_cb]() { - m_stepM1->setAcceleration(m_cfg_acc); - m_stepM1->setDeceleration(m_cfg_dec); + m_stepM1->setAcceleration(m_param.acc); + m_stepM1->setDeceleration(m_param.dec); m_stepM1->rotate(speed); int32_t startticket = zos_get_tick(); @@ -199,83 +201,34 @@ int32_t StepMotorCtrlModule::read_detailed_status(detailed_status_t& debug_info) return 0; } -int32_t StepMotorCtrlModule::set_run_param(uint8_t operation, const run_param_t& param, run_param_t& ack) { - if (operation == kset_cmd_type_set) { - m_cfg_distance_scale = param.distance_scale / 1000.0; - m_cfg_acc = param.acc; - m_cfg_dec = param.dec; - m_cfg_max_speed = param.maxspeed; - m_cfg_min_x = param.min_x; - m_cfg_max_x = param.max_x; - m_cfg_ihold = param.ihold; - m_cfg_irun = param.irun; - m_cfg_iholddelay = param.iholddelay; - m_cfg_x_shaft = param.x_shaft; - m_cfg_shift_x = param.shift_x; - - ZLOGI(TAG, "=========== run param ============"); - ZLOGI(TAG, "= distance_scale :%f", m_cfg_distance_scale); - ZLOGI(TAG, "= acc :%d", m_cfg_acc); - ZLOGI(TAG, "= dec :%d", m_cfg_dec); - ZLOGI(TAG, "= max_speed :%d", m_cfg_max_speed); - ZLOGI(TAG, "= min_x :%d", m_cfg_min_x); - ZLOGI(TAG, "= max_x :%d", m_cfg_max_x); - ZLOGI(TAG, "= ihold :%d", m_cfg_ihold); - ZLOGI(TAG, "= irun :%d", m_cfg_irun); - ZLOGI(TAG, "= iholddelay :%d", m_cfg_iholddelay); - ZLOGI(TAG, "= x_shaft :%d", m_cfg_x_shaft); - ZLOGI(TAG, "= shift_x :%d", m_cfg_shift_x); - ZLOGI(TAG, "=================================="); - - m_stepM1->setIHOLD_IRUN(m_cfg_ihold, m_cfg_irun, m_cfg_iholddelay); - m_stepM1->setScale(m_cfg_distance_scale); - } - - ack.distance_scale = m_cfg_distance_scale * 1000; - ack.acc = m_cfg_acc; - ack.dec = m_cfg_dec; - ack.maxspeed = m_cfg_max_speed; - ack.min_x = m_cfg_min_x; - ack.max_x = m_cfg_max_x; - ack.ihold = m_cfg_ihold; - ack.irun = m_cfg_irun; - ack.iholddelay = m_cfg_iholddelay; - ack.x_shaft = m_cfg_x_shaft; - ack.shift_x = m_cfg_shift_x; - - return 0; +int32_t StepMotorCtrlModule::set_base_param(const base_param_t& param) { + m_param = param; + m_stepM1->setIHOLD_IRUN(m_param.ihold, m_param.irun, m_param.iholddelay); + m_stepM1->setScale(m_param.distance_scale / 1000.0); + + ZLOGI(TAG, "=========== base param ============"); + ZLOGI(TAG, "= x_shaft :%d", m_param.x_shaft); + ZLOGI(TAG, "= distance_scale :%f", m_param.distance_scale); + ZLOGI(TAG, "= ihold :%d", m_param.ihold); + ZLOGI(TAG, "= irun :%d", m_param.irun); + ZLOGI(TAG, "= iholddelay :%d", m_param.iholddelay); + ZLOGI(TAG, "= acc :%d", m_param.acc); + ZLOGI(TAG, "= dec :%d", m_param.dec); + ZLOGI(TAG, "= maxspeed :%d", m_param.maxspeed); + ZLOGI(TAG, "= min_x :%d", m_param.min_x); + ZLOGI(TAG, "= max_x :%d", m_param.max_x); + ZLOGI(TAG, "= shift_x :%d", m_param.shift_x); + ZLOGI(TAG, "= run_to_zero_max_d :%d", m_param.run_to_zero_move_to_zero_max_d); + ZLOGI(TAG, "= leave_from_zero_max_d :%d", m_param.run_to_zero_leave_from_zero_max_d); + ZLOGI(TAG, "= run_to_zero_speed :%d", m_param.run_to_zero_speed); + ZLOGI(TAG, "= run_to_zero_dec :%d", m_param.run_to_zero_dec); + ZLOGI(TAG, "=================================="); } -int32_t StepMotorCtrlModule::set_run_to_zero_param(uint8_t operation, const run_to_zero_param_t& param, run_to_zero_param_t& ack) { - if (operation == kset_cmd_type_set) { - m_cfg_runtozero_maxX = param.move_to_zero_max_d; - m_cfg_runtozero_leave_away_zero_max_distance = param.leave_from_zero_max_d; - m_cfg_runtozero_speed = param.speed; - m_cfg_runtozero_dec = param.dec; - - ZLOGI(TAG, "=========== run_to_zero_param ============"); - ZLOGI(TAG, "= move_to_zero_max_d :%d", m_cfg_runtozero_maxX); - ZLOGI(TAG, "= leave_from_zero_max_d :%d", m_cfg_runtozero_leave_away_zero_max_distance); - ZLOGI(TAG, "= speed :%d", m_cfg_runtozero_speed); - ZLOGI(TAG, "= dec :%d", m_cfg_runtozero_dec); - ZLOGI(TAG, "==========================================="); - - m_stepM1->setAcceleration(m_cfg_runtozero_dec); - m_stepM1->setDeceleration(m_cfg_runtozero_dec); - } - - ack.move_to_zero_max_d = m_cfg_runtozero_maxX; - ack.leave_from_zero_max_d = m_cfg_runtozero_leave_away_zero_max_distance; - ack.speed = m_cfg_runtozero_speed; - ack.dec = m_cfg_runtozero_dec; - - return 0; -} -int32_t StepMotorCtrlModule::set_warning_limit_param(uint8_t operation, const warning_limit_param_t& param, warning_limit_param_t& ack) { - if (operation == kset_cmd_type_set) { - ZLOGI(TAG, "set warning limit param"); - } +int32_t StepMotorCtrlModule::get_base_param(base_param_t& param) { + param = m_param; return 0; } + int32_t StepMotorCtrlModule::exec_move_to_zero_task(int32_t& dx) { int32_t xnow; getnowpos(xnow); @@ -296,7 +249,7 @@ int32_t StepMotorCtrlModule::exec_move_to_zero_task() { /** * @brief 如果设备已经在零点，则反向移动一定距离远离零点 */ - _motor_move_by(m_cfg_runtozero_leave_away_zero_max_distance, m_cfg_runtozero_speed, m_cfg_acc, m_cfg_runtozero_dec); + _motor_move_by(m_param.run_to_zero_leave_from_zero_max_d, m_param.run_to_zero_speed, m_param.acc, m_param.run_to_zero_dec); while (!_motor_is_reach_target()) { if (m_thread.getExitFlag()) break; vTaskDelay(1); @@ -314,7 +267,7 @@ int32_t StepMotorCtrlModule::exec_move_to_zero_task() { /******************************************************************************* * 移动X轴到零点 * *******************************************************************************/ - _motor_move_by(-m_cfg_runtozero_maxX, m_cfg_runtozero_speed, m_cfg_acc, m_cfg_runtozero_dec); + _motor_move_by(-m_param.run_to_zero_move_to_zero_max_d, m_param.run_to_zero_speed, m_param.acc, m_param.run_to_zero_dec); bool xreach_zero = false; while (!m_thread.getExitFlag() && !_motor_is_reach_target()) { // ZLOGI(TAG, "xgpio %d %d %d", m_Xgpio->getState(), m_stepM1->isReachTarget(), m_stepM2->isReachTarget()); @@ -379,12 +332,12 @@ bool StepMotorCtrlModule::_motor_is_reach_target() { return m_stepM1->isReachTar void StepMotorCtrlModule::inverse_kinematics(int32_t motor_pos, int32_t& x) { // m_zero_shift_x x = motor_pos; - if (m_cfg_x_shaft != 0) x = -x; - x += m_zero_shift_x; + if (m_param.x_shaft != 0) x = -x; + x += m_param.shift_x; } void StepMotorCtrlModule::forward_kinematics(int32_t x, int32_t& motor_pos) { - if (m_cfg_x_shaft != 0) x = -x; - x -= m_zero_shift_x; + if (m_param.x_shaft != 0) x = -x; + x -= m_param.shift_x; motor_pos = x; } #if 0 @@ -392,4 +345,40 @@ void StepMotorCtrlModule::updateStatus(uint8_t status) { zlock_guard lock(m_statu_lock); m_status = status; } -#endif \ No newline at end of file +#endif + +int32_t StepMotorCtrlModule::do_motor_action_block(function action) { + if (action == nullptr) return -1; + int32_t ret = action(); + if (ret != 0) { + stop(0); + return ret; + } + + ThisThread thisThread; + + while (!thisThread.getExitFlag()) { + if (!isbusy()) break; + thisThread.sleep(1000); + } + + if (isbusy()) { + stop(0); + return err::kcommon_error_break_by_user; + } + + return get_last_exec_status(); +} + +int32_t StepMotorCtrlModule::move_to_block(int32_t tox) { + return do_motor_action_block([this, tox]() { return move_to(tox, nullptr); }); +} +int32_t StepMotorCtrlModule::move_by_block(int32_t dx) { + return do_motor_action_block([this, dx]() { return move_by(dx, nullptr); }); +} +int32_t StepMotorCtrlModule::move_to_zero_block() { + return do_motor_action_block([this]() { return move_to_zero(nullptr); }); +} +int32_t StepMotorCtrlModule::move_to_zero_with_calibrate_block(int32_t x) { + return do_motor_action_block([this, x]() { return move_to_zero_with_calibrate(x, nullptr); }); +} diff --git a/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp b/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp index 61eba76..8416f39 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp @@ -16,34 +16,11 @@ class StepMotorCtrlModule : public I_StepMotorCtrlModule { ZThread m_thread; - // uint8_t m_status = 0; - - int32_t m_zero_shift_x = 0; - zmutex m_lock; zmutex m_statu_lock; - int32_t m_cfg_acc = 300000; - int32_t m_cfg_dec = 300000; - int32_t m_cfg_break_dec = 300000; - int32_t m_cfg_velocity = 300000; - int32_t m_cfg_max_speed = 300000; - - u8 m_cfg_ihold = 0; - u8 m_cfg_irun = 0; - u16 m_cfg_iholddelay = 0; - u8 m_cfg_x_shaft = 0; - int32_t m_cfg_shift_x = 0; - int32_t m_cfg_min_x = 0; - int32_t m_cfg_max_x = 0; - - int32_t m_cfg_runtozero_maxX = 5120000; - int32_t m_cfg_runtozero_speed = 30000; - int32_t m_cfg_runtozero_dec = 600000; - int32_t m_cfg_runtozero_leave_away_zero_max_distance = 51200; - float m_cfg_distance_scale = 1.0f; + base_param_t m_param; // bool m_x_shaft = false; - int32_t m_lastexecstatus = 0; public: @@ -52,10 +29,16 @@ class StepMotorCtrlModule : public I_StepMotorCtrlModule { virtual bool isbusy() override; virtual int32_t get_last_exec_status() override { return m_lastexecstatus; }; - virtual int32_t move_to(int32_t tox, action_cb_status_t status_cb); - virtual int32_t move_by(int32_t dx, action_cb_status_t status_cb); - virtual int32_t move_to_zero(action_cb_status_t status_cb); - virtual int32_t move_to_zero_with_calibrate(int32_t x, action_cb_status_t status_cb); + virtual int32_t move_to(int32_t tox, action_cb_status_t status_cb) override; + virtual int32_t move_by(int32_t dx, action_cb_status_t status_cb) override; + virtual int32_t move_to_zero(action_cb_status_t status_cb) override; + virtual int32_t move_to_zero_with_calibrate(int32_t x, action_cb_status_t status_cb) override; + + virtual int32_t move_to_block(int32_t tox) override; + virtual int32_t move_by_block(int32_t dx) override; + virtual int32_t move_to_zero_block() override; + virtual int32_t move_to_zero_with_calibrate_block(int32_t x) override; + virtual int32_t rotate(int32_t speed, int32_t lastforms, action_cb_status_t status_cb); virtual int32_t enable(bool venable) override; @@ -66,8 +49,12 @@ class StepMotorCtrlModule : public I_StepMotorCtrlModule { virtual int32_t read_status(status_t& status) override; virtual int32_t read_detailed_status(detailed_status_t& debug_info) override; + virtual int32_t set_base_param(const base_param_t& param) override; + virtual int32_t get_base_param(base_param_t& param) override; + +#if 0 virtual int32_t set_run_param(uint8_t operation, const run_param_t& param, run_param_t& ack) override; - virtual int32_t set_run_to_zero_param(uint8_t operation, const run_to_zero_param_t& param, run_to_zero_param_t& ack) override; + // virtual int32_t set_run_to_zero_param(uint8_t operation, const run_to_zero_param_t& param, run_to_zero_param_t& ack) override; virtual int32_t set_warning_limit_param(uint8_t operation, const warning_limit_param_t& param, warning_limit_param_t& ack) override; int32_t read_run_param(run_param_t& param) { return set_run_param(kset_cmd_type_read, param, param); } @@ -77,8 +64,10 @@ class StepMotorCtrlModule : public I_StepMotorCtrlModule { int32_t set_run_param(run_param_t& param) { return set_run_param(kset_cmd_type_set, param, param); } int32_t set_run_to_zero_param(run_to_zero_param_t& param) { return set_run_to_zero_param(kset_cmd_type_set, param, param); } int32_t set_warning_limit_param(warning_limit_param_t& param) { return set_warning_limit_param(kset_cmd_type_set, param, param); } +#endif private: + int32_t do_motor_action_block(function action); // void updateStatus(uint8_t status); void getnowpos(int32_t& pos); diff --git a/components/zcancmder_module/zcan_step_motor_ctrl_module.cpp b/components/zcancmder_module/zcan_step_motor_ctrl_module.cpp index 804b555..dab4af6 100644 --- a/components/zcancmder_module/zcan_step_motor_ctrl_module.cpp +++ b/components/zcancmder_module/zcan_step_motor_ctrl_module.cpp @@ -102,19 +102,15 @@ void ZCanStepMotorCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) { } END_PP(); - PROCESS_PACKET(kcmd_step_motor_ctrl_set_run_param, m_id) { // - errorcode = m_module->set_run_param(cmd->opt_type, cmd->param, ack->ack); + PROCESS_PACKET(kcmd_step_motor_ctrl_set_base_param, m_id) { // + errorcode = m_module->set_base_param(cmd->param); } END_PP(); - PROCESS_PACKET(kcmd_step_motor_ctrl_set_warning_limit_param, m_id) { // - errorcode = m_module->set_warning_limit_param(cmd->opt_type, cmd->param, ack->ack); + PROCESS_PACKET(kcmd_step_motor_ctrl_get_base_param, m_id) { // + errorcode = m_module->get_base_param(ack->ack); } END_PP(); - PROCESS_PACKET(kcmd_step_motor_ctrl_set_run_to_zero_param, m_id) { // - errorcode = m_module->set_run_to_zero_param(cmd->opt_type, cmd->param, ack->ack); - } - END_PP(); } #endif \ No newline at end of file diff --git a/components/zprotocols/zcancmder b/components/zprotocols/zcancmder index 358d158..3703970 160000 --- a/components/zprotocols/zcancmder +++ b/components/zprotocols/zcancmder @@ -1 +1 @@ -Subproject commit 358d158302546707f113d4028d4fa66e1da29d89 +Subproject commit 37039700689af7ef1de1f5f83c2277bae69a139a diff --git a/os/zos.cpp b/os/zos.cpp index f61d0a0..b5de870 100644 --- a/os/zos.cpp +++ b/os/zos.cpp @@ -1,7 +1,10 @@ #include "zos.hpp" + +#include "zthread.hpp" extern "C" { void zos_init(zos_cfg_t* cfg) { // zos_loggger_init(); iflytop::OSDefaultSchduler::getInstance()->initialize(); + iflytop::ZThread::zthread_module_init(); } } \ No newline at end of file diff --git a/os/zthread.cpp b/os/zthread.cpp index 6510ab1..6780c10 100644 --- a/os/zthread.cpp +++ b/os/zthread.cpp @@ -1,17 +1,46 @@ #include "zthread.hpp" +#include + #include "zoslogger.hpp" + using namespace iflytop; using namespace std; +static bool s_map_inited = false; +static zmutex s_zthread_map_lock; +static map s_zthread_map; +static ZThread g_default_thread; + static void zosthread_default_task(void const *argument) { ZThread *thread = (ZThread *)argument; ZASSERT(thread); thread->threadcb(); }; + +void ZThread::zthread_module_init() { + if (s_map_inited) return; + s_map_inited = true; + s_zthread_map_lock.init(); +} + +ZThread *ZThread::getThread() { + ZThread *thread = nullptr; + { + zlock_guard guard(s_zthread_map_lock); + thread = s_zthread_map[osThreadGetId()]; + } + return thread; +} + void ZThread::threadcb() { m_status = kidle; + { + zlock_guard guard(s_zthread_map_lock); + s_zthread_map[osThreadGetId()] = this; + } + while (true) { if (m_threadisworkingFlagCallSide) { m_status = kworking; @@ -83,4 +112,24 @@ void ZThread::wake() { } else { xTaskNotifyGive(m_defaultTaskHandle); } +} + +ThisThread::ThisThread() { + ZThread *thread = ZThread::getThread(); + this->thread = thread; +} +bool ThisThread::getExitFlag() { + if (thread) return thread->getExitFlag(); + return false; +} +bool ThisThread::isworking() { + if (thread) return thread->isworking(); + return true; +} +void ThisThread::sleep(uint32_t ms) { + if (thread) { + thread->sleep(ms); + } else { + vTaskDelay(pdMS_TO_TICKS(ms)); + } } \ No newline at end of file diff --git a/os/zthread.hpp b/os/zthread.hpp index efa1ae2..aca86ff 100644 --- a/os/zthread.hpp +++ b/os/zthread.hpp @@ -32,6 +32,11 @@ class ZThread { SemaphoreHandle_t m_lock; public: + /** + * @brief init in zos.cpp, called by zos_init + */ + static void zthread_module_init(); + void init(const char* threadname, int stack_size = 1024, osPriority priority = osPriorityNormal); void start(zosthread_cb_t cb); void start(zosthread_cb_t cb, zosthread_cb_t exitcb); @@ -40,11 +45,23 @@ class ZThread { bool getExitFlag() { return !m_threadisworkingFlagCallSide; } bool isworking() { return m_status == kworking; } - void sleep(uint32_t ms); void wake(); + static void sleep(uint32_t ms); + static ZThread* getThread(); + public: void threadcb(); }; +class ThisThread { + ZThread* thread; + + public: + ThisThread(); + bool getExitFlag(); + bool isworking(); + void sleep(uint32_t ms); +}; + } // namespace iflytop