From 13227d849942f51f842cc22754b6a6a0afef13b2 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Mon, 16 Oct 2023 15:59:04 +0800 Subject: [PATCH] update --- .../step_motor_ctrl_module.cpp | 103 +++++++++++++-------- .../step_motor_ctrl_script_cmder_module.cpp | 27 ++++-- components/zprotocols/zcancmder | 2 +- 3 files changed, 82 insertions(+), 50 deletions(-) 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 df01cb3..2c79df1 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp @@ -110,6 +110,7 @@ int32_t StepMotorCtrlModule::_move_by(int32_t dx, action_cb_status_t status_cb) getnowpos(xnow); int32_t toxnow = xnow + dx; + // ZLOGI(TAG, "m%d move_by %d xnow:%d toxnow:%d", m_id, dx, xnow, toxnow); return move_to(toxnow, [this, status_cb, xnow](int32_t status) { if (status_cb) status_cb(status); }); @@ -316,10 +317,27 @@ int32_t StepMotorCtrlModule::set_base_param(const base_param_t& param) { ZLOGI(TAG, "= min_x :%d", m_param.min_x); ZLOGI(TAG, "= max_x :%d", m_param.max_x); ZLOGI(TAG, "= shift_x :%d", m_param.shift_x); - ZLOGI(TAG, "= run_to_zero_max_d :%d", m_param.run_to_zero_move_to_zero_max_d); - ZLOGI(TAG, "= leave_from_zero_max_d :%d", m_param.run_to_zero_leave_from_zero_max_d); + +#if 0 + s32 run_to_zero_max_d; + s32 run_to_zero_speed; + s32 run_to_zero_dec; + + s32 look_zero_edge_max_d; + s32 look_zero_edge_speed; + s32 look_zero_edge_dec; +#endif + ZLOGI(TAG, "= run_to_zero_max_d :%d", m_param.run_to_zero_max_d); ZLOGI(TAG, "= run_to_zero_speed :%d", m_param.run_to_zero_speed); ZLOGI(TAG, "= run_to_zero_dec :%d", m_param.run_to_zero_dec); + ZLOGI(TAG, "= look_zero_edge_max_d :%d", m_param.look_zero_edge_max_d); + ZLOGI(TAG, "= look_zero_edge_speed :%d", m_param.look_zero_edge_speed); + ZLOGI(TAG, "= look_zero_edge_dec :%d", m_param.look_zero_edge_dec); + + // ZLOGI(TAG, "= run_to_zero_max_d :%d", m_param.run_to_zero_move_to_zero_max_d); + // ZLOGI(TAG, "= leave_from_zero_max_d :%d", m_param.run_to_zero_leave_from_zero_max_d); + // ZLOGI(TAG, "= run_to_zero_speed :%d", m_param.run_to_zero_speed); + // ZLOGI(TAG, "= run_to_zero_dec :%d", m_param.run_to_zero_dec); ZLOGI(TAG, "=================================="); return 0; } @@ -362,7 +380,7 @@ 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_move_to_zero_max_d, m_param.run_to_zero_speed * 2, m_param.acc, m_param.run_to_zero_dec); + _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); 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()); @@ -396,7 +414,7 @@ int32_t StepMotorCtrlModule::exec_move_to_zero_task() { * @brief 如果设备已经在零点，则反向移动一定距离远离零点 */ if (m_Xgpio->getState()) { - _motor_move_by(m_param.run_to_zero_leave_from_zero_max_d, m_param.run_to_zero_speed, m_param.acc, m_param.run_to_zero_dec); + _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()) { if (m_thread.getExitFlag()) break; if (!m_Xgpio->getState()) break; @@ -417,37 +435,37 @@ int32_t StepMotorCtrlModule::exec_move_to_zero_task() { /******************************************************************************* * 移动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::kce_break_by_user; - } - - if (!xreach_zero) { - // 触发异常回调 - ZLOGE(TAG, "x reach zero failed"); - return err::kce_not_found_x_zero_point; - } - } + // { + // 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::kce_break_by_user; + // } + + // if (!xreach_zero) { + // // 触发异常回调 + // ZLOGE(TAG, "x reach zero failed"); + // return err::kce_not_found_x_zero_point; + // } + // } if (m_thread.getExitFlag()) { ZLOGI(TAG, "break move to zero thread exit"); @@ -579,17 +597,20 @@ void StepMotorCtrlModule::call_exec_status_cb(int32_t status, action_cb_status_t void StepMotorCtrlModule::create_default_cfg(flash_config_t& cfg) { memset(&cfg, 0, sizeof(cfg)); cfg.base_param.distance_scale = 10000; - cfg.base_param.ihold = 0; - cfg.base_param.irun = 8; + cfg.base_param.ihold = 1; + cfg.base_param.irun = 3; cfg.base_param.iholddelay = 100; cfg.base_param.acc = 300; cfg.base_param.dec = 300; cfg.base_param.maxspeed = 800; - cfg.base_param.run_to_zero_move_to_zero_max_d = 10000 * 100; - cfg.base_param.run_to_zero_leave_from_zero_max_d = 10000 * 2; - cfg.base_param.run_to_zero_speed = 100; - cfg.base_param.run_to_zero_dec = 600; + cfg.base_param.run_to_zero_max_d = 10000 * 100; + cfg.base_param.run_to_zero_speed = 100; + cfg.base_param.run_to_zero_dec = 600; + + cfg.base_param.look_zero_edge_max_d = 10000 * 5; + cfg.base_param.look_zero_edge_speed = 100; + cfg.base_param.look_zero_edge_dec = 600; } int32_t StepMotorCtrlModule::flush() { diff --git a/components/step_motor_ctrl_module/step_motor_ctrl_script_cmder_module.cpp b/components/step_motor_ctrl_module/step_motor_ctrl_script_cmder_module.cpp index 22b74bf..219b916 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_script_cmder_module.cpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_script_cmder_module.cpp @@ -55,10 +55,17 @@ static void cmd_dump_ack(I_StepMotorCtrlModule::base_param_t& ack) { ZLOGI(TAG, " min_x: %d", ack.min_x); ZLOGI(TAG, " max_x: %d", ack.max_x); ZLOGI(TAG, " shift_x: %d", ack.shift_x); - ZLOGI(TAG, " run_to_zero_move_to_zero_max_d: %d", ack.run_to_zero_move_to_zero_max_d); - ZLOGI(TAG, " run_to_zero_leave_from_zero_max_d: %d", ack.run_to_zero_leave_from_zero_max_d); - ZLOGI(TAG, " run_to_zero_speed: %d", ack.run_to_zero_speed); - ZLOGI(TAG, " run_to_zero_dec: %d", ack.run_to_zero_dec); + ZLOGI(TAG, "= run_to_zero_max_d :%d", ack.run_to_zero_max_d); + ZLOGI(TAG, "= run_to_zero_speed :%d", ack.run_to_zero_speed); + ZLOGI(TAG, "= run_to_zero_dec :%d", ack.run_to_zero_dec); + ZLOGI(TAG, "= look_zero_edge_max_d :%d", ack.look_zero_edge_max_d); + ZLOGI(TAG, "= look_zero_edge_speed :%d", ack.look_zero_edge_speed); + ZLOGI(TAG, "= look_zero_edge_dec :%d", ack.look_zero_edge_dec); + + // ZLOGI(TAG, " run_to_zero_move_to_zero_max_d: %d", ack.run_to_zero_move_to_zero_max_d); + // ZLOGI(TAG, " run_to_zero_leave_from_zero_max_d: %d", ack.run_to_zero_leave_from_zero_max_d); + // ZLOGI(TAG, " run_to_zero_speed: %d", ack.run_to_zero_speed); + // ZLOGI(TAG, " run_to_zero_dec: %d", ack.run_to_zero_dec); } static void cmd_dump_ack(I_StepMotorCtrlModule::logic_point_t& ack) { @@ -244,14 +251,18 @@ void StepMotorCtrlScriptCmderModule::regcmd() { // param.max_x = value; } else if (streq(paramName, "shift_x")) { param.shift_x = value; - } else if (streq(paramName, "run_to_zero_move_to_zero_max_d")) { - param.run_to_zero_move_to_zero_max_d = value; - } else if (streq(paramName, "run_to_zero_leave_from_zero_max_d")) { - param.run_to_zero_leave_from_zero_max_d = value; + } else if (streq(paramName, "run_to_zero_max_d")) { + param.run_to_zero_max_d = value; } else if (streq(paramName, "run_to_zero_speed")) { param.run_to_zero_speed = value; } else if (streq(paramName, "run_to_zero_dec")) { param.run_to_zero_dec = value; + } else if (streq(paramName, "look_zero_edge_max_d")) { + param.look_zero_edge_max_d = value; + } else if (streq(paramName, "look_zero_edge_speed")) { + param.look_zero_edge_speed = value; + } else if (streq(paramName, "look_zero_edge_dec")) { + param.look_zero_edge_dec = value; } else { ZLOGE(TAG, "invalid param name:%s", paramName); return (int32_t)err::kce_param_out_of_range; diff --git a/components/zprotocols/zcancmder b/components/zprotocols/zcancmder index 66f198b..5c9d5ad 160000 --- a/components/zprotocols/zcancmder +++ b/components/zprotocols/zcancmder @@ -1 +1 @@ -Subproject commit 66f198bd2f0f38fefbdba179b98317f3948e0c04 +Subproject commit 5c9d5ade69831f8d65d7ed30e49987706100bbfa