From c28ee14978fc41a44187baa0a7af7eb9c8b7e22d Mon Sep 17 00:00:00 2001 From: zhaohe Date: Sun, 2 Jun 2024 20:46:05 +0800 Subject: [PATCH] update --- .../step_motor_ctrl_module.cpp | 320 +++++++- .../step_motor_ctrl_module.hpp | 42 +- .../step_motor_ctrl_module.hpp.bak | 206 ------ .../step_motor_ctrl_module_cpp.bak | 810 --------------------- 4 files changed, 335 insertions(+), 1043 deletions(-) delete mode 100644 components/step_motor_ctrl_module/step_motor_ctrl_module.hpp.bak delete mode 100644 components/step_motor_ctrl_module/step_motor_ctrl_module_cpp.bak 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 7a2e0dc..d090307 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp @@ -8,22 +8,316 @@ using namespace iflytop; #define TAG "SMCM" -void StepMotorCtrlModule::initialize(int moduleid, IStepperMotor* stepM, ZGPIO* zero, ZGPIO* end, config_t* cfg) {} +void StepMotorCtrlModule::initialize(int moduleid, IStepperMotor* stepM, ZGPIO iotable[], int nio, config_t* cfg) { + m_id = moduleid; + m_stepM1 = stepM; -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; } + m_iotable = iotable; + m_nio = nio; + m_thread.init(TAG, 1024, osPriorityNormal); + m_cfg = *cfg; + + m_state.enable = false; + m_state.dpos = 0; + + step_motor_active_cfg(); +} + +/*********************************************************************************************************************** + * EXT * + ***********************************************************************************************************************/ +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, inter_get_pos(val), ACTION_NONE); + PROCESS_REG(kreg_step_motor_shaft, REG_GET(m_cfg.motor_shaft), REG_SET(m_cfg.motor_shaft)); + PROCESS_REG(kreg_step_motor_one_circle_pulse, REG_GET(m_cfg.motor_one_circle_pulse), REG_SET(m_cfg.motor_one_circle_pulse)); + PROCESS_REG(kreg_step_motor_one_circle_pulse_denominator, REG_GET(m_cfg.motor_one_circle_pulse_denominator), REG_SET(m_cfg.motor_one_circle_pulse_denominator)); + PROCESS_REG(kreg_step_motor_default_velocity, REG_GET(m_cfg.motor_default_velocity), REG_SET(m_cfg.motor_default_velocity)); + PROCESS_REG(kreg_step_motor_default_acc, REG_GET(m_cfg.motor_default_acc), REG_SET(m_cfg.motor_default_acc)); + PROCESS_REG(kreg_step_motor_default_dec, REG_GET(m_cfg.motor_default_dec), REG_SET(m_cfg.motor_default_dec)); + PROCESS_REG(kreg_step_motor_run_to_zero_speed, REG_GET(m_cfg.motor_run_to_zero_speed), REG_SET(m_cfg.motor_run_to_zero_speed)); + PROCESS_REG(kreg_step_motor_run_to_zero_dec, REG_GET(m_cfg.motor_run_to_zero_dec), REG_SET(m_cfg.motor_run_to_zero_dec)); + PROCESS_REG(kreg_step_motor_look_zero_edge_speed, REG_GET(m_cfg.motor_look_zero_edge_speed), REG_SET(m_cfg.motor_look_zero_edge_speed)); + PROCESS_REG(kreg_step_motor_look_zero_edge_dec, REG_GET(m_cfg.motor_look_zero_edge_dec), REG_SET(m_cfg.motor_look_zero_edge_dec)); + PROCESS_REG(kreg_step_motor_ihold, REG_GET(m_cfg.stepmotor_ihold), REG_SET(m_cfg.stepmotor_ihold)); + PROCESS_REG(kreg_step_motor_irun, REG_GET(m_cfg.stepmotor_irun), REG_SET(m_cfg.stepmotor_irun)); + PROCESS_REG(kreg_step_motor_iholddelay, REG_GET(m_cfg.stepmotor_iholddelay), REG_SET(m_cfg.stepmotor_iholddelay)); + PROCESS_REG(kreg_step_motor_max_d, REG_GET(m_cfg.max_d), REG_SET(m_cfg.max_d)); + PROCESS_REG(kreg_step_motor_min_d, REG_GET(m_cfg.min_d), REG_SET(m_cfg.min_d)); + + default: + return err::kmodule_not_find_config_index; + break; + } + 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); + m_state.enable = enable; + return 0; +} +int32_t StepMotorCtrlModule::step_motor_read_pos(int32_t* pos) { + int32_t motor_pos = m_stepM1->getXACTUAL(); + inter_inverse_kinematics(motor_pos, *pos); + return 0; +} +int32_t StepMotorCtrlModule::step_motor_stop(int32_t breakstop) { + ZLOGI(TAG, "m%d motor_stop %d", m_id, breakstop); + m_thread.stop(); + m_stepM1->stop(); + return 0; } -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; } -int32_t StepMotorCtrlModule::step_motor_easy_move_by(int32_t distance) { return 0; } -int32_t StepMotorCtrlModule::step_motor_easy_move_to(int32_t position) { return 0; } -int32_t StepMotorCtrlModule::step_motor_easy_move_to_zero() { return 0; } -int32_t StepMotorCtrlModule::step_motor_easy_set_current_pos(int32_t pos) { return 0; } -int32_t StepMotorCtrlModule::step_motor_easy_move_to_io(int32_t ioindex, int32_t direction) { return 0; } -int32_t StepMotorCtrlModule::step_motor_active_cfg() { return 0; } \ No newline at end of file +int32_t StepMotorCtrlModule::step_motor_easy_rotate(int32_t direction) { + ZLOGI(TAG, "m%d motor_rotate %d", m_id, direction); + m_thread.stop(); + + m_thread.start( + [this, direction]() { + befor_motor_move(); + + { + m_stepM1->setAcceleration(m_cfg.motor_default_acc); + m_stepM1->setDeceleration(m_cfg.motor_default_dec); + + int32_t vel = m_cfg.motor_default_velocity; + if (direction == 0) vel = -vel; + m_stepM1->rotate(vel); + + while (true) { + if (m_thread.getExitFlag()) break; + if (!check_when_run()) break; + vTaskDelay(5); + } + } + + after_motor_move(); + }, + [this]() { m_stepM1->stop(); }); + + return 0; +} +int32_t StepMotorCtrlModule::step_motor_easy_move_to(int32_t tox) { + ZLOGI(TAG, "m%d motor_move_to %d", m_id, tox); + m_thread.stop(); + if (!m_state.enable) { + return err::kstep_motor_not_enable; + } + + if (m_cfg.min_d != 0 && tox < m_cfg.min_d) tox = m_cfg.min_d; + if (m_cfg.max_d != 0 && tox > m_cfg.max_d) tox = m_cfg.max_d; + + m_thread.start( + [this, tox]() { + befor_motor_move(); + + { + m_stepM1->setAcceleration(m_cfg.motor_default_acc); + m_stepM1->setDeceleration(m_cfg.motor_default_dec); + int32_t motor_pos = 0; + inter_forward_kinematics(tox, motor_pos); + m_stepM1->moveTo(motor_pos, m_cfg.motor_default_velocity); + + while (!m_stepM1->isReachTarget()) { + if (m_thread.getExitFlag()) break; + if (!check_when_run()) break; + + vTaskDelay(5); + } + } + + after_motor_move(); + }, + [this, tox]() { m_stepM1->stop(); }); + + return 0; +} + +int32_t StepMotorCtrlModule::step_motor_easy_move_by(int32_t distance) { + ZLOGI(TAG, "m%d motor_move_by %d", m_id, distance); + int32_t motor_pos = 0; + step_motor_read_pos(&motor_pos); + motor_pos += distance; + step_motor_easy_move_to(motor_pos); + return 0; +} + +int32_t StepMotorCtrlModule::step_motor_easy_move_to_zero() { + ZLOGI(TAG, "m%d motor_move_to_zero", m_id); + m_thread.stop(); + if (!m_state.enable) { + return err::kstep_motor_not_enable; + } + + if (m_nio == 0) { + return err::kstep_motor_ioindex_out_of_range; + } + + m_thread.start( + [this]() { + befor_motor_move(); + exec_move_to_io_task(0, -1); + after_motor_move(); + }, + [this]() { m_stepM1->stop(); }); + return 0; +} + +int32_t StepMotorCtrlModule::step_motor_easy_move_to_io(int32_t ioindex, int32_t direction) { + ZLOGI(TAG, "m%d motor_move_to_io %d %d", m_id, ioindex, direction); + m_thread.stop(); + if (!m_state.enable) { + return err::kstep_motor_not_enable; + } + + if (ioindex < 0 || ioindex >= m_nio) { + return err::kstep_motor_ioindex_out_of_range; + } + + m_thread.start( + [this, ioindex, direction]() { + befor_motor_move(); + exec_move_to_io_task(ioindex, direction); + after_motor_move(); + }, + [this]() { m_stepM1->stop(); }); + + return 0; +} + +int32_t StepMotorCtrlModule::step_motor_easy_set_current_pos(int32_t pos) { + int32_t motor_pos = 0; + inter_forward_kinematics(pos, motor_pos); + m_stepM1->setXACTUAL(motor_pos); + return 0; +} + +int32_t StepMotorCtrlModule::step_motor_active_cfg() { + m_stepM1->enable(false); + m_stepM1->setIHOLD_IRUN(m_cfg.stepmotor_ihold, m_cfg.stepmotor_irun, m_cfg.stepmotor_iholddelay); + m_stepM1->setScale(m_cfg.motor_one_circle_pulse); + m_stepM1->setScaleDenominator(m_cfg.motor_one_circle_pulse_denominator); + m_stepM1->setMotorShaft(m_cfg.motor_shaft); + return 0; +} + +int32_t StepMotorCtrlModule::step_motor_read_io_state(int32_t ioindex, int32_t* state) { + if (ioindex < 0 || ioindex >= m_nio) { + return err::kstep_motor_ioindex_out_of_range; + } + *state = m_iotable[ioindex].getState(); + return 0; +} + +/*********************************************************************************************************************** + * INTER * + ***********************************************************************************************************************/ +void StepMotorCtrlModule::inter_inverse_kinematics(int32_t motor_pos, int32_t& x) { x = motor_pos; } +void StepMotorCtrlModule::inter_forward_kinematics(int32_t x, int32_t& motor_pos) { motor_pos = x; } +int StepMotorCtrlModule::inter_get_pos() { + int32_t pos; + int32_t motor_pos = m_stepM1->getXACTUAL(); + inter_inverse_kinematics(motor_pos, pos); + return pos; +} +int StepMotorCtrlModule::inter_get_pos(int32_t& x) { + x = inter_get_pos(); + return 0; +} +void StepMotorCtrlModule::befor_motor_move() { // + m_state.before_move_pos = m_stepM1->getXACTUAL(); + creg.m_module_status = 1; + creg.module_errorcode = 0; + creg.module_errorbitflag0 = 0; +} +void StepMotorCtrlModule::after_motor_move() { + m_state.after_move_pos = m_stepM1->getXACTUAL(); + m_state.dpos = m_state.after_move_pos - m_state.before_move_pos; + if (creg.module_errorcode != 0) { + creg.m_module_status = 2; + } + creg.m_module_status = 0; +} +bool StepMotorCtrlModule::check_when_run() { return true; } + +int32_t StepMotorCtrlModule::exec_move_to_io_task(int32_t ioindex, int32_t direction) { + int32_t runToPointSpeed = m_cfg.motor_run_to_zero_speed; + int32_t runToPointDec = m_cfg.motor_run_to_zero_dec; + + int32_t lookPointVelocity = m_cfg.motor_look_zero_edge_speed; + int32_t lookPointDec = m_cfg.motor_look_zero_edge_dec; + + 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; + m_stepM1->stop(); + break; + } + vTaskDelay(1); + } + + // 等待电机停止 + while (!m_stepM1->isStoped() && !m_thread.getExitFlag()) { + m_stepM1->stop(); + 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; + m_stepM1->stop(); + break; + } + vTaskDelay(1); + } + + while (!m_stepM1->isStoped() && !m_thread.getExitFlag()) { // 等待电机停止 + m_stepM1->stop(); + 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; +} + +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); +} \ 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 a3881c6..6850bf5 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp @@ -22,34 +22,34 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor { int32_t min_d; int32_t max_d; // - int32_t motor_shift; - // - int32_t motor_run_to_zero_max_d; int32_t motor_run_to_zero_speed; int32_t motor_run_to_zero_dec; // - int32_t motor_look_zero_edge_max_d; int32_t motor_look_zero_edge_speed; int32_t motor_look_zero_edge_dec; } config_t; - typedef std::function action_cb_status_t; + typedef struct { + int32_t dpos; + int32_t after_move_pos; + int32_t before_move_pos; + int32_t enable; + } state_t; private: - IStepperMotor* m_stepM1; - int m_id = 0; + IStepperMotor* m_stepM1 = nullptr; + int m_id = 0; config_t m_cfg; + state_t m_state; - ZGPIO* m_zero_gpio; - ZGPIO* m_end_gpio; + ZGPIO* m_iotable = nullptr; + int m_nio = 0; ZThread m_thread; - zmutex m_lock; - zmutex m_statu_lock; - public: - void initialize(int moduleid, IStepperMotor* stepM, ZGPIO* zero, ZGPIO* end, config_t* cfg); + void initialize(int moduleid, IStepperMotor* stepM, ZGPIO iotable[], int nio, config_t* cfg); + static void create_default_cfg(config_t& cfg); /*********************************************************************************************************************** @@ -59,7 +59,6 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor { *id = m_id; return 0; } - 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 * @@ -75,5 +74,20 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor { virtual int32_t step_motor_easy_set_current_pos(int32_t pos); virtual int32_t step_motor_easy_move_to_io(int32_t ioindex, int32_t direction); virtual int32_t step_motor_active_cfg(); + virtual int32_t step_motor_read_io_state(int32_t ioindex, int32_t* state); + + private: + void inter_inverse_kinematics(int32_t motor_pos, int32_t& x); + void inter_forward_kinematics(int32_t x, int32_t& motor_pos); + + int inter_get_pos(); + int inter_get_pos(int32_t& x); + + void befor_motor_move(); + void after_motor_move(); + bool check_when_run(); + + int32_t exec_move_to_io_task(int32_t ioindex, int32_t direction); + void _rotate(int32_t vel, int32_t acc, int32_t dec); }; } // namespace iflytop \ No newline at end of file diff --git a/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp.bak b/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp.bak deleted file mode 100644 index 131ef2b..0000000 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp.bak +++ /dev/null @@ -1,206 +0,0 @@ -#pragma once -// -#include "sdk/os/zos.hpp" -#include "sdk\components\tmc\basic\tmc_ic_interface.hpp" -#include "sdk\components\zcancmder\zcanreceiver.hpp" -namespace iflytop { -class StepMotorCtrlModule : public ZIModule, public ZIStepMotor { - ENABLE_MODULE(StepMotorCtrlModule, kmotor_module, 0x0001); - - public: - typedef struct { - int32_t motor_shaft; - int32_t motor_one_circle_pulse; // - int32_t motor_one_circle_pulse_denominator; // - int32_t stepmotor_ihold; - int32_t stepmotor_irun; - int32_t stepmotor_iholddelay; - int32_t stepmotor_iglobalscaler; - int32_t motor_default_acc; - int32_t motor_default_dec; - int32_t motor_default_velocity; - int32_t min_d; - int32_t max_d; - // - int32_t motor_shift; - - int32_t motor_run_to_zero_max_d; - int32_t motor_run_to_zero_speed; - int32_t motor_run_to_zero_dec; - - int32_t motor_look_zero_edge_max_d; - int32_t motor_look_zero_edge_speed; - int32_t motor_look_zero_edge_dec; - } base_param_t; - - typedef struct { - uint8_t index; - int32_t acc; - int32_t dec; - int32_t velocity; - int32_t x; - } logic_point_t; - - typedef struct { - bool configInited; - base_param_t base_param; - logic_point_t logic_point[5]; - } flash_config_t; - typedef std::function action_cb_status_t; - - private: - IStepperMotor* m_stepM1; - int m_id = 0; - - ZGPIO* m_Xgpio; - ZGPIO* m_end_gpio; - - ZThread m_thread; - - zmutex m_lock; - zmutex m_statu_lock; - - flash_config_t m_cfg; - flash_config_t m_factory_config; - base_param_t& m_param = m_cfg.base_param; - int32_t m_default_speed = 0; - - ZGPIO* m_iotable; - int m_nio; - - int32_t m_calculate_curpos_result = 0; - const char* m_flashmark = nullptr; - // action_cb_status_t m_status_cb; - - // int32_t m_enable = 0; - - public: - void initialize(int moduleid, IStepperMotor* stepM, ZGPIO iotable[], int nio, const char* flashmark, flash_config_t* defaultcfg); - static void create_default_cfg(flash_config_t& cfg); - static uint32_t get_flash_cfg_size() { return sizeof(flash_config_t); } - - virtual bool isbusy(); - virtual int32_t get_last_exec_status() { return 0; }; - - virtual int32_t move_to_logic_point(int32_t logic_point_num, action_cb_status_t status_cb); - virtual int32_t set_logic_point(int logic_point_num, int32_t x, int32_t vel, int32_t acc, int32_t dec); - virtual int32_t get_logic_point(int logic_point_num, logic_point_t& logic_point); - - virtual int32_t move_to_block(int32_t tox, int overtime = 0); - virtual int32_t move_by_block(int32_t dx, int overtime = 0); - - virtual int32_t move_to_block(int32_t tox, int32_t velocity, int overtime = 0); - virtual int32_t move_by_block(int32_t dx, int32_t velocity, int overtime = 0); - - virtual int32_t move_to_zero_block(int overtime = 0); - virtual int32_t move_to_zero_with_calibrate_block(int32_t x, int overtime = 0); - - virtual int32_t enable(bool venable); - virtual int32_t stop(uint8_t stopType); - virtual int32_t force_change_current_pos(int32_t x); - - virtual int32_t read_pos(int32_t& pos); - virtual int32_t read_pos(); - virtual bool read_zero_io_state(); - - int32_t do_motor_action_block_2(function action, function break_condition); - - /******************************************************************************* - * OVERRIDE MODULE * - *******************************************************************************/ - virtual int32_t module_ping() { return 0; }; - - virtual int32_t getid(int32_t* id) override; - virtual int32_t module_stop() override; - virtual int32_t module_break() override; - virtual int32_t module_get_last_exec_status(int32_t* status) override; - virtual int32_t module_get_status(int32_t* status) override; - virtual int32_t module_get_error(int32_t* iserror) override; - virtual int32_t module_clear_error() override; - - virtual int32_t module_readio(int32_t* io) override; - - virtual int32_t module_read_adc(int32_t adcindex, int32_t* adc) override; - - virtual int32_t module_factory_reset() override; - virtual int32_t module_flush_cfg() override; - virtual int32_t module_active_cfg() override; - - virtual int32_t module_enable(int32_t enable) override { return motor_enable(enable); } - - /******************************************************************************* - * Motor * - *******************************************************************************/ - virtual int32_t motor_enable(int32_t enable) override; - virtual int32_t motor_rotate(int32_t direction, int32_t motor_velocity, int32_t acc) override; - virtual int32_t motor_move_by(int32_t distance, int32_t motor_velocity, int32_t acc) override; - virtual int32_t motor_move_to(int32_t position, int32_t motor_velocity, int32_t acc) override; - - virtual int32_t motor_move_to_zero_backward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) override; - virtual int32_t motor_read_pos(int32_t* pos) override; - - virtual int32_t motor_move_to_zero_backward_and_calculated_shift(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) override; - virtual int32_t motor_calculated_pos_by_move_to_zero() override; - - virtual int32_t motor_easy_rotate(int32_t direction) override; - virtual int32_t motor_easy_move_by(int32_t distance) override; - virtual int32_t motor_easy_move_to(int32_t position) override; - virtual int32_t motor_easy_move_to_zero(int32_t direction) override; - virtual int32_t motor_easy_move_to_io(int32_t ioindex, int32_t direction) override; - - IStepperMotor* _getStepMotor() { return m_stepM1; } - base_param_t& _get_base_param() { return m_param; } - - private: - void active_cfg(); - - int32_t do_motor_action_block(function action); - // void updateStatus(uint8_t status); - - void getnowpos(int32_t& pos); - - void _move_to_nolimit(int32_t pos, int32_t maxv, int32_t acc, int32_t dec); - void _move_by_nolimit(int32_t dpos, int32_t maxv, int32_t acc, int32_t dec); - void _motor_stop(int32_t dec = -1); - bool _motor_is_reach_target(); - void _rotate(int32_t vel, int32_t acc, int32_t dec); - - void inverse_kinematics(int32_t motor_pos, int32_t& x); - void forward_kinematics(int32_t x, int32_t& motor_pos); - - int32_t _move_to(int32_t tox, action_cb_status_t status_cb); - int32_t _move_by(int32_t dx, action_cb_status_t status_cb); - - void call_exec_status_cb(int32_t status, action_cb_status_t status_cb); - void set_last_exec_status(int32_t ecode, int32_t val0 = 0, int32_t val1 = 0, int32_t val2 = 0, int32_t val3 = 0, int32_t val4 = 0); - - private: - int32_t _read_io(); - -#if 1 - int32_t move_to(int32_t tox, action_cb_status_t status_cb); - int32_t move_by(int32_t dx, action_cb_status_t status_cb); - int32_t move_to(int32_t x, int32_t velocity, action_cb_status_t status_cb); - int32_t move_by(int32_t dx, int32_t velocity, action_cb_status_t status_cb); - int32_t calculate_curpos(action_cb_status_t status_cb); - int32_t move_to_zero(action_cb_status_t status_cb); - int32_t move_to_zero_with_calibrate(int32_t x, action_cb_status_t status_cb); - int32_t rotate(int32_t speed, int32_t lastforms, action_cb_status_t status_cb); - int32_t read_calculate_curpos_action_result(int32_t& pos); - int32_t flush(); - int32_t factory_reset(); -#endif - - private: - int32_t module_xxx_reg(int32_t param_id, bool read, int32_t& val); - - private: - /******************************************************************************* - * EXEC_TASK * - *******************************************************************************/ - int32_t _exec_move_to_zero_task(int32_t& dx); - int32_t _exec_move_to_zero_task(); - int32_t _exec_move_to_io_task(int32_t ioindex, int32_t direction); - int32_t _exec_move_to_io_task_fn(int32_t ioindex, int32_t direction); -}; -} // namespace iflytop \ No newline at end of file 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