From cfa0d290e0a5a6822c0a68b05242ede778fbfbf0 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Sun, 2 Jun 2024 19:17:04 +0800 Subject: [PATCH] update --- .../pipette_module/pipette_ctrl_module_v2.hpp | 8 +- components/sensors/m3078/m3078_code_scaner.cpp | 6 +- components/sensors/m3078/m3078_code_scaner.hpp | 6 +- .../step_motor_ctrl_module.cpp | 8 +- .../step_motor_ctrl_module.cpp.bak | 810 --------------------- .../step_motor_ctrl_module.hpp | 7 +- .../step_motor_ctrl_module_cpp.bak | 810 +++++++++++++++++++++ .../water_cooling_temperature_control_module.hpp | 2 +- components/zcancmder/zcan_protocol_parser.cpp | 44 +- 9 files changed, 852 insertions(+), 849 deletions(-) delete mode 100644 components/step_motor_ctrl_module/step_motor_ctrl_module.cpp.bak create mode 100644 components/step_motor_ctrl_module/step_motor_ctrl_module_cpp.bak diff --git a/components/pipette_module/pipette_ctrl_module_v2.hpp b/components/pipette_module/pipette_ctrl_module_v2.hpp index 203da9d..f6c1443 100644 --- a/components/pipette_module/pipette_ctrl_module_v2.hpp +++ b/components/pipette_module/pipette_ctrl_module_v2.hpp @@ -43,9 +43,9 @@ class PipetteModule : public ZIModule, public ZIPipetteCtrlModule { * Module * *******************************************************************************/ - virtual int32_t module_enable(int32_t enable) override; - virtual int32_t module_stop() override; - virtual int32_t module_break() override; + virtual int32_t module_enable(int32_t enable) ; + virtual int32_t module_stop() ; + virtual int32_t module_break() ; virtual int32_t module_get_status(int32_t *status) override; @@ -67,4 +67,4 @@ class PipetteModule : public ZIModule, public ZIPipetteCtrlModule { private: int32_t _read_error_status(); }; -} // namespace iflytop \ No newline at end of file +} // namespace iflytop diff --git a/components/sensors/m3078/m3078_code_scaner.cpp b/components/sensors/m3078/m3078_code_scaner.cpp index 76bbadc..b3d6eac 100644 --- a/components/sensors/m3078/m3078_code_scaner.cpp +++ b/components/sensors/m3078/m3078_code_scaner.cpp @@ -27,7 +27,7 @@ void M3078CodeScanner::initialize(int moduleid, hardware_config_t* hardwareconfi }); m_uart.startRxIt(); m_triggerGpio.initAsOutput(hardwareconfig->triggerPin, ZGPIO::kMode_nopull, true, false); - creg.module_enable = 1; + //creg.module_enable = 1; id = moduleid; } void M3078CodeScanner::trigger() { @@ -90,8 +90,8 @@ int32_t M3078CodeScanner::get_raw_sector_num(int32_t* num) { int32_t M3078CodeScanner::module_xxx_reg(int32_t param_id, bool read, int32_t& val) { switch (param_id) { MODULE_COMMON_PROCESS_REG_CB(); - PROCESS_REG(kreg_module_raw_sector_size, /* */ get_id_info_len(&val), ACTION_NONE); - PROCESS_REG(kreg_module_raw_sector_num, /* */ get_raw_sector_num(&val), ACTION_NONE); + // PROCESS_REG(kreg_module_raw_sector_size, /* */ get_id_info_len(&val), ACTION_NONE); + // PROCESS_REG(kreg_module_raw_sector_num, /* */ get_raw_sector_num(&val), ACTION_NONE); default: return err::kmodule_not_find_config_index; diff --git a/components/sensors/m3078/m3078_code_scaner.hpp b/components/sensors/m3078/m3078_code_scaner.hpp index 0e00a4e..a019750 100644 --- a/components/sensors/m3078/m3078_code_scaner.hpp +++ b/components/sensors/m3078/m3078_code_scaner.hpp @@ -85,10 +85,10 @@ class M3078CodeScanner : public ZIModule { *id = this->id; return 0; } - virtual int32_t module_stop() override; - virtual int32_t module_break() override; + virtual int32_t module_stop() ; + virtual int32_t module_break() ; virtual int32_t module_start() ; - virtual int32_t module_read_raw(int32_t index, uint8_t* data, int32_t* len) override; + virtual int32_t module_read_raw(int32_t index, uint8_t* data, int32_t* len) ; virtual int32_t module_get_status(int32_t* status) override; private: 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 581d647..7a2e0dc 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp @@ -8,10 +8,16 @@ using namespace iflytop; #define TAG "SMCM" +void StepMotorCtrlModule::initialize(int moduleid, IStepperMotor* stepM, ZGPIO* zero, ZGPIO* end, config_t* cfg) {} + int32_t StepMotorCtrlModule::module_xxx_reg(int32_t param_id, bool read, int32_t& val) { return 0; } int32_t StepMotorCtrlModule::module_get_status(int32_t* status) { return 0; } -int32_t StepMotorCtrlModule::step_motor_enable(int32_t enable) { return 0; } +int32_t StepMotorCtrlModule::step_motor_enable(int32_t enable) { + zlock_guard l(m_lock); + ZLOGI(TAG, "m%d motor_enable %ld", m_id, enable); + m_stepM1->enable(enable); +} int32_t StepMotorCtrlModule::step_motor_read_pos(int32_t* pos) { return 0; } int32_t StepMotorCtrlModule::step_motor_stop(int32_t breakstop) { return 0; } int32_t StepMotorCtrlModule::step_motor_easy_rotate(int32_t direction) { return 0; } diff --git a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp.bak b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp.bak deleted file mode 100644 index d8f4d1e..0000000 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp.bak +++ /dev/null @@ -1,810 +0,0 @@ -#include "step_motor_ctrl_module.hpp" - -#include -#include - -#include "a8000_protocol\protocol.hpp" -#include "sdk\components\flash\znvs.hpp" -using namespace iflytop; -#define TAG "SMCM" -void StepMotorCtrlModule::initialize(int moduleid, IStepperMotor* stepM, ZGPIO iotable[], int nio, const char* flashmark, flash_config_t* defaultcfg) { - m_id = moduleid; - m_stepM1 = stepM; - m_iotable = iotable; - m_nio = nio; - if (m_nio >= 8) m_nio = 8; - if (m_nio >= 1) m_Xgpio = &m_iotable[0]; - if (m_nio >= 2) m_end_gpio = &m_iotable[1]; - - m_lock.init(); - m_statu_lock.init(); - m_thread.init(TAG, 1024, osPriorityNormal); - m_flashmark = flashmark; - m_factory_config = *defaultcfg; - - if (m_flashmark) { - ZASSERT(ZNVS::ins().get_config(m_flashmark, (uint8_t*)&m_cfg, sizeof(m_cfg))); - if (!m_cfg.configInited) { - m_cfg.configInited = true; - m_cfg = m_factory_config; - flush(); - } - } else { - m_cfg = m_factory_config; - } - - active_cfg(); -} - -bool StepMotorCtrlModule::isbusy() { return m_thread.isworking(); } - -int32_t StepMotorCtrlModule::move_to_logic_point(int32_t logic_point_num, action_cb_status_t status_cb) { - ZLOGI(TAG, "move_to_logic_point %d", logic_point_num); - if (logic_point_num >= ZARRAY_SIZE(m_cfg.logic_point)) { - return err::kparam_out_of_range; - } - logic_point_t logic_point = m_cfg.logic_point[logic_point_num]; - return move_to(logic_point.x, logic_point.velocity, status_cb); -} -int32_t StepMotorCtrlModule::set_logic_point(int logic_point_num, int32_t x, int32_t vel, int32_t acc, int32_t dec) { - ZLOGI(TAG, "set_logic_point %d %d %d %d %d", logic_point_num, x, vel, acc, dec); - if (logic_point_num >= ZARRAY_SIZE(m_cfg.logic_point)) return err::kparam_out_of_range; - if (logic_point_num < 0) logic_point_num = 0; - // memset(&m_cfg, 0, sizeof(m_cfg)); - m_cfg.logic_point[logic_point_num].x = x; - m_cfg.logic_point[logic_point_num].velocity = vel; - m_cfg.logic_point[logic_point_num].acc = acc; - m_cfg.logic_point[logic_point_num].dec = dec; - return 0; -} -int32_t StepMotorCtrlModule::get_logic_point(int logic_point_num, logic_point_t& logic_point) { - if (logic_point_num >= ZARRAY_SIZE(m_cfg.logic_point)) { - return err::kparam_out_of_range; - } - logic_point = m_cfg.logic_point[logic_point_num]; - return 0; -} - -int32_t StepMotorCtrlModule::_move_to(int32_t tox, action_cb_status_t status_cb) { // - - zlock_guard lock(m_lock); - ZLOGI(TAG, "m%d move_to %d", m_id, tox); - m_thread.stop(); - - if (m_param.min_d != 0 && tox < m_param.min_d) { - tox = m_param.min_d; - } - if (m_param.max_d != 0 && tox > m_param.max_d) { - tox = m_param.max_d; - } - - m_thread.start( - [this, tox, status_cb]() { - _move_to_nolimit(tox, m_default_speed, m_param.motor_default_acc, m_param.motor_default_dec); - while (!_motor_is_reach_target()) { - if (m_thread.getExitFlag()) break; - vTaskDelay(10); - } - call_exec_status_cb(0, status_cb); - }, - [this, tox, status_cb]() { _motor_stop(); }); - return 0; -} -int32_t StepMotorCtrlModule::_move_by(int32_t dx, action_cb_status_t status_cb) { - zlock_guard lock(m_lock); - ZLOGI(TAG, "m%d move_by %d", m_id, dx); - m_thread.stop(); - - int32_t xnow = 0; - getnowpos(xnow); - - int32_t toxnow = xnow + dx; - // ZLOGI(TAG, "m%d move_by %d xnow:%d toxnow:%d", m_id, dx, xnow, toxnow); - return move_to(toxnow, [this, status_cb, xnow](int32_t status) { - if (status_cb) status_cb(status); - }); -} - -int32_t StepMotorCtrlModule::move_to(int32_t x, int32_t velocity, action_cb_status_t status_cb) { - if (m_default_speed > m_param.motor_default_velocity) { - ZLOGW(TAG, "m%d default speed:%d > m_cfg_max_speed:%d", m_id, m_default_speed, m_param.motor_default_velocity); - m_default_speed = m_param.motor_default_velocity; - } - if (velocity != 0) m_default_speed = velocity; - return move_to(x, status_cb); -} -int32_t StepMotorCtrlModule::move_by(int32_t dx, int32_t velocity, action_cb_status_t status_cb) { - if (m_default_speed > m_param.motor_default_velocity) { - ZLOGW(TAG, "m%d default speed:%d > m_cfg_max_speed:%d", m_id, m_default_speed, m_param.motor_default_velocity); - m_default_speed = m_param.motor_default_velocity; - } - if (velocity != 0) m_default_speed = velocity; - return move_by(dx, status_cb); -} - -int32_t StepMotorCtrlModule::move_to(int32_t tox, action_cb_status_t status_cb) { - m_default_speed = m_param.motor_default_velocity; - return _move_to(tox, status_cb); -} -int32_t StepMotorCtrlModule::move_by(int32_t dx, action_cb_status_t status_cb) { - m_default_speed = m_param.motor_default_velocity; - return _move_by(dx, status_cb); -} - -int32_t StepMotorCtrlModule::calculate_curpos(action_cb_status_t status_cb) { - zlock_guard lock(m_lock); - ZLOGI(TAG, "read_curpos_by_move2zero"); - m_thread.stop(); - m_thread.start( - [this, status_cb]() { - // - int32_t dx; - int32_t erro = _exec_move_to_zero_task(dx); - int xstart = dx + m_param.motor_shift; - m_calculate_curpos_result = xstart; - call_exec_status_cb(erro, status_cb); - }, - [this, status_cb]() { _motor_stop(); }); - return 0; -} -int32_t StepMotorCtrlModule::read_calculate_curpos_action_result(int32_t& pos) { - pos = m_calculate_curpos_result; - return 0; -} - -int32_t StepMotorCtrlModule::move_to_zero(action_cb_status_t status_cb) { - zlock_guard lock(m_lock); - ZLOGI(TAG, "move_to_zero"); - m_thread.stop(); - - m_thread.start( - [this, status_cb]() { - int32_t dx; - int32_t erro = _exec_move_to_zero_task(dx); - if (erro == 0) { - m_stepM1->setXACTUAL(0); - } - call_exec_status_cb(erro, status_cb); - }, - [this, status_cb]() { _motor_stop(); }); - - return 0; -} -int32_t StepMotorCtrlModule::move_to_zero_with_calibrate(int32_t nowx, action_cb_status_t status_cb) { - zlock_guard lock(m_lock); - ZLOGI(TAG, "move_to_zero_with_calibrate x:%d", nowx); - m_thread.stop(); - - m_thread.start([this, nowx, status_cb]() { - int32_t dx; - - int32_t erro = _exec_move_to_zero_task(dx); - if (erro != 0) { - ZLOGI(TAG, "_exec_move_to_zero_task failed:%d", erro); - _motor_stop(); - call_exec_status_cb(erro, status_cb); - - return; - } - - int32_t calibrate_x; - calibrate_x = dx + nowx; - m_param.motor_shift = calibrate_x; - - m_stepM1->setXACTUAL(0); - call_exec_status_cb(erro, status_cb); - return; - }); - - return 0; -} -int32_t StepMotorCtrlModule::enable(bool venable) { - zlock_guard l(m_lock); - ZLOGI(TAG, "m%d enable %d", m_id, venable); - m_stepM1->enable(venable); - // m_enable = venable; - creg.module_enable = venable; - return 0; -} -int32_t StepMotorCtrlModule::stop(uint8_t stopType) { - zlock_guard l(m_lock); - ZLOGI(TAG, "m%d stop", m_id); - m_thread.stop(); - m_stepM1->stop(); - return 0; -} -int32_t StepMotorCtrlModule::force_change_current_pos(int32_t xpos) { - zlock_guard l(m_lock); - ZLOGI(TAG, "m%d force_change_current_pos %d", m_id, xpos); - int32_t motor_pos = 0; - forward_kinematics(xpos, motor_pos); - m_stepM1->setXACTUAL(motor_pos); - return 0; -} -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_param.motor_default_acc, m_param.motor_default_dec); - - if (lastforms < 0) { - ZLOGW(TAG, "lastforms:%d < 0", lastforms); - return err::kparam_out_of_range; - } - - if (m_param.motor_default_velocity != 0 && abs(speed) > m_param.motor_default_velocity) { - ZLOGW(TAG, "speed:%d > m_cfg_max_speed:%d", speed, m_param.motor_default_velocity); - speed = m_param.motor_default_velocity; - } - m_thread.stop(); - m_thread.start([this, lastforms, speed, status_cb]() { - ZLOGI(TAG, "(in work thread)rotate speed:%d lastforms:%d acc:%d dec:%d", speed, lastforms, m_param.motor_default_acc, m_param.motor_default_dec); - _rotate(speed, m_param.motor_default_acc, m_param.motor_default_dec); - - int32_t startticket = zos_get_tick(); - // bool reachtime = false; - - while (!m_thread.getExitFlag()) { - if ((int32_t)zos_haspassedms(startticket) > lastforms && lastforms != 0) { - // reachtime = true; - m_stepM1->stop(); - break; - } - // ZLOGI(TAG,"..... state %d",m_thread.getExitFlag()); - osDelay(100); - } - call_exec_status_cb(0, status_cb); - m_stepM1->stop(); - return; - }); - return 0; -} - -int32_t StepMotorCtrlModule::read_pos(int32_t& pos) { - getnowpos(pos); - return 0; -} -int32_t StepMotorCtrlModule::read_pos() { - int32_t pos; - getnowpos(pos); - return pos; -} - -bool StepMotorCtrlModule::read_zero_io_state() { - if (m_Xgpio) return m_Xgpio->getState(); - return false; -} - -void StepMotorCtrlModule::_rotate(int32_t vel, int32_t acc, int32_t dec) { - ZLOGI(TAG, "m%d _rotate %d %d %d", m_id, vel, acc, dec); - m_stepM1->setAcceleration(acc); - m_stepM1->setDeceleration(dec); - m_stepM1->rotate(vel); -} - -void StepMotorCtrlModule::getnowpos(int32_t& pos) { - int32_t motor_pos = m_stepM1->getXACTUAL(); - inverse_kinematics(motor_pos, pos); -} -void StepMotorCtrlModule::_move_to_nolimit(int32_t pos, int32_t maxv, int32_t acc, int32_t dec) { - ZLOGI(TAG, "m%d _move_to_nolimit %d maxv:%d acc:%d dec:%d", m_id, pos, maxv, acc, dec); - m_stepM1->setAcceleration(acc); - m_stepM1->setDeceleration(dec); - int32_t motor_pos = 0; - forward_kinematics(pos, motor_pos); - m_stepM1->moveTo(motor_pos, maxv); -} -void StepMotorCtrlModule::_move_by_nolimit(int32_t dpos, int32_t maxv, int32_t acc, int32_t dec) { - ZLOGI(TAG, "m%d _motor_move_by %d maxv:%d acc:%d dec:%d", m_id, dpos, maxv, acc, dec); - m_stepM1->setAcceleration(acc); - m_stepM1->setDeceleration(dec); - - int32_t nowpos = 0; - getnowpos(nowpos); - - int32_t topos = nowpos + dpos; - int32_t motorpos = 0; - forward_kinematics(topos, motorpos); - ZLOGI(TAG, "motor moveTo %d %d", motorpos, maxv); - m_stepM1->moveTo(motorpos, maxv); -} -void StepMotorCtrlModule::_motor_stop(int32_t dec) { - ZLOGI(TAG, "m%d _motor_stop %d", m_id, dec); - if (dec > 0) { - m_stepM1->setDeceleration(dec); - } - m_stepM1->stop(); -} -bool StepMotorCtrlModule::_motor_is_reach_target() { return m_stepM1->isReachTarget(); } -void StepMotorCtrlModule::inverse_kinematics(int32_t motor_pos, int32_t& x) { - // m_zero_motor_shift - x = motor_pos; - x += m_param.motor_shift; -} -void StepMotorCtrlModule::forward_kinematics(int32_t x, int32_t& motor_pos) { - x -= m_param.motor_shift; - motor_pos = x; -} -#if 0 -void StepMotorCtrlModule::updateStatus(uint8_t status) { - zlock_guard lock(m_statu_lock); - m_status = status; -} -#endif - -int32_t StepMotorCtrlModule::do_motor_action_block_2(function action, function break_condition) { - if (action == nullptr) return -1; - int32_t ret = action(); - if (ret != 0) { - stop(0); - return ret; - } - - ThisThread thisThread; - int count = 0; - while (!thisThread.getExitFlag()) { - if (!isbusy()) break; - thisThread.sleep(1); - count += 1; - - if (break_condition) { - bool break_flag = false; - int32_t err = break_condition(break_flag, count); - if (err != 0) { - stop(0); - return err; - } - - if (break_flag) { - stop(0); - return 0; - } - } - } - - if (isbusy()) { - stop(0); - return err::kmodule_opeation_break_by_user; - } - - return get_last_exec_status(); -} - -int32_t StepMotorCtrlModule::do_motor_action_block(function action) { return do_motor_action_block_2(action, nullptr); } -int32_t StepMotorCtrlModule::move_to_block(int32_t tox, int overtime) { - return do_motor_action_block([this, tox]() { return move_to(tox, nullptr); }); -} -int32_t StepMotorCtrlModule::move_by_block(int32_t dx, int overtime) { - return do_motor_action_block([this, dx]() { return move_by(dx, nullptr); }); -} -int32_t StepMotorCtrlModule::move_to_zero_block(int overtime) { - return do_motor_action_block([this]() { return move_to_zero(nullptr); }); -} -int32_t StepMotorCtrlModule::move_to_zero_with_calibrate_block(int32_t x, int overtime) { - return do_motor_action_block([this, x]() { return move_to_zero_with_calibrate(x, nullptr); }); -} -int32_t StepMotorCtrlModule::move_to_block(int32_t tox, int32_t velocity, int overtime) { - return do_motor_action_block([this, velocity, tox]() { return move_to(tox, velocity, nullptr); }); -} -int32_t StepMotorCtrlModule::move_by_block(int32_t dx, int32_t velocity, int overtime) { - return do_motor_action_block([this, velocity, dx]() { return move_by(dx, velocity, nullptr); }); -} - -void StepMotorCtrlModule::call_exec_status_cb(int32_t status, action_cb_status_t status_cb) {} -void StepMotorCtrlModule::create_default_cfg(flash_config_t& cfg) { - memset(&cfg, 0, sizeof(cfg)); - cfg.base_param.motor_one_circle_pulse = 10000; - cfg.base_param.motor_one_circle_pulse_denominator = 1; - cfg.base_param.stepmotor_ihold = 1; - cfg.base_param.stepmotor_irun = 15; - cfg.base_param.stepmotor_iholddelay = 100; - cfg.base_param.stepmotor_iglobalscaler = 1; - cfg.base_param.motor_default_acc = 300; - cfg.base_param.motor_default_dec = 300; - cfg.base_param.motor_default_velocity = 500; - - cfg.base_param.motor_run_to_zero_max_d = 10000 * 100; - cfg.base_param.motor_run_to_zero_speed = 100; - cfg.base_param.motor_run_to_zero_dec = 600; - - cfg.base_param.motor_look_zero_edge_max_d = 10000 * 5; - cfg.base_param.motor_look_zero_edge_speed = 100; - cfg.base_param.motor_look_zero_edge_dec = 600; -} - -int32_t StepMotorCtrlModule::flush() { - ZLOGI(TAG, "flush"); - if (m_flashmark) { - ZNVS::ins().set_config(m_flashmark, (uint8_t*)&m_cfg, sizeof(m_cfg)); - ZNVS::ins().flush(); - } - return 0; -} -int32_t StepMotorCtrlModule::factory_reset() { - // - ZLOGI(TAG, "factory_reset"); - m_cfg = m_factory_config; - if (m_flashmark) { - ZNVS::ins().set_config(m_flashmark, (uint8_t*)&m_cfg, sizeof(m_cfg)); - ZNVS::ins().flush(); - } - active_cfg(); - return 0; -} -void StepMotorCtrlModule::active_cfg() { - m_stepM1->setIHOLD_IRUN(m_param.stepmotor_ihold, m_param.stepmotor_irun, m_param.stepmotor_iholddelay); - // stepmotor_iglobalscaler - m_stepM1->setScale(m_param.motor_one_circle_pulse); - m_stepM1->setScaleDenominator(m_param.motor_one_circle_pulse_denominator); - m_stepM1->setMotorShaft(m_param.motor_shaft); -} - -int32_t StepMotorCtrlModule::getid(int32_t* id) { - *id = m_id; - return 0; -} -int32_t StepMotorCtrlModule::module_stop() { return stop(0); } -int32_t StepMotorCtrlModule::module_break() { return stop(0); } -int32_t StepMotorCtrlModule::module_get_last_exec_status(int32_t* status) { - *status = 0; - return 0; -} -int32_t StepMotorCtrlModule::module_get_status(int32_t* status) { - // ZLOGI(TAG, "[%d]-module_get_status %d", m_id, m_thread.isworking()); - *status = m_thread.isworking() ? 1 : 0; - return 0; -} -int32_t StepMotorCtrlModule::module_get_error(int32_t* iserror) { - *iserror = 0; - return 0; -} - -int32_t StepMotorCtrlModule::module_xxx_reg(int32_t param_id, bool read, int32_t& val) { - switch (param_id) { - MODULE_COMMON_PROCESS_REG_CB(); - PROCESS_REG(kreg_step_motor_pos, read_pos(val), ACTION_NONE); - PROCESS_REG(kreg_step_motor_shift, REG_GET(m_param.motor_shift), REG_SET(m_param.motor_shift)); - PROCESS_REG(kreg_step_motor_shaft, REG_GET(m_param.motor_shaft), REG_SET(m_param.motor_shaft)); - PROCESS_REG(kreg_step_motor_one_circle_pulse, REG_GET(m_param.motor_one_circle_pulse), REG_SET(m_param.motor_one_circle_pulse)); - PROCESS_REG(kreg_step_motor_one_circle_pulse_denominator, REG_GET(m_param.motor_one_circle_pulse_denominator), REG_SET(m_param.motor_one_circle_pulse_denominator)); - PROCESS_REG(kreg_step_motor_default_velocity, REG_GET(m_param.motor_default_velocity), REG_SET(m_param.motor_default_velocity)); - PROCESS_REG(kreg_step_motor_default_acc, REG_GET(m_param.motor_default_acc), REG_SET(m_param.motor_default_acc)); - PROCESS_REG(kreg_step_motor_default_dec, REG_GET(m_param.motor_default_dec), REG_SET(m_param.motor_default_dec)); - PROCESS_REG(kreg_step_motor_run_to_zero_max_d, REG_GET(m_param.motor_run_to_zero_max_d), REG_SET(m_param.motor_run_to_zero_max_d)); - PROCESS_REG(kreg_step_motor_look_zero_edge_max_d, REG_GET(m_param.motor_look_zero_edge_max_d), REG_SET(m_param.motor_look_zero_edge_max_d)); - PROCESS_REG(kreg_step_motor_run_to_zero_speed, REG_GET(m_param.motor_run_to_zero_speed), REG_SET(m_param.motor_run_to_zero_speed)); - PROCESS_REG(kreg_step_motor_run_to_zero_dec, REG_GET(m_param.motor_run_to_zero_dec), REG_SET(m_param.motor_run_to_zero_dec)); - PROCESS_REG(kreg_step_motor_look_zero_edge_speed, REG_GET(m_param.motor_look_zero_edge_speed), REG_SET(m_param.motor_look_zero_edge_speed)); - PROCESS_REG(kreg_step_motor_look_zero_edge_dec, REG_GET(m_param.motor_look_zero_edge_dec), REG_SET(m_param.motor_look_zero_edge_dec)); - PROCESS_REG(kreg_step_motor_ihold, REG_GET(m_param.stepmotor_ihold), REG_SET(m_param.stepmotor_ihold)); - PROCESS_REG(kreg_step_motor_irun, REG_GET(m_param.stepmotor_irun), REG_SET(m_param.stepmotor_irun)); - PROCESS_REG(kreg_step_motor_iholddelay, REG_GET(m_param.stepmotor_iholddelay), REG_SET(m_param.stepmotor_iholddelay)); - PROCESS_REG(kreg_step_motor_iglobalscaler, REG_GET(m_param.stepmotor_iglobalscaler), REG_SET(m_param.stepmotor_iglobalscaler)); - PROCESS_REG(kreg_step_motor_max_d, REG_GET(m_param.max_d), REG_SET(m_param.max_d)); - PROCESS_REG(kreg_step_motor_min_d, REG_GET(m_param.min_d), REG_SET(m_param.min_d)); - PROCESS_REG(kreg_step_motor_io_state, module_readio(&val), ACTION_NONE); - - default: - return err::kmodule_not_find_config_index; - break; - } - return 0; -} -int32_t StepMotorCtrlModule::module_clear_error() { return 0; } -int32_t StepMotorCtrlModule::module_readio(int32_t* io) { - *io = 0; - for (int32_t i = 0; i < m_nio; i++) { - if (m_iotable[i].getState()) { - *io |= (0x01 << i); - } - } - return 0; -} -int32_t StepMotorCtrlModule::_read_io() { - int32_t iostate = 0; - module_readio(&iostate); - return iostate; -} - -int32_t StepMotorCtrlModule::module_read_adc(int32_t adcindex, int32_t* adc) { - *adc = 0; - return 0; -} -int32_t StepMotorCtrlModule::module_factory_reset() { return factory_reset(); } -int32_t StepMotorCtrlModule::module_flush_cfg() { return flush(); } -int32_t StepMotorCtrlModule::module_active_cfg() { - active_cfg(); - return 0; -} -int32_t StepMotorCtrlModule::motor_enable(int32_t varenable) { - zlock_guard l(m_lock); - ZLOGI(TAG, "m%d motor_enable %ld", m_id, varenable); - enable(varenable); - return 0; -} -int32_t StepMotorCtrlModule::motor_rotate(int32_t direction, int32_t motor_velocity, int32_t acc) { - ZLOGI(TAG, "m%d motor_rotate %d", m_id, direction); - m_thread.stop(); - if (acc == 0) { - acc = m_param.motor_default_acc; - } - _rotate(direction * motor_velocity, acc, acc); - return 0; -} -int32_t StepMotorCtrlModule::motor_move_by(int32_t dx, int32_t motor_velocity, int32_t acc) { - zlock_guard lock(m_lock); - int32_t xnow = 0; - getnowpos(xnow); - int32_t toxnow = xnow + dx; - return motor_move_to(toxnow, motor_velocity, acc); -} -int32_t StepMotorCtrlModule::motor_move_to(int32_t tox, int32_t motor_velocity, int32_t acc) { - zlock_guard lock(m_lock); - m_thread.stop(); - - if (m_param.min_d != 0 && tox < m_param.min_d) { - tox = m_param.min_d; - } - if (m_param.max_d != 0 && tox > m_param.max_d) { - tox = m_param.max_d; - } - - if (acc == 0) { - acc = m_param.motor_default_acc; - } - if (motor_velocity == 0) { - motor_velocity = m_param.motor_default_velocity; - } - - m_thread.start( - [this, tox, motor_velocity, acc]() { - _move_to_nolimit(tox, motor_velocity, acc, acc); - while (!_motor_is_reach_target()) { - if (m_thread.getExitFlag()) break; - vTaskDelay(10); - } - call_exec_status_cb(0, nullptr); - }, - [this, tox]() { _motor_stop(); }); - // ZLOGI(TAG, "m%d motor_move_to %d end", m_id, tox); - return 0; -} -int32_t StepMotorCtrlModule::motor_move_to_zero_backward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) { - ZLOGI(TAG, "motor_move_to_zero_backward"); - if (findzerospeed != 0) { - m_param.motor_run_to_zero_speed = findzerospeed; - } - if (findzeroedge_speed != 0) { - m_param.motor_look_zero_edge_speed = findzeroedge_speed; - } - if (acc != 0) { - m_param.motor_look_zero_edge_dec = acc; - m_param.motor_run_to_zero_dec = acc; - } - return move_to_zero(nullptr); -} - -int32_t StepMotorCtrlModule::motor_move_to_zero_backward_and_calculated_shift(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) { return move_to_zero_with_calibrate(0, nullptr); } -int32_t StepMotorCtrlModule::motor_read_pos(int32_t* pos) { - int32_t xnow = 0; - getnowpos(xnow); - *pos = xnow; - return 0; -} -void StepMotorCtrlModule::set_last_exec_status(int32_t ecode, int32_t val0, int32_t val1, int32_t val2, int32_t val3, int32_t val4) {} - -int32_t StepMotorCtrlModule::motor_calculated_pos_by_move_to_zero() { - zlock_guard lock(m_lock); - ZLOGI(TAG, "motor_calculated_pos_by_move_to_zero"); - m_thread.stop(); - m_thread.start( - [this]() { - // - int32_t dx; - int32_t erro = _exec_move_to_zero_task(dx); - if (erro != 0) { - set_last_exec_status(erro); - } else { - set_last_exec_status(erro, -dx + m_param.motor_shift); - m_stepM1->setXACTUAL(0); - } - }, - [this]() { _motor_stop(); }); - return 0; -} - -int32_t StepMotorCtrlModule::motor_easy_rotate(int32_t direction) { - zlock_guard lock(m_lock); - ZLOGI(TAG, "m%d motor_easy_rotate %d", m_id, direction); - m_thread.stop(); - _rotate(direction * m_param.motor_default_velocity, m_param.motor_default_acc, m_param.motor_default_dec); - return 0; -} -int32_t StepMotorCtrlModule::motor_easy_move_by(int32_t distance) { - zlock_guard lock(m_lock); - ZLOGI(TAG, "m%d motor_easy_move_by %d", m_id, distance); - m_thread.stop(); - return motor_move_by(distance, m_param.motor_default_velocity, m_param.motor_default_acc); -} -int32_t StepMotorCtrlModule::motor_easy_move_to(int32_t position) { - zlock_guard lock(m_lock); - ZLOGI(TAG, "m%d motor_easy_move_to %d", m_id, position); - m_thread.stop(); - return motor_move_to(position, m_param.motor_default_velocity, m_param.motor_default_acc); -} -int32_t StepMotorCtrlModule::motor_easy_move_to_zero(int32_t direction) { - if (direction <= 0) { - return move_to_zero(nullptr); - } else { - return err::koperation_not_support; - } -} -int32_t StepMotorCtrlModule::motor_easy_move_to_io(int32_t ioindex, int32_t direction) { return _exec_move_to_io_task(ioindex, direction); } - -/******************************************************************************* - * EXEC_TASK * - *******************************************************************************/ - -int32_t StepMotorCtrlModule::_exec_move_to_io_task(int32_t ioindex, int32_t direction) { - m_thread.stop(); - m_thread.start([this, ioindex, direction]() { _exec_move_to_io_task_fn(ioindex, direction); }); - return 0; -} -int32_t StepMotorCtrlModule::_exec_move_to_io_task_fn(int32_t ioindex, int32_t direction) { - int32_t runToPointSpeed = m_param.motor_run_to_zero_speed; - int32_t runToPointDec = m_param.motor_run_to_zero_dec; - // int32_t runToPointMaxD = m_param.motor_run_to_zero_max_d; - - int32_t lookPointVelocity = m_param.motor_look_zero_edge_speed; - int32_t lookPointDec = m_param.motor_look_zero_edge_dec; - // int32_t lookPointMaxD = m_param.motor_look_zero_edge_max_d; - - direction = direction >= 0 ? 1 : -1; - - ZGPIO* gpio = &m_iotable[ioindex]; - - if (!gpio->getState()) { - ZLOGI(TAG, "---------STEP1-------- move to point"); - _rotate(runToPointSpeed * direction, runToPointDec, runToPointDec); - bool xreach_io = false; - - while (!m_thread.getExitFlag()) { - if (gpio->getState()) { - xreach_io = true; - _motor_stop(-1); - break; - } - vTaskDelay(1); - } - - // 等待电机停止 - while (!m_stepM1->isStoped() && !m_thread.getExitFlag()) { - _motor_stop(-1); - vTaskDelay(100); - } - - // 失败返回 - if (!xreach_io) { - ZLOGE(TAG, "x reach io failed"); - return err::kxymotor_not_found_x_zero_point; - } - } - - // 如果设备已经在零点，则反向移动一定距离远离零点 - if (gpio->getState()) { - ZLOGI(TAG, "---------STEP2-------- find edge"); - _rotate(-direction * lookPointVelocity, lookPointDec, lookPointDec); - bool reach_edge = false; - - while (!m_thread.getExitFlag()) { - if (!gpio->getState()) { - reach_edge = true; - _motor_stop(-1); - break; - } - vTaskDelay(1); - } - - while (!m_stepM1->isStoped() && !m_thread.getExitFlag()) { // 等待电机停止 - _motor_stop(-1); - vTaskDelay(100); - } - - if (!reach_edge) { - ZLOGE(TAG, "leave away zero failed"); - return err::kxymotor_x_find_zero_edge_fail; - } - } - - if (m_thread.getExitFlag()) { - ZLOGI(TAG, "break move to zero thread exit"); - return err::kmodule_opeation_break_by_user; - } - - ZLOGI(TAG, "_exec_move_to_io_task_fn success"); - return 0; -} - -int32_t StepMotorCtrlModule::_exec_move_to_zero_task(int32_t& dx) { - int32_t pos1 = 0; - int32_t pos2 = 0; - int32_t ret = 0; - getnowpos(pos1); - ret = _exec_move_to_zero_task(); - getnowpos(pos2); - dx = pos2 - pos1; - return ret; -} -int32_t StepMotorCtrlModule::_exec_move_to_zero_task() { - /******************************************************************************* - * 第一次移动到零点 * - *******************************************************************************/ - if (!m_Xgpio->getState()) { - { - ZLOGI(TAG, "---------STEP1-------- move to zero first"); - _rotate(-m_param.motor_run_to_zero_speed, m_param.motor_default_acc, m_param.motor_run_to_zero_dec); - bool xreach_zero = false; - while (!m_thread.getExitFlag()) { - // ZLOGI(TAG, "xgpio %d %d %d", m_Xgpio->getState(), m_stepM1->isReachTarget(), m_stepM2->isReachTarget()); - if (m_Xgpio->getState()) { - xreach_zero = true; - _motor_stop(-1); - - while (!m_stepM1->isStoped() && !m_thread.getExitFlag()) { // 等待电机停止 - _motor_stop(-1); - vTaskDelay(10); - } - - break; - } - vTaskDelay(1); - } - if (!xreach_zero) { - // 触发异常回调 - ZLOGE(TAG, "x reach zero failed"); - return err::kxymotor_not_found_x_zero_point; - } - } - } - - /******************************************************************************* - * 远离X零点 * - *******************************************************************************/ - { - ZLOGI(TAG, "---------STEP2-------- leave away zero"); - /** - * @brief 如果设备已经在零点，则反向移动一定距离远离零点 - */ - if (m_Xgpio->getState()) { - _rotate(m_param.motor_look_zero_edge_speed, m_param.motor_default_acc, m_param.motor_look_zero_edge_dec); - while (true) { - if (m_thread.getExitFlag()) break; - if (!m_Xgpio->getState()) { - _motor_stop(-1); - while (!m_stepM1->isStoped() && !m_thread.getExitFlag()) { // 等待电机停止 - vTaskDelay(1); - } - break; - } - vTaskDelay(1); - } - } - - if (m_thread.getExitFlag()) { - ZLOGI(TAG, "break move to zero thread exit"); - return err::kmodule_opeation_break_by_user; - } - if (m_Xgpio->getState()) { - ZLOGE(TAG, "leave away zero failed"); - return err::kxymotor_x_find_zero_edge_fail; - } - } - - if (m_thread.getExitFlag()) { - ZLOGI(TAG, "break move to zero thread exit"); - return 0; - } - - ZLOGI(TAG, "x reach zero"); - ZLOGI(TAG, "move_to_zero success"); - - return 0; -} \ No newline at end of file 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 c43832c..a3881c6 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp @@ -59,14 +59,11 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor { *id = m_id; return 0; } - virtual int32_t module_stop() { return step_motor_stop(0); } - virtual int32_t module_break() { return step_motor_stop(1); } - virtual int32_t module_enable(int32_t enable) { return step_motor_enable(enable); } + virtual int32_t module_get_status(int32_t* status); + virtual int32_t module_xxx_reg(int32_t param_id, bool read, int32_t& val); /*********************************************************************************************************************** * ZIStepMotor * ***********************************************************************************************************************/ - virtual int32_t module_get_status(int32_t* status); - virtual int32_t module_xxx_reg(int32_t param_id, bool read, int32_t& val); virtual int32_t step_motor_enable(int32_t enable); virtual int32_t step_motor_read_pos(int32_t* pos); diff --git a/components/step_motor_ctrl_module/step_motor_ctrl_module_cpp.bak b/components/step_motor_ctrl_module/step_motor_ctrl_module_cpp.bak new file mode 100644 index 0000000..d8f4d1e --- /dev/null +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module_cpp.bak @@ -0,0 +1,810 @@ +#include "step_motor_ctrl_module.hpp" + +#include +#include + +#include "a8000_protocol\protocol.hpp" +#include "sdk\components\flash\znvs.hpp" +using namespace iflytop; +#define TAG "SMCM" +void StepMotorCtrlModule::initialize(int moduleid, IStepperMotor* stepM, ZGPIO iotable[], int nio, const char* flashmark, flash_config_t* defaultcfg) { + m_id = moduleid; + m_stepM1 = stepM; + m_iotable = iotable; + m_nio = nio; + if (m_nio >= 8) m_nio = 8; + if (m_nio >= 1) m_Xgpio = &m_iotable[0]; + if (m_nio >= 2) m_end_gpio = &m_iotable[1]; + + m_lock.init(); + m_statu_lock.init(); + m_thread.init(TAG, 1024, osPriorityNormal); + m_flashmark = flashmark; + m_factory_config = *defaultcfg; + + if (m_flashmark) { + ZASSERT(ZNVS::ins().get_config(m_flashmark, (uint8_t*)&m_cfg, sizeof(m_cfg))); + if (!m_cfg.configInited) { + m_cfg.configInited = true; + m_cfg = m_factory_config; + flush(); + } + } else { + m_cfg = m_factory_config; + } + + active_cfg(); +} + +bool StepMotorCtrlModule::isbusy() { return m_thread.isworking(); } + +int32_t StepMotorCtrlModule::move_to_logic_point(int32_t logic_point_num, action_cb_status_t status_cb) { + ZLOGI(TAG, "move_to_logic_point %d", logic_point_num); + if (logic_point_num >= ZARRAY_SIZE(m_cfg.logic_point)) { + return err::kparam_out_of_range; + } + logic_point_t logic_point = m_cfg.logic_point[logic_point_num]; + return move_to(logic_point.x, logic_point.velocity, status_cb); +} +int32_t StepMotorCtrlModule::set_logic_point(int logic_point_num, int32_t x, int32_t vel, int32_t acc, int32_t dec) { + ZLOGI(TAG, "set_logic_point %d %d %d %d %d", logic_point_num, x, vel, acc, dec); + if (logic_point_num >= ZARRAY_SIZE(m_cfg.logic_point)) return err::kparam_out_of_range; + if (logic_point_num < 0) logic_point_num = 0; + // memset(&m_cfg, 0, sizeof(m_cfg)); + m_cfg.logic_point[logic_point_num].x = x; + m_cfg.logic_point[logic_point_num].velocity = vel; + m_cfg.logic_point[logic_point_num].acc = acc; + m_cfg.logic_point[logic_point_num].dec = dec; + return 0; +} +int32_t StepMotorCtrlModule::get_logic_point(int logic_point_num, logic_point_t& logic_point) { + if (logic_point_num >= ZARRAY_SIZE(m_cfg.logic_point)) { + return err::kparam_out_of_range; + } + logic_point = m_cfg.logic_point[logic_point_num]; + return 0; +} + +int32_t StepMotorCtrlModule::_move_to(int32_t tox, action_cb_status_t status_cb) { // + + zlock_guard lock(m_lock); + ZLOGI(TAG, "m%d move_to %d", m_id, tox); + m_thread.stop(); + + if (m_param.min_d != 0 && tox < m_param.min_d) { + tox = m_param.min_d; + } + if (m_param.max_d != 0 && tox > m_param.max_d) { + tox = m_param.max_d; + } + + m_thread.start( + [this, tox, status_cb]() { + _move_to_nolimit(tox, m_default_speed, m_param.motor_default_acc, m_param.motor_default_dec); + while (!_motor_is_reach_target()) { + if (m_thread.getExitFlag()) break; + vTaskDelay(10); + } + call_exec_status_cb(0, status_cb); + }, + [this, tox, status_cb]() { _motor_stop(); }); + return 0; +} +int32_t StepMotorCtrlModule::_move_by(int32_t dx, action_cb_status_t status_cb) { + zlock_guard lock(m_lock); + ZLOGI(TAG, "m%d move_by %d", m_id, dx); + m_thread.stop(); + + int32_t xnow = 0; + getnowpos(xnow); + + int32_t toxnow = xnow + dx; + // ZLOGI(TAG, "m%d move_by %d xnow:%d toxnow:%d", m_id, dx, xnow, toxnow); + return move_to(toxnow, [this, status_cb, xnow](int32_t status) { + if (status_cb) status_cb(status); + }); +} + +int32_t StepMotorCtrlModule::move_to(int32_t x, int32_t velocity, action_cb_status_t status_cb) { + if (m_default_speed > m_param.motor_default_velocity) { + ZLOGW(TAG, "m%d default speed:%d > m_cfg_max_speed:%d", m_id, m_default_speed, m_param.motor_default_velocity); + m_default_speed = m_param.motor_default_velocity; + } + if (velocity != 0) m_default_speed = velocity; + return move_to(x, status_cb); +} +int32_t StepMotorCtrlModule::move_by(int32_t dx, int32_t velocity, action_cb_status_t status_cb) { + if (m_default_speed > m_param.motor_default_velocity) { + ZLOGW(TAG, "m%d default speed:%d > m_cfg_max_speed:%d", m_id, m_default_speed, m_param.motor_default_velocity); + m_default_speed = m_param.motor_default_velocity; + } + if (velocity != 0) m_default_speed = velocity; + return move_by(dx, status_cb); +} + +int32_t StepMotorCtrlModule::move_to(int32_t tox, action_cb_status_t status_cb) { + m_default_speed = m_param.motor_default_velocity; + return _move_to(tox, status_cb); +} +int32_t StepMotorCtrlModule::move_by(int32_t dx, action_cb_status_t status_cb) { + m_default_speed = m_param.motor_default_velocity; + return _move_by(dx, status_cb); +} + +int32_t StepMotorCtrlModule::calculate_curpos(action_cb_status_t status_cb) { + zlock_guard lock(m_lock); + ZLOGI(TAG, "read_curpos_by_move2zero"); + m_thread.stop(); + m_thread.start( + [this, status_cb]() { + // + int32_t dx; + int32_t erro = _exec_move_to_zero_task(dx); + int xstart = dx + m_param.motor_shift; + m_calculate_curpos_result = xstart; + call_exec_status_cb(erro, status_cb); + }, + [this, status_cb]() { _motor_stop(); }); + return 0; +} +int32_t StepMotorCtrlModule::read_calculate_curpos_action_result(int32_t& pos) { + pos = m_calculate_curpos_result; + return 0; +} + +int32_t StepMotorCtrlModule::move_to_zero(action_cb_status_t status_cb) { + zlock_guard lock(m_lock); + ZLOGI(TAG, "move_to_zero"); + m_thread.stop(); + + m_thread.start( + [this, status_cb]() { + int32_t dx; + int32_t erro = _exec_move_to_zero_task(dx); + if (erro == 0) { + m_stepM1->setXACTUAL(0); + } + call_exec_status_cb(erro, status_cb); + }, + [this, status_cb]() { _motor_stop(); }); + + return 0; +} +int32_t StepMotorCtrlModule::move_to_zero_with_calibrate(int32_t nowx, action_cb_status_t status_cb) { + zlock_guard lock(m_lock); + ZLOGI(TAG, "move_to_zero_with_calibrate x:%d", nowx); + m_thread.stop(); + + m_thread.start([this, nowx, status_cb]() { + int32_t dx; + + int32_t erro = _exec_move_to_zero_task(dx); + if (erro != 0) { + ZLOGI(TAG, "_exec_move_to_zero_task failed:%d", erro); + _motor_stop(); + call_exec_status_cb(erro, status_cb); + + return; + } + + int32_t calibrate_x; + calibrate_x = dx + nowx; + m_param.motor_shift = calibrate_x; + + m_stepM1->setXACTUAL(0); + call_exec_status_cb(erro, status_cb); + return; + }); + + return 0; +} +int32_t StepMotorCtrlModule::enable(bool venable) { + zlock_guard l(m_lock); + ZLOGI(TAG, "m%d enable %d", m_id, venable); + m_stepM1->enable(venable); + // m_enable = venable; + creg.module_enable = venable; + return 0; +} +int32_t StepMotorCtrlModule::stop(uint8_t stopType) { + zlock_guard l(m_lock); + ZLOGI(TAG, "m%d stop", m_id); + m_thread.stop(); + m_stepM1->stop(); + return 0; +} +int32_t StepMotorCtrlModule::force_change_current_pos(int32_t xpos) { + zlock_guard l(m_lock); + ZLOGI(TAG, "m%d force_change_current_pos %d", m_id, xpos); + int32_t motor_pos = 0; + forward_kinematics(xpos, motor_pos); + m_stepM1->setXACTUAL(motor_pos); + return 0; +} +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_param.motor_default_acc, m_param.motor_default_dec); + + if (lastforms < 0) { + ZLOGW(TAG, "lastforms:%d < 0", lastforms); + return err::kparam_out_of_range; + } + + if (m_param.motor_default_velocity != 0 && abs(speed) > m_param.motor_default_velocity) { + ZLOGW(TAG, "speed:%d > m_cfg_max_speed:%d", speed, m_param.motor_default_velocity); + speed = m_param.motor_default_velocity; + } + m_thread.stop(); + m_thread.start([this, lastforms, speed, status_cb]() { + ZLOGI(TAG, "(in work thread)rotate speed:%d lastforms:%d acc:%d dec:%d", speed, lastforms, m_param.motor_default_acc, m_param.motor_default_dec); + _rotate(speed, m_param.motor_default_acc, m_param.motor_default_dec); + + int32_t startticket = zos_get_tick(); + // bool reachtime = false; + + while (!m_thread.getExitFlag()) { + if ((int32_t)zos_haspassedms(startticket) > lastforms && lastforms != 0) { + // reachtime = true; + m_stepM1->stop(); + break; + } + // ZLOGI(TAG,"..... state %d",m_thread.getExitFlag()); + osDelay(100); + } + call_exec_status_cb(0, status_cb); + m_stepM1->stop(); + return; + }); + return 0; +} + +int32_t StepMotorCtrlModule::read_pos(int32_t& pos) { + getnowpos(pos); + return 0; +} +int32_t StepMotorCtrlModule::read_pos() { + int32_t pos; + getnowpos(pos); + return pos; +} + +bool StepMotorCtrlModule::read_zero_io_state() { + if (m_Xgpio) return m_Xgpio->getState(); + return false; +} + +void StepMotorCtrlModule::_rotate(int32_t vel, int32_t acc, int32_t dec) { + ZLOGI(TAG, "m%d _rotate %d %d %d", m_id, vel, acc, dec); + m_stepM1->setAcceleration(acc); + m_stepM1->setDeceleration(dec); + m_stepM1->rotate(vel); +} + +void StepMotorCtrlModule::getnowpos(int32_t& pos) { + int32_t motor_pos = m_stepM1->getXACTUAL(); + inverse_kinematics(motor_pos, pos); +} +void StepMotorCtrlModule::_move_to_nolimit(int32_t pos, int32_t maxv, int32_t acc, int32_t dec) { + ZLOGI(TAG, "m%d _move_to_nolimit %d maxv:%d acc:%d dec:%d", m_id, pos, maxv, acc, dec); + m_stepM1->setAcceleration(acc); + m_stepM1->setDeceleration(dec); + int32_t motor_pos = 0; + forward_kinematics(pos, motor_pos); + m_stepM1->moveTo(motor_pos, maxv); +} +void StepMotorCtrlModule::_move_by_nolimit(int32_t dpos, int32_t maxv, int32_t acc, int32_t dec) { + ZLOGI(TAG, "m%d _motor_move_by %d maxv:%d acc:%d dec:%d", m_id, dpos, maxv, acc, dec); + m_stepM1->setAcceleration(acc); + m_stepM1->setDeceleration(dec); + + int32_t nowpos = 0; + getnowpos(nowpos); + + int32_t topos = nowpos + dpos; + int32_t motorpos = 0; + forward_kinematics(topos, motorpos); + ZLOGI(TAG, "motor moveTo %d %d", motorpos, maxv); + m_stepM1->moveTo(motorpos, maxv); +} +void StepMotorCtrlModule::_motor_stop(int32_t dec) { + ZLOGI(TAG, "m%d _motor_stop %d", m_id, dec); + if (dec > 0) { + m_stepM1->setDeceleration(dec); + } + m_stepM1->stop(); +} +bool StepMotorCtrlModule::_motor_is_reach_target() { return m_stepM1->isReachTarget(); } +void StepMotorCtrlModule::inverse_kinematics(int32_t motor_pos, int32_t& x) { + // m_zero_motor_shift + x = motor_pos; + x += m_param.motor_shift; +} +void StepMotorCtrlModule::forward_kinematics(int32_t x, int32_t& motor_pos) { + x -= m_param.motor_shift; + motor_pos = x; +} +#if 0 +void StepMotorCtrlModule::updateStatus(uint8_t status) { + zlock_guard lock(m_statu_lock); + m_status = status; +} +#endif + +int32_t StepMotorCtrlModule::do_motor_action_block_2(function action, function break_condition) { + if (action == nullptr) return -1; + int32_t ret = action(); + if (ret != 0) { + stop(0); + return ret; + } + + ThisThread thisThread; + int count = 0; + while (!thisThread.getExitFlag()) { + if (!isbusy()) break; + thisThread.sleep(1); + count += 1; + + if (break_condition) { + bool break_flag = false; + int32_t err = break_condition(break_flag, count); + if (err != 0) { + stop(0); + return err; + } + + if (break_flag) { + stop(0); + return 0; + } + } + } + + if (isbusy()) { + stop(0); + return err::kmodule_opeation_break_by_user; + } + + return get_last_exec_status(); +} + +int32_t StepMotorCtrlModule::do_motor_action_block(function action) { return do_motor_action_block_2(action, nullptr); } +int32_t StepMotorCtrlModule::move_to_block(int32_t tox, int overtime) { + return do_motor_action_block([this, tox]() { return move_to(tox, nullptr); }); +} +int32_t StepMotorCtrlModule::move_by_block(int32_t dx, int overtime) { + return do_motor_action_block([this, dx]() { return move_by(dx, nullptr); }); +} +int32_t StepMotorCtrlModule::move_to_zero_block(int overtime) { + return do_motor_action_block([this]() { return move_to_zero(nullptr); }); +} +int32_t StepMotorCtrlModule::move_to_zero_with_calibrate_block(int32_t x, int overtime) { + return do_motor_action_block([this, x]() { return move_to_zero_with_calibrate(x, nullptr); }); +} +int32_t StepMotorCtrlModule::move_to_block(int32_t tox, int32_t velocity, int overtime) { + return do_motor_action_block([this, velocity, tox]() { return move_to(tox, velocity, nullptr); }); +} +int32_t StepMotorCtrlModule::move_by_block(int32_t dx, int32_t velocity, int overtime) { + return do_motor_action_block([this, velocity, dx]() { return move_by(dx, velocity, nullptr); }); +} + +void StepMotorCtrlModule::call_exec_status_cb(int32_t status, action_cb_status_t status_cb) {} +void StepMotorCtrlModule::create_default_cfg(flash_config_t& cfg) { + memset(&cfg, 0, sizeof(cfg)); + cfg.base_param.motor_one_circle_pulse = 10000; + cfg.base_param.motor_one_circle_pulse_denominator = 1; + cfg.base_param.stepmotor_ihold = 1; + cfg.base_param.stepmotor_irun = 15; + cfg.base_param.stepmotor_iholddelay = 100; + cfg.base_param.stepmotor_iglobalscaler = 1; + cfg.base_param.motor_default_acc = 300; + cfg.base_param.motor_default_dec = 300; + cfg.base_param.motor_default_velocity = 500; + + cfg.base_param.motor_run_to_zero_max_d = 10000 * 100; + cfg.base_param.motor_run_to_zero_speed = 100; + cfg.base_param.motor_run_to_zero_dec = 600; + + cfg.base_param.motor_look_zero_edge_max_d = 10000 * 5; + cfg.base_param.motor_look_zero_edge_speed = 100; + cfg.base_param.motor_look_zero_edge_dec = 600; +} + +int32_t StepMotorCtrlModule::flush() { + ZLOGI(TAG, "flush"); + if (m_flashmark) { + ZNVS::ins().set_config(m_flashmark, (uint8_t*)&m_cfg, sizeof(m_cfg)); + ZNVS::ins().flush(); + } + return 0; +} +int32_t StepMotorCtrlModule::factory_reset() { + // + ZLOGI(TAG, "factory_reset"); + m_cfg = m_factory_config; + if (m_flashmark) { + ZNVS::ins().set_config(m_flashmark, (uint8_t*)&m_cfg, sizeof(m_cfg)); + ZNVS::ins().flush(); + } + active_cfg(); + return 0; +} +void StepMotorCtrlModule::active_cfg() { + m_stepM1->setIHOLD_IRUN(m_param.stepmotor_ihold, m_param.stepmotor_irun, m_param.stepmotor_iholddelay); + // stepmotor_iglobalscaler + m_stepM1->setScale(m_param.motor_one_circle_pulse); + m_stepM1->setScaleDenominator(m_param.motor_one_circle_pulse_denominator); + m_stepM1->setMotorShaft(m_param.motor_shaft); +} + +int32_t StepMotorCtrlModule::getid(int32_t* id) { + *id = m_id; + return 0; +} +int32_t StepMotorCtrlModule::module_stop() { return stop(0); } +int32_t StepMotorCtrlModule::module_break() { return stop(0); } +int32_t StepMotorCtrlModule::module_get_last_exec_status(int32_t* status) { + *status = 0; + return 0; +} +int32_t StepMotorCtrlModule::module_get_status(int32_t* status) { + // ZLOGI(TAG, "[%d]-module_get_status %d", m_id, m_thread.isworking()); + *status = m_thread.isworking() ? 1 : 0; + return 0; +} +int32_t StepMotorCtrlModule::module_get_error(int32_t* iserror) { + *iserror = 0; + return 0; +} + +int32_t StepMotorCtrlModule::module_xxx_reg(int32_t param_id, bool read, int32_t& val) { + switch (param_id) { + MODULE_COMMON_PROCESS_REG_CB(); + PROCESS_REG(kreg_step_motor_pos, read_pos(val), ACTION_NONE); + PROCESS_REG(kreg_step_motor_shift, REG_GET(m_param.motor_shift), REG_SET(m_param.motor_shift)); + PROCESS_REG(kreg_step_motor_shaft, REG_GET(m_param.motor_shaft), REG_SET(m_param.motor_shaft)); + PROCESS_REG(kreg_step_motor_one_circle_pulse, REG_GET(m_param.motor_one_circle_pulse), REG_SET(m_param.motor_one_circle_pulse)); + PROCESS_REG(kreg_step_motor_one_circle_pulse_denominator, REG_GET(m_param.motor_one_circle_pulse_denominator), REG_SET(m_param.motor_one_circle_pulse_denominator)); + PROCESS_REG(kreg_step_motor_default_velocity, REG_GET(m_param.motor_default_velocity), REG_SET(m_param.motor_default_velocity)); + PROCESS_REG(kreg_step_motor_default_acc, REG_GET(m_param.motor_default_acc), REG_SET(m_param.motor_default_acc)); + PROCESS_REG(kreg_step_motor_default_dec, REG_GET(m_param.motor_default_dec), REG_SET(m_param.motor_default_dec)); + PROCESS_REG(kreg_step_motor_run_to_zero_max_d, REG_GET(m_param.motor_run_to_zero_max_d), REG_SET(m_param.motor_run_to_zero_max_d)); + PROCESS_REG(kreg_step_motor_look_zero_edge_max_d, REG_GET(m_param.motor_look_zero_edge_max_d), REG_SET(m_param.motor_look_zero_edge_max_d)); + PROCESS_REG(kreg_step_motor_run_to_zero_speed, REG_GET(m_param.motor_run_to_zero_speed), REG_SET(m_param.motor_run_to_zero_speed)); + PROCESS_REG(kreg_step_motor_run_to_zero_dec, REG_GET(m_param.motor_run_to_zero_dec), REG_SET(m_param.motor_run_to_zero_dec)); + PROCESS_REG(kreg_step_motor_look_zero_edge_speed, REG_GET(m_param.motor_look_zero_edge_speed), REG_SET(m_param.motor_look_zero_edge_speed)); + PROCESS_REG(kreg_step_motor_look_zero_edge_dec, REG_GET(m_param.motor_look_zero_edge_dec), REG_SET(m_param.motor_look_zero_edge_dec)); + PROCESS_REG(kreg_step_motor_ihold, REG_GET(m_param.stepmotor_ihold), REG_SET(m_param.stepmotor_ihold)); + PROCESS_REG(kreg_step_motor_irun, REG_GET(m_param.stepmotor_irun), REG_SET(m_param.stepmotor_irun)); + PROCESS_REG(kreg_step_motor_iholddelay, REG_GET(m_param.stepmotor_iholddelay), REG_SET(m_param.stepmotor_iholddelay)); + PROCESS_REG(kreg_step_motor_iglobalscaler, REG_GET(m_param.stepmotor_iglobalscaler), REG_SET(m_param.stepmotor_iglobalscaler)); + PROCESS_REG(kreg_step_motor_max_d, REG_GET(m_param.max_d), REG_SET(m_param.max_d)); + PROCESS_REG(kreg_step_motor_min_d, REG_GET(m_param.min_d), REG_SET(m_param.min_d)); + PROCESS_REG(kreg_step_motor_io_state, module_readio(&val), ACTION_NONE); + + default: + return err::kmodule_not_find_config_index; + break; + } + return 0; +} +int32_t StepMotorCtrlModule::module_clear_error() { return 0; } +int32_t StepMotorCtrlModule::module_readio(int32_t* io) { + *io = 0; + for (int32_t i = 0; i < m_nio; i++) { + if (m_iotable[i].getState()) { + *io |= (0x01 << i); + } + } + return 0; +} +int32_t StepMotorCtrlModule::_read_io() { + int32_t iostate = 0; + module_readio(&iostate); + return iostate; +} + +int32_t StepMotorCtrlModule::module_read_adc(int32_t adcindex, int32_t* adc) { + *adc = 0; + return 0; +} +int32_t StepMotorCtrlModule::module_factory_reset() { return factory_reset(); } +int32_t StepMotorCtrlModule::module_flush_cfg() { return flush(); } +int32_t StepMotorCtrlModule::module_active_cfg() { + active_cfg(); + return 0; +} +int32_t StepMotorCtrlModule::motor_enable(int32_t varenable) { + zlock_guard l(m_lock); + ZLOGI(TAG, "m%d motor_enable %ld", m_id, varenable); + enable(varenable); + return 0; +} +int32_t StepMotorCtrlModule::motor_rotate(int32_t direction, int32_t motor_velocity, int32_t acc) { + ZLOGI(TAG, "m%d motor_rotate %d", m_id, direction); + m_thread.stop(); + if (acc == 0) { + acc = m_param.motor_default_acc; + } + _rotate(direction * motor_velocity, acc, acc); + return 0; +} +int32_t StepMotorCtrlModule::motor_move_by(int32_t dx, int32_t motor_velocity, int32_t acc) { + zlock_guard lock(m_lock); + int32_t xnow = 0; + getnowpos(xnow); + int32_t toxnow = xnow + dx; + return motor_move_to(toxnow, motor_velocity, acc); +} +int32_t StepMotorCtrlModule::motor_move_to(int32_t tox, int32_t motor_velocity, int32_t acc) { + zlock_guard lock(m_lock); + m_thread.stop(); + + if (m_param.min_d != 0 && tox < m_param.min_d) { + tox = m_param.min_d; + } + if (m_param.max_d != 0 && tox > m_param.max_d) { + tox = m_param.max_d; + } + + if (acc == 0) { + acc = m_param.motor_default_acc; + } + if (motor_velocity == 0) { + motor_velocity = m_param.motor_default_velocity; + } + + m_thread.start( + [this, tox, motor_velocity, acc]() { + _move_to_nolimit(tox, motor_velocity, acc, acc); + while (!_motor_is_reach_target()) { + if (m_thread.getExitFlag()) break; + vTaskDelay(10); + } + call_exec_status_cb(0, nullptr); + }, + [this, tox]() { _motor_stop(); }); + // ZLOGI(TAG, "m%d motor_move_to %d end", m_id, tox); + return 0; +} +int32_t StepMotorCtrlModule::motor_move_to_zero_backward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) { + ZLOGI(TAG, "motor_move_to_zero_backward"); + if (findzerospeed != 0) { + m_param.motor_run_to_zero_speed = findzerospeed; + } + if (findzeroedge_speed != 0) { + m_param.motor_look_zero_edge_speed = findzeroedge_speed; + } + if (acc != 0) { + m_param.motor_look_zero_edge_dec = acc; + m_param.motor_run_to_zero_dec = acc; + } + return move_to_zero(nullptr); +} + +int32_t StepMotorCtrlModule::motor_move_to_zero_backward_and_calculated_shift(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) { return move_to_zero_with_calibrate(0, nullptr); } +int32_t StepMotorCtrlModule::motor_read_pos(int32_t* pos) { + int32_t xnow = 0; + getnowpos(xnow); + *pos = xnow; + return 0; +} +void StepMotorCtrlModule::set_last_exec_status(int32_t ecode, int32_t val0, int32_t val1, int32_t val2, int32_t val3, int32_t val4) {} + +int32_t StepMotorCtrlModule::motor_calculated_pos_by_move_to_zero() { + zlock_guard lock(m_lock); + ZLOGI(TAG, "motor_calculated_pos_by_move_to_zero"); + m_thread.stop(); + m_thread.start( + [this]() { + // + int32_t dx; + int32_t erro = _exec_move_to_zero_task(dx); + if (erro != 0) { + set_last_exec_status(erro); + } else { + set_last_exec_status(erro, -dx + m_param.motor_shift); + m_stepM1->setXACTUAL(0); + } + }, + [this]() { _motor_stop(); }); + return 0; +} + +int32_t StepMotorCtrlModule::motor_easy_rotate(int32_t direction) { + zlock_guard lock(m_lock); + ZLOGI(TAG, "m%d motor_easy_rotate %d", m_id, direction); + m_thread.stop(); + _rotate(direction * m_param.motor_default_velocity, m_param.motor_default_acc, m_param.motor_default_dec); + return 0; +} +int32_t StepMotorCtrlModule::motor_easy_move_by(int32_t distance) { + zlock_guard lock(m_lock); + ZLOGI(TAG, "m%d motor_easy_move_by %d", m_id, distance); + m_thread.stop(); + return motor_move_by(distance, m_param.motor_default_velocity, m_param.motor_default_acc); +} +int32_t StepMotorCtrlModule::motor_easy_move_to(int32_t position) { + zlock_guard lock(m_lock); + ZLOGI(TAG, "m%d motor_easy_move_to %d", m_id, position); + m_thread.stop(); + return motor_move_to(position, m_param.motor_default_velocity, m_param.motor_default_acc); +} +int32_t StepMotorCtrlModule::motor_easy_move_to_zero(int32_t direction) { + if (direction <= 0) { + return move_to_zero(nullptr); + } else { + return err::koperation_not_support; + } +} +int32_t StepMotorCtrlModule::motor_easy_move_to_io(int32_t ioindex, int32_t direction) { return _exec_move_to_io_task(ioindex, direction); } + +/******************************************************************************* + * EXEC_TASK * + *******************************************************************************/ + +int32_t StepMotorCtrlModule::_exec_move_to_io_task(int32_t ioindex, int32_t direction) { + m_thread.stop(); + m_thread.start([this, ioindex, direction]() { _exec_move_to_io_task_fn(ioindex, direction); }); + return 0; +} +int32_t StepMotorCtrlModule::_exec_move_to_io_task_fn(int32_t ioindex, int32_t direction) { + int32_t runToPointSpeed = m_param.motor_run_to_zero_speed; + int32_t runToPointDec = m_param.motor_run_to_zero_dec; + // int32_t runToPointMaxD = m_param.motor_run_to_zero_max_d; + + int32_t lookPointVelocity = m_param.motor_look_zero_edge_speed; + int32_t lookPointDec = m_param.motor_look_zero_edge_dec; + // int32_t lookPointMaxD = m_param.motor_look_zero_edge_max_d; + + direction = direction >= 0 ? 1 : -1; + + ZGPIO* gpio = &m_iotable[ioindex]; + + if (!gpio->getState()) { + ZLOGI(TAG, "---------STEP1-------- move to point"); + _rotate(runToPointSpeed * direction, runToPointDec, runToPointDec); + bool xreach_io = false; + + while (!m_thread.getExitFlag()) { + if (gpio->getState()) { + xreach_io = true; + _motor_stop(-1); + break; + } + vTaskDelay(1); + } + + // 等待电机停止 + while (!m_stepM1->isStoped() && !m_thread.getExitFlag()) { + _motor_stop(-1); + vTaskDelay(100); + } + + // 失败返回 + if (!xreach_io) { + ZLOGE(TAG, "x reach io failed"); + return err::kxymotor_not_found_x_zero_point; + } + } + + // 如果设备已经在零点，则反向移动一定距离远离零点 + if (gpio->getState()) { + ZLOGI(TAG, "---------STEP2-------- find edge"); + _rotate(-direction * lookPointVelocity, lookPointDec, lookPointDec); + bool reach_edge = false; + + while (!m_thread.getExitFlag()) { + if (!gpio->getState()) { + reach_edge = true; + _motor_stop(-1); + break; + } + vTaskDelay(1); + } + + while (!m_stepM1->isStoped() && !m_thread.getExitFlag()) { // 等待电机停止 + _motor_stop(-1); + vTaskDelay(100); + } + + if (!reach_edge) { + ZLOGE(TAG, "leave away zero failed"); + return err::kxymotor_x_find_zero_edge_fail; + } + } + + if (m_thread.getExitFlag()) { + ZLOGI(TAG, "break move to zero thread exit"); + return err::kmodule_opeation_break_by_user; + } + + ZLOGI(TAG, "_exec_move_to_io_task_fn success"); + return 0; +} + +int32_t StepMotorCtrlModule::_exec_move_to_zero_task(int32_t& dx) { + int32_t pos1 = 0; + int32_t pos2 = 0; + int32_t ret = 0; + getnowpos(pos1); + ret = _exec_move_to_zero_task(); + getnowpos(pos2); + dx = pos2 - pos1; + return ret; +} +int32_t StepMotorCtrlModule::_exec_move_to_zero_task() { + /******************************************************************************* + * 第一次移动到零点 * + *******************************************************************************/ + if (!m_Xgpio->getState()) { + { + ZLOGI(TAG, "---------STEP1-------- move to zero first"); + _rotate(-m_param.motor_run_to_zero_speed, m_param.motor_default_acc, m_param.motor_run_to_zero_dec); + bool xreach_zero = false; + while (!m_thread.getExitFlag()) { + // ZLOGI(TAG, "xgpio %d %d %d", m_Xgpio->getState(), m_stepM1->isReachTarget(), m_stepM2->isReachTarget()); + if (m_Xgpio->getState()) { + xreach_zero = true; + _motor_stop(-1); + + while (!m_stepM1->isStoped() && !m_thread.getExitFlag()) { // 等待电机停止 + _motor_stop(-1); + vTaskDelay(10); + } + + break; + } + vTaskDelay(1); + } + if (!xreach_zero) { + // 触发异常回调 + ZLOGE(TAG, "x reach zero failed"); + return err::kxymotor_not_found_x_zero_point; + } + } + } + + /******************************************************************************* + * 远离X零点 * + *******************************************************************************/ + { + ZLOGI(TAG, "---------STEP2-------- leave away zero"); + /** + * @brief 如果设备已经在零点，则反向移动一定距离远离零点 + */ + if (m_Xgpio->getState()) { + _rotate(m_param.motor_look_zero_edge_speed, m_param.motor_default_acc, m_param.motor_look_zero_edge_dec); + while (true) { + if (m_thread.getExitFlag()) break; + if (!m_Xgpio->getState()) { + _motor_stop(-1); + while (!m_stepM1->isStoped() && !m_thread.getExitFlag()) { // 等待电机停止 + vTaskDelay(1); + } + break; + } + vTaskDelay(1); + } + } + + if (m_thread.getExitFlag()) { + ZLOGI(TAG, "break move to zero thread exit"); + return err::kmodule_opeation_break_by_user; + } + if (m_Xgpio->getState()) { + ZLOGE(TAG, "leave away zero failed"); + return err::kxymotor_x_find_zero_edge_fail; + } + } + + if (m_thread.getExitFlag()) { + ZLOGI(TAG, "break move to zero thread exit"); + return 0; + } + + ZLOGI(TAG, "x reach zero"); + ZLOGI(TAG, "move_to_zero success"); + + return 0; +} \ No newline at end of file diff --git a/components/water_cooling_temperature_control_module/water_cooling_temperature_control_module.hpp b/components/water_cooling_temperature_control_module/water_cooling_temperature_control_module.hpp index 8b0f372..4e108c2 100644 --- a/components/water_cooling_temperature_control_module/water_cooling_temperature_control_module.hpp +++ b/components/water_cooling_temperature_control_module/water_cooling_temperature_control_module.hpp @@ -88,7 +88,7 @@ class WaterCoolingTemperatureControlModule : public ZIModule { virtual int32_t module_active_cfg() ; - virtual int32_t module_stop() override; + virtual int32_t module_stop() ; virtual int32_t module_start() ; virtual int32_t module_clear_error() override; diff --git a/components/zcancmder/zcan_protocol_parser.cpp b/components/zcancmder/zcan_protocol_parser.cpp index af4da9e..379a32c 100644 --- a/components/zcancmder/zcan_protocol_parser.cpp +++ b/components/zcancmder/zcan_protocol_parser.cpp @@ -15,8 +15,8 @@ void ZCanProtocolParser::initialize(IZCanReceiver* cancmder) { m_cancmder->registerListener(this); REGFN(module_ping); - REGFN(module_stop); - REGFN(module_break); + // REGFN(module_stop); + // REGFN(module_break); // REGFN(module_get_last_exec_status); REGFN(module_get_status); REGFN(module_set_reg); @@ -31,8 +31,8 @@ void ZCanProtocolParser::initialize(IZCanReceiver* cancmder) { // REGFN(module_factory_reset); // REGFN(module_flush_cfg); // REGFN(module_active_cfg); - REGFN(module_read_raw); - REGFN(module_enable); + // REGFN(module_read_raw); + // REGFN(module_enable); // REGFN(module_start); REGFN(step_motor_enable); @@ -137,14 +137,14 @@ int32_t ZCanProtocolParser::module_ping(cmdcontxt_t* cxt) { CHECK_AND_GET_MODULE(0); return module->module_ping(); } -int32_t ZCanProtocolParser::module_stop(cmdcontxt_t* cxt) { - CHECK_AND_GET_MODULE(0); - return module->module_stop(); -} -int32_t ZCanProtocolParser::module_break(cmdcontxt_t* cxt) { - CHECK_AND_GET_MODULE(0); - return module->module_break(); -} +// int32_t ZCanProtocolParser::module_stop(cmdcontxt_t* cxt) { +// CHECK_AND_GET_MODULE(0); +// return module->module_stop(); +// } +// int32_t ZCanProtocolParser::module_break(cmdcontxt_t* cxt) { +// CHECK_AND_GET_MODULE(0); +// return module->module_break(); +// } // int32_t ZCanProtocolParser::module_get_last_exec_status(cmdcontxt_t* cxt) { // CHECK_AND_GET_MODULE(0); // int32_t* ack = (int32_t*)cxt->ackbuf; @@ -215,16 +215,16 @@ int32_t ZCanProtocolParser::module_clear_error(cmdcontxt_t* cxt) { // CHECK_AND_GET_MODULE(0); // return module->module_active_cfg(); // } -int32_t ZCanProtocolParser::module_read_raw(cmdcontxt_t* cxt) { - CHECK_AND_GET_MODULE(1); - int32_t* ack = (int32_t*)cxt->ackbuf; - cxt->acklen = ZCANCMD_READ_BUF_MAX_SIZE; - return module->module_read_raw(cxt->param[0], cxt->ackbuf, &cxt->acklen); -} -int32_t ZCanProtocolParser::module_enable(cmdcontxt_t* cxt) { - CHECK_AND_GET_MODULE(1); - return module->module_enable(cxt->param[0]); -} +// int32_t ZCanProtocolParser::module_read_raw(cmdcontxt_t* cxt) { +// CHECK_AND_GET_MODULE(1); +// int32_t* ack = (int32_t*)cxt->ackbuf; +// cxt->acklen = ZCANCMD_READ_BUF_MAX_SIZE; +// return module->module_read_raw(cxt->param[0], cxt->ackbuf, &cxt->acklen); +// } +// int32_t ZCanProtocolParser::module_enable(cmdcontxt_t* cxt) { +// CHECK_AND_GET_MODULE(1); +// return module->module_enable(cxt->param[0]); +// } // int32_t ZCanProtocolParser::module_start(cmdcontxt_t* cxt) { // CHECK_AND_GET_MODULE(0); // return module->module_start();