From edde3d10c35d53b0921a30975c9f255549551b18 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Thu, 25 Jul 2024 09:23:03 +0800 Subject: [PATCH] update --- a8000_protocol | 2 +- .../step_motor_ctrl_module.cpp | 97 +++++++++++++++++++--- .../step_motor_ctrl_module.hpp | 6 ++ sdk/components/zcancmder/zcan_protocol_parser.cpp | 6 ++ sdk/components/zcancmder/zcan_protocol_parser.hpp | 1 + .../subboard60_inlet_and_outlet_module/pri_board.h | 2 +- 6 files changed, 100 insertions(+), 14 deletions(-) diff --git a/a8000_protocol b/a8000_protocol index 5598b81..826f059 160000 --- a/a8000_protocol +++ b/a8000_protocol @@ -1 +1 @@ -Subproject commit 5598b81844f8ca2e8373db46455e35aa6a5ae4f4 +Subproject commit 826f05973b88e7f311373d9f7d1ce74a2554512f diff --git a/sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp b/sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp index 27306c3..2a43f1d 100644 --- a/sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp +++ b/sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp @@ -112,6 +112,8 @@ int32_t StepMotorCtrlModule::pri_module_xxx_reg(int32_t param_id, bool read, int PROCESS_REG(kreg_step_motor_enable_enc, REG_GET(m_cfg.motor_enable_enc), REG_SET(m_cfg.motor_enable_enc)); PROCESS_REG(kreg_step_motor_dzero_pos, REG_GET(m_cfg.motor_dzero), update_dzero(m_cfg.motor_dzero)); + PROCESS_REG(kret_step_motor_pos_devi_tolerance, REG_GET(m_cfg.pos_devi_tolerance), update_dzero(m_cfg.pos_devi_tolerance)); + default: return err::kmodule_not_find_reg; break; @@ -121,7 +123,7 @@ int32_t StepMotorCtrlModule::pri_module_xxx_reg(int32_t param_id, bool read, int int32_t StepMotorCtrlModule::step_motor_enable(int32_t enable) { ZLOGI(TAG, "m%d motor_enable %ld", m_id, enable); - m_stepM1->stop(); + m_thread.stop(); m_stepM1->enable(enable); m_state.enable = enable; return 0; @@ -505,7 +507,7 @@ int32_t StepMotorCtrlModule::step_motor_easy_reciprocating_motion(int32_t startp ZLOGI(TAG, "reciprocating_motion move pos:%d", x_startpos); moveTo(x_startpos, m_cfg.motor_default_velocity); while (!m_stepM1->isReachTarget()) { - osDelay(50); + osDelay(20); if (m_thread.getExitFlag()) break; if (!check_when_run()) break; @@ -514,7 +516,7 @@ int32_t StepMotorCtrlModule::step_motor_easy_reciprocating_motion(int32_t startp ZLOGI(TAG, "reciprocating_motion move pos:%d", x_endpos); moveTo(x_endpos, m_cfg.motor_default_velocity); while (!m_stepM1->isReachTarget()) { - osDelay(50); + osDelay(20); if (m_thread.getExitFlag()) break; if (!check_when_run()) break; @@ -527,7 +529,7 @@ int32_t StepMotorCtrlModule::step_motor_easy_reciprocating_motion(int32_t startp if (m_thread.getExitFlag()) break; if (!check_when_run()) break; - osDelay(50); + osDelay(20); } after_motor_move(); @@ -549,7 +551,7 @@ int32_t StepMotorCtrlModule::do_step_motor_easy_rotate(int32_t direction) { while (true) { if (m_thread.getExitFlag()) break; if (!check_when_run()) break; - osDelay(50); + osDelay(20); } } @@ -567,15 +569,13 @@ int32_t StepMotorCtrlModule::do_step_motor_easy_move_to(int32_t tox) { [this, tox]() { befor_motor_move(); - { + do { + int32_t expectation_dpos = tox - getnowpos(); moveTo(tox, m_cfg.motor_default_velocity); - while (!m_stepM1->isReachTarget()) { - if (m_thread.getExitFlag()) break; - if (!check_when_run()) break; + if (!waitforstop(nullptr)) break; - osDelay(50); - } - } + checkdpos(expectation_dpos); + } while (false); after_motor_move(); }, @@ -628,6 +628,47 @@ int32_t StepMotorCtrlModule::do_step_motor_easy_move_to_io(int32_t ioindex, int3 int32_t StepMotorCtrlModule::do_step_motor_easy_move_to_end_point() { return do_step_motor_easy_move_to_io(1, 1); } +void StepMotorCtrlModule::checkdpos(int32_t expect_dpos) { + int32_t dpos = getnowpos() - m_state.before_move_pos; + if (m_cfg.pos_devi_tolerance != 0 && (abs(expect_dpos - dpos) > m_cfg.pos_devi_tolerance)) { + ZLOGE(TAG, "check pos devi tolerance fail.... dpos%d,expectation_dpos:%d", dpos, expect_dpos); + setErrorFlag(err::kstep_motor_lost_step); + } else { + ZLOGE(TAG, "dpos%d,expect_dpos:%d", dpos, expect_dpos); + } +} + +int32_t StepMotorCtrlModule::step_motor_easy_move_to_zero_point_quick() { + // + ZLOGI(TAG, "m%d motor_move_to_zero_point_quick", m_id); + m_thread.stop(); + creg.module_status = 1; + m_thread.start( + [this]() { + befor_motor_move(); + bool moveToZero = false; + do { + // 期望偏差 + int32_t expectation_dpos = 0 + m_cfg.motor_dzero - getnowpos(); + // 快速移动到零点 + moveTo(0 + m_cfg.motor_dzero, m_cfg.motor_default_velocity); + // 等待移动到目标点 + if (!waitforstop([this]() { return m_iotable[0].getState(); })) break; + // 移动到零点 + if (!exec_move_to_io_task(0, -1)) break; + moveToZero = true; + // 校验偏差 + checkdpos(expectation_dpos); + } while (false); + + after_motor_move(); + if (moveToZero) setnowpos(0 + m_cfg.motor_dzero); + }, + [this]() { m_stepM1->stop(); }); + + return 0; +} + // int32_t StepMotorCtrlModule::getnowpos(int32_t& x) { if (m_cfg.motor_enable_enc == 0) { @@ -637,6 +678,12 @@ int32_t StepMotorCtrlModule::getnowpos(int32_t& x) { x = m_stepM1->read_enc_val(); return 0; } +int32_t StepMotorCtrlModule::getnowpos() { + int32_t x; + getnowpos(x); + return x; +} + int32_t StepMotorCtrlModule::trysyncpos() { if (m_cfg.motor_enable_enc == 0) { return 0; @@ -670,3 +717,29 @@ int32_t StepMotorCtrlModule::update_dzero(int32_t dzero) { setnowpos(nowabs + m_cfg.motor_dzero); return 0; } + +bool StepMotorCtrlModule::waitforstop(function checkstop, int checkperiod) { + bool suc = true; + while (true) { + if (m_thread.getExitFlag() || !check_when_run()) { + suc = false; + break; + } + if (checkstop && checkstop()) { + break; + } + + if (m_stepM1->isReachTarget()) { + break; + } + osDelay(checkperiod); + } + m_stepM1->stop(); + + while (m_thread.getExitFlag()) { + if (m_stepM1->isReachTarget()) break; + osDelay(checkperiod); + } + + return suc; +} diff --git a/sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp b/sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp index 0b55a5f..2bd5d98 100644 --- a/sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp +++ b/sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp @@ -33,6 +33,7 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor { int32_t motor_enc_resolution; int32_t motor_enable_enc; int32_t motor_dzero; // 零点偏移 + int32_t pos_devi_tolerance; } config_t; typedef struct { @@ -99,6 +100,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; + virtual int32_t step_motor_easy_move_to_zero_point_quick() override; + public: TMC51X0* getMotor() { return m_stepM1; } config_t* getCfg() { return &m_cfg; } @@ -131,6 +134,7 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor { void setErrorFlag(int32_t ecode); int32_t getnowpos(int32_t& x); + int32_t getnowpos(); int32_t trysyncpos(); int32_t setnowpos(int32_t x); int32_t moveTo(int32_t x, int32_t v); @@ -138,6 +142,8 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor { int32_t rotate(int32_t direction, int32_t v); int32_t update_dzero(int32_t dzero); + bool waitforstop(function checkstop, int checkperiod = 20); + void checkdpos(int32_t expect_dpos); // int32_t read_enc_val(int32_t& enc_val); // int32_t write_enc_val(int32_t enc_val); diff --git a/sdk/components/zcancmder/zcan_protocol_parser.cpp b/sdk/components/zcancmder/zcan_protocol_parser.cpp index d3e193e..ecde147 100644 --- a/sdk/components/zcancmder/zcan_protocol_parser.cpp +++ b/sdk/components/zcancmder/zcan_protocol_parser.cpp @@ -42,6 +42,7 @@ void ZCanProtocolParser::initialize(IZCanReceiver* cancmder) { REGFN(step_motor_read_io_index_in_stm32); REGFN(step_motor_set_subdevice_reg); REGFN(step_motor_get_subdevice_reg); + REGFN(step_motor_easy_move_to_zero_point_quick); REGFN(mini_servo_enable); REGFN(mini_servo_read_pos); @@ -361,6 +362,11 @@ int32_t ZCanProtocolParser::step_motor_get_subdevice_reg(cmdcontxt_t* cxt) { return module->step_motor_get_subdevice_reg(cxt->params[0], ack); } +int32_t ZCanProtocolParser::step_motor_easy_move_to_zero_point_quick(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(0); + return module->step_motor_easy_move_to_zero_point_quick(); +} + #undef MODULE_CLASS #define MODULE_CLASS ZIMiniServo // virtual int32_t mini_servo_enable(int32_t enable) = 0; diff --git a/sdk/components/zcancmder/zcan_protocol_parser.hpp b/sdk/components/zcancmder/zcan_protocol_parser.hpp index 0a3bca0..4cdf73d 100644 --- a/sdk/components/zcancmder/zcan_protocol_parser.hpp +++ b/sdk/components/zcancmder/zcan_protocol_parser.hpp @@ -78,6 +78,7 @@ class ZCanProtocolParser : public IZCanReceiverListener { CMDFN(step_motor_set_subdevice_reg); CMDFN(step_motor_get_subdevice_reg); CMDFN(step_motor_easy_reciprocating_motion); + CMDFN(step_motor_easy_move_to_zero_point_quick); CMDFN(mini_servo_enable); CMDFN(mini_servo_read_pos); diff --git a/usrc/subboards/subboard60_inlet_and_outlet_module/pri_board.h b/usrc/subboards/subboard60_inlet_and_outlet_module/pri_board.h index d077338..3b60865 100644 --- a/usrc/subboards/subboard60_inlet_and_outlet_module/pri_board.h +++ b/usrc/subboards/subboard60_inlet_and_outlet_module/pri_board.h @@ -72,7 +72,7 @@ #define MOTOR2_REFR_MIRROR true #define MOTOR2_VSTART 100 -#define MOTOR2_A1 300 +#define MOTOR2_A1 100 #define MOTOR2_V1 800 #define MOTOR2_AMAX 500