diff --git a/components/step_motor_45/step_motor_45.hpp b/components/step_motor_45/step_motor_45.hpp index f64c124..6505044 100644 --- a/components/step_motor_45/step_motor_45.hpp +++ b/components/step_motor_45/step_motor_45.hpp @@ -133,7 +133,7 @@ class StepMotor45 : public ZIModule, public ZIMotor { virtual int32_t motor_move_to_zero_forward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) 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); + virtual int32_t motor_read_pos(int32_t *pos) override; private: bool getzeropinstate(); 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 c8aaac0..3de2073 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp @@ -252,10 +252,7 @@ int32_t StepMotorCtrlModule::rotate(int32_t speed, int32_t lastforms, action_cb_ 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.acc, m_param.dec); - - m_stepM1->setAcceleration(m_param.acc); - m_stepM1->setDeceleration(m_param.dec); - m_stepM1->rotate(speed); + _rotate(speed, m_param.acc, m_param.dec); int32_t startticket = zos_get_tick(); bool reachtime = false; @@ -380,9 +377,9 @@ int32_t StepMotorCtrlModule::exec_move_to_zero_task() { if (!m_Xgpio->getState()) { { ZLOGI(TAG, "---------STEP1-------- move to zero first"); - _motor_move_by(-m_param.run_to_zero_max_d, m_param.run_to_zero_speed, m_param.acc, m_param.run_to_zero_dec); + _rotate(-m_param.run_to_zero_speed, m_param.acc, m_param.run_to_zero_dec); bool xreach_zero = false; - while (!m_thread.getExitFlag() && !_motor_is_reach_target()) { + 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; @@ -414,8 +411,8 @@ int32_t StepMotorCtrlModule::exec_move_to_zero_task() { * @brief 如果设备已经在零点，则反向移动一定距离远离零点 */ if (m_Xgpio->getState()) { - _motor_move_by(m_param.look_zero_edge_max_d, m_param.look_zero_edge_speed, m_param.acc, m_param.look_zero_edge_dec); - while (!_motor_is_reach_target()) { + _rotate(m_param.look_zero_edge_speed, m_param.acc, m_param.look_zero_edge_dec); + while (true) { if (m_thread.getExitFlag()) break; if (!m_Xgpio->getState()) { _motor_stop(-1); @@ -434,42 +431,6 @@ int32_t StepMotorCtrlModule::exec_move_to_zero_task() { return err::kxymotor_x_find_zero_edge_fail; } } - - /******************************************************************************* - * 移动X轴到零点 * - *******************************************************************************/ - // { - // ZLOGI(TAG, "---------STEP3-------- move to zero"); - // _motor_move_by(-m_param.run_to_zero_move_to_zero_max_d, m_param.run_to_zero_speed, m_param.acc, m_param.run_to_zero_dec); - // bool xreach_zero = false; - // while (!m_thread.getExitFlag() && !_motor_is_reach_target()) { - // // ZLOGI(TAG, "xgpio %d %d %d", m_Xgpio->getState(), m_stepM1->isReachTarget(), m_stepM2->isReachTarget()); - // if (m_Xgpio->getState()) { - // xreach_zero = true; - // _motor_stop(-1); - - // while (!m_stepM1->isStoped()) { // 等待电机停止 - // _motor_stop(-1); - // vTaskDelay(10); - // } - - // break; - // } - // vTaskDelay(1); - // } - - // if (m_thread.getExitFlag()) { - // ZLOGI(TAG, "break move to zero thread exit"); - // return err::kmodule_opeation_break_by_user; - // } - - // if (!xreach_zero) { - // // 触发异常回调 - // ZLOGE(TAG, "x reach zero failed"); - // return err::kxymotor_not_found_x_zero_point; - // } - // } - if (m_thread.getExitFlag()) { ZLOGI(TAG, "break move to zero thread exit"); return 0; @@ -480,6 +441,12 @@ int32_t StepMotorCtrlModule::exec_move_to_zero_task() { 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); +} void StepMotorCtrlModule::getnowpos(int32_t& pos) { int32_t motor_pos = m_stepM1->getXACTUAL(); @@ -638,3 +605,183 @@ void StepMotorCtrlModule::active_cfg() { m_stepM1->setScale(m_param.distance_scale); m_stepM1->setMotorShaft(m_param.x_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 = m_lastexecstatus; + return 0; +} +int32_t StepMotorCtrlModule::module_get_status(int32_t* status) { + *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_set_param(int32_t param_id, int32_t param_value) { + if (param_id == kcfg_motor_x_shift) { + m_param.shift_x = param_value; + return 0; + } + + if (param_id == kcfg_motor_x_shaft) { + m_param.x_shaft = param_value; + return 0; + } + + if (param_id == kcfg_motor_x_one_circle_pulse) { + m_param.distance_scale = param_value; + return 0; + } + + if (param_id == k_cfg_stepmotor_ihold) { + m_param.ihold = param_value; + return 0; + } + + if (param_id == k_cfg_stepmotor_irun) { + m_param.irun = param_value; + return 0; + } + + if (param_id == k_cfg_stepmotor_iholddelay) { + m_param.iholddelay = param_value; + return 0; + } + return err::kmodule_not_find_config_index; +} +int32_t StepMotorCtrlModule::module_get_param(int32_t param_id, int32_t* param_value) { + if (param_id == kcfg_motor_x_shift) { + *param_value = m_param.shift_x; + return 0; + } + + if (param_id == kcfg_motor_x_shaft) { + *param_value = m_param.x_shaft; + return 0; + } + + if (param_id == kcfg_motor_x_one_circle_pulse) { + *param_value = m_param.distance_scale; + return 0; + } + + if (param_id == k_cfg_stepmotor_ihold) { + *param_value = m_param.ihold; + return 0; + } + + if (param_id == k_cfg_stepmotor_irun) { + *param_value = m_param.irun; + return 0; + } + + if (param_id == k_cfg_stepmotor_iholddelay) { + *param_value = m_param.iholddelay; + return 0; + } + return err::kmodule_not_find_config_index; +} + +int32_t StepMotorCtrlModule::module_clear_error() { return 0; } + +int32_t StepMotorCtrlModule::module_readio(int32_t* io) { + *io = 0; + for (size_t i = 0; i < m_nio; i++) { + if (m_iotable[i].getState()) { + *io |= (0x01 << i); + } + } + return 0; +} +int32_t StepMotorCtrlModule::module_writeio(int32_t io) { return 0; } + +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 enable) { + zlock_guard l(m_lock); + ZLOGI(TAG, "m%d motor_enable %d", m_id, enable); + if (enable != 0) { + m_stepM1->enable(true); + } else { + m_stepM1->enable(false); + } + 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_status_cb = nullptr; + m_thread.stop(); + _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); + ZLOGI(TAG, "m%d motor_move_to %d", m_id, tox); + m_status_cb = nullptr; + m_thread.stop(); + + if (m_param.min_x != 0 && tox < m_param.min_x) { + tox = m_param.min_x; + } + if (m_param.max_x != 0 && tox > m_param.max_x) { + tox = m_param.max_x; + } + m_thread.start( + [this, tox, motor_velocity, acc]() { + _motor_move_to(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(); }); + return 0; +} +int32_t StepMotorCtrlModule::motor_move_to_with_torque(int32_t pos, int32_t torque) { return err::koperation_not_support; } + +int32_t StepMotorCtrlModule::motor_rotate_acctime(int32_t direction, int32_t motor_velocity, int32_t acctime) { return err::koperation_not_support; } +int32_t StepMotorCtrlModule::motor_move_by_acctime(int32_t distance, int32_t motor_velocity, int32_t acctime) { return err::koperation_not_support; } +int32_t StepMotorCtrlModule::motor_move_to_acctime(int32_t position, int32_t motor_velocity, int32_t acctime) { return err::koperation_not_support; } + +int32_t StepMotorCtrlModule::motor_move_to_zero_forward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) { return err::koperation_not_support; } +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"); + m_param.run_to_zero_speed = findzerospeed; + m_param.look_zero_edge_speed = findzeroedge_speed; + m_param.look_zero_edge_dec = acc; + m_param.run_to_zero_dec = acc; + return move_to_zero(nullptr); +} + +int32_t StepMotorCtrlModule::motor_read_pos(int32_t* pos) { + int32_t xnow = 0; + getnowpos(xnow); + *pos = xnow; + 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 93eb26b..d060441 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp @@ -5,7 +5,7 @@ #include "sdk\components\zcancmder\zcanreceiver.hpp" #include "sdk\components\zprotocols\zcancmder\api\i_step_motor_ctrl_module.hpp" namespace iflytop { -class StepMotorCtrlModule : public I_StepMotorCtrlModule { +class StepMotorCtrlModule : public I_StepMotorCtrlModule, public ZIModule, public ZIMotor { public: private: IStepperMotor* m_stepM1; @@ -33,7 +33,7 @@ class StepMotorCtrlModule : public I_StepMotorCtrlModule { action_cb_status_t m_status_cb; public: - void initialize(int id, IStepperMotor* stepM, ZGPIO iotable[], int nio, const char* flashmark); + void initialize(int moduleid, IStepperMotor* stepM, ZGPIO iotable[], int nio, const char* flashmark); static void create_default_cfg(flash_config_t& cfg); virtual bool isbusy() override; @@ -99,6 +99,48 @@ class StepMotorCtrlModule : public I_StepMotorCtrlModule { int32_t do_motor_action_block_2(function action, function break_condition); + /******************************************************************************* + * OVERRIDE MODULE * + *******************************************************************************/ + + 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_set_param(int32_t param_id, int32_t param_value) override; + virtual int32_t module_get_param(int32_t param_id, int32_t* param_value) override; + + virtual int32_t module_readio(int32_t* io) override; + virtual int32_t module_writeio(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; + + /******************************************************************************* + * 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_with_torque(int32_t pos, int32_t torque) override; + + virtual int32_t motor_rotate_acctime(int32_t direction, int32_t motor_velocity, int32_t acctime) override; + virtual int32_t motor_move_by_acctime(int32_t distance, int32_t motor_velocity, int32_t acctime) override; + virtual int32_t motor_move_to_acctime(int32_t position, int32_t motor_velocity, int32_t acctime) override; + + virtual int32_t motor_move_to_zero_forward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) 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; + private: void active_cfg(); @@ -114,6 +156,7 @@ class StepMotorCtrlModule : public I_StepMotorCtrlModule { void _motor_move_by(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); diff --git a/components/zprotocols/zcancmder_v2 b/components/zprotocols/zcancmder_v2 index 95626e2..87d1a54 160000 --- a/components/zprotocols/zcancmder_v2 +++ b/components/zprotocols/zcancmder_v2 @@ -1 +1 @@ -Subproject commit 95626e25193bdb17428bf4edca04af30f1306c04 +Subproject commit 87d1a547ef95f5739c1177063dfb8b3db6b6df81