From ec0227ca837bb4ec0d65e99b79e7bb70c26d3876 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Wed, 27 Sep 2023 14:30:56 +0800 Subject: [PATCH] update --- components/errorcode/errorcode.hpp | 16 ++++--- components/tmc/ic/ztmc4361A.hpp | 4 +- .../xy_robot_ctrl_module/xy_robot_ctrl_module.cpp | 50 +++++++++++++++++++++- .../xy_robot_ctrl_module/xy_robot_ctrl_module.hpp | 4 +- .../zcancmder_module/zcan_xy_robot_module.cpp | 15 +++++-- 5 files changed, 72 insertions(+), 17 deletions(-) diff --git a/components/errorcode/errorcode.hpp b/components/errorcode/errorcode.hpp index cb2a5ce..f3f12aa 100644 --- a/components/errorcode/errorcode.hpp +++ b/components/errorcode/errorcode.hpp @@ -59,13 +59,15 @@ typedef enum { * 50000 */ - kcommon_error_device_not_zero = ERROR_CODE(50000, 0), // 设备未归零 - kcommon_error_over_temperature = ERROR_CODE(50000, 1), // 过温 - kcommon_error_over_voltage = ERROR_CODE(50000, 2), // 过压 - kcommon_error_param_out_of_range = ERROR_CODE(50000, 3), // 参数超出范围 - kcommon_error_not_found_zero_point = ERROR_CODE(50000, 4), // 未找到零点 - kcommon_error_not_found_x_zero_point = ERROR_CODE(50000, 5), // 未找到零点 - kcommon_error_not_found_y_zero_point = ERROR_CODE(50000, 6), // 未找到零点 + kcommon_error_device_not_zero = ERROR_CODE(50000, 0), // 设备未归零 + kcommon_error_over_temperature = ERROR_CODE(50000, 1), // 过温 + kcommon_error_over_voltage = ERROR_CODE(50000, 2), // 过压 + kcommon_error_param_out_of_range = ERROR_CODE(50000, 3), // 参数超出范围 + kcommon_error_not_found_zero_point = ERROR_CODE(50000, 4), // 未找到零点 + kcommon_error_not_found_x_zero_point = ERROR_CODE(50000, 5), // 未找到零点 + kcommon_error_not_found_y_zero_point = ERROR_CODE(50000, 6), // 未找到零点 + kcommon_error_x_leave_away_zero_point_fail = ERROR_CODE(50000, 7), // 离开零点失败 + kcommon_error_y_leave_away_zero_point_fail = ERROR_CODE(50000, 8), // 离开零点失败 } error_t; } // namespace err diff --git a/components/tmc/ic/ztmc4361A.hpp b/components/tmc/ic/ztmc4361A.hpp index de84bc6..4b618a0 100644 --- a/components/tmc/ic/ztmc4361A.hpp +++ b/components/tmc/ic/ztmc4361A.hpp @@ -110,8 +110,8 @@ class TMC4361A : public IStepperMotor { virtual int32_t getVACTUAL(); virtual int32_t getENC_POS(); virtual void setENC_POS(int32_t value); - virtual void setAcceleration(float accelerationpps2); // 设置最大加速度 - virtual void setDeceleration(float accelerationpps2); // 设置最大减速度 + virtual void setAcceleration(float accelerationpps2); // 设置最大加速度,最大值4000000 + virtual void setDeceleration(float accelerationpps2); // 设置最大减速度,最大值4000000 virtual void setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay) { driverIC_setIHOLD_IRUN(ihold, irun, iholddelay); } virtual void setMotorShaft(bool reverse) { driverIC_setMotorShaft(reverse); } diff --git a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp index ebd4bd3..efef66b 100644 --- a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp +++ b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp @@ -146,9 +146,31 @@ int32_t XYRobotCtrlModule::exec_move_to_zero_task(int32_t& dx, int32_t& dy) { int32_t XYRobotCtrlModule::exec_move_to_zero_task() { /******************************************************************************* + * 远离X零点 * + *******************************************************************************/ + if (m_Xgpio->getState()) { + /** + * @brief 如果设备已经在零点，则反向移动一定距离远离零点 + */ + _motor_move_by(m_cfg_runtozero_leave_away_zero_maxXY, 0, m_cfg_runtozero_speed, m_cfg_acc, m_cfg_runtozero_dec); + while (!_motor_is_reach_target()) { + if (m_thread.getExitFlag()) break; + vTaskDelay(1); + } + + if (m_thread.getExitFlag()) { + ZLOGI(TAG, "break move to zero thread exit"); + return 0; + } + if (m_Xgpio->getState()) { + ZLOGE(TAG, "leave away zero failed"); + return err::kcommon_error_x_leave_away_zero_point_fail; + } + } + /******************************************************************************* * 移动X轴到零点 * *******************************************************************************/ - _motor_move_by(m_cfg_runtozero_maxX, 0, m_cfg_runtozero_speed, m_cfg_acc, m_cfg_runtozero_dec); + _motor_move_by(-m_cfg_runtozero_maxX, 0, m_cfg_runtozero_speed, m_cfg_acc, m_cfg_runtozero_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()); @@ -172,10 +194,34 @@ int32_t XYRobotCtrlModule::exec_move_to_zero_task() { } ZLOGI(TAG, "x reach zero"); + + /******************************************************************************* + * 远离Y零点 * + *******************************************************************************/ + + if (m_Ygpio->getState()) { + /** + * @brief 如果设备已经在零点，则反向移动一定距离远离零点 + */ + _motor_move_by(0, m_cfg_runtozero_leave_away_zero_maxXY, m_cfg_runtozero_speed, m_cfg_acc, m_cfg_dec); + while (!_motor_is_reach_target()) { + if (m_thread.getExitFlag()) break; + vTaskDelay(1); + } + if (m_thread.getExitFlag()) { + ZLOGI(TAG, "break move to zero thread exit"); + return 0; + } + if (m_Ygpio->getState()) { + ZLOGE(TAG, "leave away zero failed"); + return err::kcommon_error_y_leave_away_zero_point_fail; + } + } + /******************************************************************************* * 移动Y轴到零点 * *******************************************************************************/ - _motor_move_by(0, m_cfg_runtozero_maxY, m_cfg_runtozero_speed, m_cfg_acc, m_cfg_runtozero_dec); + _motor_move_by(0, -m_cfg_runtozero_maxY, m_cfg_runtozero_speed, m_cfg_acc, m_cfg_runtozero_dec); bool yreach_zero = false; while (!m_thread.getExitFlag() && !_motor_is_reach_target()) { // ZLOGI(TAG, "ygpio %d", m_Ygpio->getState()); diff --git a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp index 8662832..6b2b4de 100644 --- a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp +++ b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp @@ -36,8 +36,8 @@ class XYRobotCtrlModule { int32_t m_cfg_runtozero_maxX = 5120000; int32_t m_cfg_runtozero_maxY = 5120000; int32_t m_cfg_runtozero_speed = 30000; - int32_t m_cfg_runtozero_dec = 300000; - int32_t m_cfg_runtozero_leave_away_zero_maxXY = 51200 * 5; + int32_t m_cfg_runtozero_dec = 600000; + int32_t m_cfg_runtozero_leave_away_zero_maxXY = 51200 ; float m_cfg_distance_scale = 1.0f; RobotType_t m_robot_type = corexy; diff --git a/components/zcancmder_module/zcan_xy_robot_module.cpp b/components/zcancmder_module/zcan_xy_robot_module.cpp index 9da0ce3..faa1395 100644 --- a/components/zcancmder_module/zcan_xy_robot_module.cpp +++ b/components/zcancmder_module/zcan_xy_robot_module.cpp @@ -22,8 +22,10 @@ void ZCANXYRobotCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) { END_PROCESS_PACKET(); PROCESS_PACKET(kcmd_xy_robot_ctrl_move_to_zero, m_id) { // errorcode = m_xyRobotCtrlModule->move_to_zero([this, cmdheader](int32_t exec_status, int32_t dx, int32_t dy) { + osDelay(5); // 用来保证回执消息在前面 ZLOGI(TAG, "kcmd_xy_robot_ctrl_move_to_zero exec_status:%d dx:%d dy:%d", exec_status, dx, dy); - kcmd_xy_robot_ctrl_move_to_zero_status_report_t report; + kcmd_xy_robot_ctrl_move_to_zero_status_report_t report = {0}; + report.id = m_id; report.exec_status = exec_status; report.dx = dx; @@ -35,8 +37,10 @@ void ZCANXYRobotCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) { PROCESS_PACKET(kcmd_xy_robot_ctrl_move_to_zero_with_calibrate, m_id) { // errorcode = m_xyRobotCtrlModule->move_to_zero_with_calibrate(cmd->x, cmd->y, [this, cmdheader](int32_t exec_status, int32_t zero_shift_x, int32_t zero_shift_y) { + osDelay(5); // 用来保证回执消息在前面 ZLOGI(TAG, "kcmd_xy_robot_ctrl_move_to_zero_with_calibrate exec_status:%d zero_shift_x:%d zero_shift_y:%d", exec_status, zero_shift_x, zero_shift_y); - kcmd_xy_robot_ctrl_move_to_zero_with_calibrate_status_report_t report; + kcmd_xy_robot_ctrl_move_to_zero_with_calibrate_status_report_t report = {0}; + report.id = m_id; report.exec_status = exec_status; report.zero_shift_x = zero_shift_x; @@ -47,8 +51,10 @@ void ZCANXYRobotCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) { END_PROCESS_PACKET(); PROCESS_PACKET(kcmd_xy_robot_ctrl_move_to, m_id) { // errorcode = m_xyRobotCtrlModule->move_to(cmd->x, cmd->y, [this, cmdheader](int32_t exec_status, int32_t tox, int32_t toy) { + osDelay(5); // 用来保证回执消息在前面 ZLOGI(TAG, "kcmd_xy_robot_ctrl_move_to exec_status:%d tox:%d toy:%d", exec_status, tox, toy); - kcmd_xy_robot_ctrl_move_to_status_report_t report; + kcmd_xy_robot_ctrl_move_to_status_report_t report = {0}; + report.id = m_id; report.exec_status = exec_status; report.tox = tox; @@ -60,7 +66,8 @@ void ZCANXYRobotCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) { PROCESS_PACKET(kcmd_xy_robot_ctrl_move_by, m_id) { // errorcode = m_xyRobotCtrlModule->move_by(cmd->dx, cmd->dy, [this, cmdheader](int32_t exec_status, int32_t tox, int32_t toy) { ZLOGI(TAG, "kcmd_xy_robot_ctrl_move_by exec_status:%d tox:%d toy:%d", exec_status, tox, toy); - kcmd_xy_robot_ctrl_move_by_status_report_t report; + kcmd_xy_robot_ctrl_move_by_status_report_t report = {0}; + report.id = m_id; report.exec_status = exec_status; report.tox = tox;