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 641716b..ca4682e 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp @@ -474,6 +474,62 @@ int32_t StepMotorCtrlModule::step_motor_easy_move_to_end_point() { } return do_step_motor_easy_move_to_end_point(); } +int32_t StepMotorCtrlModule::step_motor_easy_reciprocating_motion(int32_t startpos, int32_t endpos, int32_t times) { + ZLOGI(TAG, "m%d motor_reciprocating_motion %d %d %d", m_id, startpos, endpos, times); + + int32_t ecode = check_befor_run(); + if (ecode != 0) return ecode; + + int32_t x_startpos; + int32_t x_endpos; + int32_t x_beginpos; + getnowpos(x_beginpos); + + if (m_cfg.min_d != 0 && x_startpos < m_cfg.min_d) x_startpos = m_cfg.min_d; + if (m_cfg.max_d != 0 && x_startpos > m_cfg.max_d) x_startpos = m_cfg.max_d; + + if (m_cfg.min_d != 0 && x_endpos < m_cfg.min_d) x_endpos = m_cfg.min_d; + if (m_cfg.max_d != 0 && x_endpos > m_cfg.max_d) x_endpos = m_cfg.max_d; + m_thread.stop(); + m_thread.start( + [this, x_startpos, x_endpos, x_beginpos, times]() { + befor_motor_move(); + + for (size_t i = 0; i < times; i++) { + ZLOGI(TAG, "reciprocating_motion move pos:%d", x_startpos); + moveTo(x_startpos, m_cfg.motor_default_velocity); + while (!m_stepM1->isReachTarget()) { + if (m_thread.getExitFlag()) break; + if (!check_when_run()) break; + + vTaskDelay(5); + } + + ZLOGI(TAG, "reciprocating_motion move pos:%d", x_endpos); + moveTo(x_endpos, m_cfg.motor_default_velocity); + while (!m_stepM1->isReachTarget()) { + if (m_thread.getExitFlag()) break; + if (!check_when_run()) break; + + vTaskDelay(5); + } + } + + ZLOGI(TAG, "reciprocating_motion finish,move begin pos:%d", x_beginpos); + moveTo(x_beginpos, 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]() { m_stepM1->stop(); }); + + return 0; +} int32_t StepMotorCtrlModule::do_step_motor_easy_rotate(int32_t direction) { m_thread.stop(); 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 5ec18a1..a56ccc5 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp @@ -84,6 +84,8 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor { virtual int32_t step_motor_easy_move_to_zero() override; virtual int32_t step_motor_easy_move_to_io(int32_t ioindex, int32_t direction) override; virtual int32_t step_motor_easy_move_to_end_point() override; + virtual int32_t step_motor_easy_reciprocating_motion(int32_t startpos, int32_t endpos, int32_t times) override; + virtual int32_t step_motor_easy_set_current_pos(int32_t pos) override; virtual int32_t step_motor_active_cfg() override; @@ -97,6 +99,8 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor { virtual int32_t step_motor_read_io_index_in_stm32(int32_t ioindex, int32_t* index_in_stm32) override; + + public: TMC51X0* getMotor() { return m_stepM1; } config_t* getCfg() { return &m_cfg; } diff --git a/components/zcancmder/zcan_protocol_parser.cpp b/components/zcancmder/zcan_protocol_parser.cpp index 4f4fd82..42029ea 100644 --- a/components/zcancmder/zcan_protocol_parser.cpp +++ b/components/zcancmder/zcan_protocol_parser.cpp @@ -35,6 +35,7 @@ void ZCanProtocolParser::initialize(IZCanReceiver* cancmder) { REGFN(step_motor_stop); REGFN(step_motor_read_io_state); REGFN(step_motor_easy_move_to_end_point); + REGFN(step_motor_easy_reciprocating_motion); REGFN(step_motor_read_tmc5130_status); REGFN(step_motor_read_tmc5130_state); @@ -322,6 +323,11 @@ int32_t ZCanProtocolParser::step_motor_easy_move_to_end_point(cmdcontxt_t* cxt) CHECK_AND_GET_MODULE(0); return module->step_motor_easy_move_to_end_point(); } +// step_motor_easy_reciprocating_motion +int32_t ZCanProtocolParser::step_motor_easy_reciprocating_motion(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(3); + return module->step_motor_easy_reciprocating_motion(cxt->params[0], cxt->params[1], cxt->params[2]); +} int32_t ZCanProtocolParser::step_motor_read_tmc5130_status(cmdcontxt_t* cxt) { CHECK_AND_GET_MODULE(0); @@ -762,8 +768,6 @@ int32_t ZCanProtocolParser::a8000_optical_read_raw(cmdcontxt_t* cxt) { return suc; } - - int32_t ZCanProtocolParser::a8k_opt_v2_t_start_scan(cmdcontxt_t* cxt) { CHECK_AND_GET_MODULE(3); return module->a8k_opt_v2_t_start_scan(cxt->params[0], cxt->params[1], cxt->params[2]); diff --git a/components/zcancmder/zcan_protocol_parser.hpp b/components/zcancmder/zcan_protocol_parser.hpp index 7dc4219..9c419dc 100644 --- a/components/zcancmder/zcan_protocol_parser.hpp +++ b/components/zcancmder/zcan_protocol_parser.hpp @@ -77,6 +77,9 @@ class ZCanProtocolParser : public IZCanReceiverListener { CMDFN(step_motor_read_io_index_in_stm32); CMDFN(step_motor_set_subdevice_reg); CMDFN(step_motor_get_subdevice_reg); + CMDFN(step_motor_easy_reciprocating_motion); + + CMDFN(mini_servo_enable); CMDFN(mini_servo_read_pos);