diff --git a/sdk/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp b/sdk/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp index 787f638..2c1388f 100644 --- a/sdk/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp +++ b/sdk/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp @@ -21,7 +21,7 @@ void XYRobotCtrlModule::initialize(int32_t id, TMC51X0* stepM1, TMC51X0* stepM2, m_default_cfg = default_cfg; m_thread.init("XYRobotCtrlModule", 1024, osPriorityNormal); module_active_cfg(); - xymotor_enable(1); + xymotor_enable(0); m_state.has_move_to_zero = 0; } @@ -598,22 +598,32 @@ int32_t XYRobotCtrlModule::xymotor_set_pos(int32_t x, int32_t y) { } int32_t XYRobotCtrlModule::xymotor_motor_move_by_direct(int32_t motor1_dpos, int32_t motor2_dpos) { - - TMC51X0::VCfg_t vcfg; - vcfg.a1 = m_cfg.a1; - vcfg.amax = m_cfg.amax; - vcfg.v1 = m_cfg.v1; - vcfg.dmax = m_cfg.dmax; - vcfg.d1 = m_cfg.d1; - vcfg.vstart = m_cfg.vstart; - vcfg.vstop = m_cfg.vstop; - vcfg.vmax = m_cfg.default_velocity; - m_stepM1->set_vcfg(&vcfg); - m_stepM2->set_vcfg(&vcfg); - - - m_stepM1->moveBy(motor1_dpos); - m_stepM2->moveBy(motor2_dpos); + m_thread.stop(); + module_status = 1; + m_thread.start( + [this, motor1_dpos, motor2_dpos]() { + TMC51X0::VCfg_t vcfg; + vcfg.a1 = m_cfg.a1; + vcfg.amax = m_cfg.amax; + vcfg.v1 = m_cfg.v1; + vcfg.dmax = m_cfg.dmax; + vcfg.d1 = m_cfg.d1; + vcfg.vstart = m_cfg.vstart; + vcfg.vstop = m_cfg.vstop; + vcfg.vmax = m_cfg.default_velocity; + m_stepM1->set_vcfg(&vcfg); + m_stepM2->set_vcfg(&vcfg); + + m_stepM1->moveBy(motor1_dpos); + m_stepM2->moveBy(motor2_dpos); + while (true) { + if (_motor_is_reach_target()) break; + if (m_thread.getExitFlag()) break; + vTaskDelay(5); + } + module_status = 0; + }, + [this]() {}); return 0; } int32_t XYRobotCtrlModule::xymotor_read_enc_direct(int32_t* enc1, int32_t* enc2) { diff --git a/usrc/version.h b/usrc/version.h index c83c8cc..392da9d 100644 --- a/usrc/version.h +++ b/usrc/version.h @@ -1,2 +1,2 @@ #pragma once -#define APP_VERSION 1209 +#define APP_VERSION 1210