diff --git a/README.md b/README.md index 88021cf..cf8e89d 100644 --- a/README.md +++ b/README.md @@ -4,8 +4,11 @@ V602 1. 修复移液枪Z轴控制部分BUG - +V1212: + 1. 增加步进电机驱动IO镜像配置项 ``` + + ``` 编译: @@ -37,28 +40,6 @@ ID地址: ``` -``` - -``` - -``` -修改 -1. 去除掉input_reg，修改驱动读取gpio的寄存器为kreg_step_motor_io_state -2. 增加舵机专用寄存器和方法 - -``` - -``` - - - - - - - - - -``` \ No newline at end of file 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 d96e2a1..95371ec 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 @@ -16,6 +16,7 @@ void StepMotorCtrlModule::initialize(int moduleid, TMC51X0* stepM, ZGPIO iotable m_iotable = iotable; m_nio = nio; + m_thread.init(TAG, 1024, osPriorityNormal); m_cfg = *cfg; m_default_cfg = *cfg; @@ -137,6 +138,9 @@ int32_t StepMotorCtrlModule::module_set_reg(int32_t regindex, int32_t val) { MODULE_REG_CASE__SET_REG_TO(kreg_step_motor_pos_devi_tolerance, (m_cfg.pos_devi_tolerance)); MODULE_REG_CASE__SET_REG_TO(kreg_step_motor_io_trigger_append_distance, (m_cfg.io_trigger_append_distance)); + MODULE_REG_CASE__SET_REG_TO(kreg_step_motor_zero_io_mirror, (m_cfg.zero_io_mirror)); + MODULE_REG_CASE__SET_REG_TO(kreg_step_motor_limit_io_mirror, (m_cfg.limit_io_mirror)); + default: return err::kmodule_not_find_reg; break; @@ -215,6 +219,9 @@ int32_t StepMotorCtrlModule::module_get_reg(int32_t regindex, int32_t* val) { MODULE_CFG_CASE__GET_REG_FROM(kreg_step_motor_pos_devi_tolerance, m_cfg.pos_devi_tolerance); MODULE_CFG_CASE__GET_REG_FROM(kreg_step_motor_io_trigger_append_distance, m_cfg.io_trigger_append_distance); + MODULE_CFG_CASE__GET_REG_FROM(kreg_step_motor_zero_io_mirror, (m_cfg.zero_io_mirror)); + MODULE_CFG_CASE__GET_REG_FROM(kreg_step_motor_limit_io_mirror, (m_cfg.limit_io_mirror)); + default: return err::kmodule_not_find_reg; break; @@ -262,16 +269,17 @@ int32_t StepMotorCtrlModule::step_motor_active_cfg() { m_stepM1->set_tzerowait(m_cfg.motor_tzerowait); m_stepM1->set_enc_resolution(m_cfg.motor_enc_resolution); - TMC51X0::VCfg_t vcfg; - vcfg.a1 = m_cfg.motor_a1; - vcfg.amax = m_cfg.motor_amax; - vcfg.v1 = m_cfg.motor_v1; - vcfg.dmax = m_cfg.motor_dmax; - vcfg.d1 = m_cfg.motor_d1; - vcfg.vstart = m_cfg.motor_vstart; - vcfg.vstop = m_cfg.motor_vstop; - vcfg.vmax = m_cfg.motor_default_velocity; - m_stepM1->set_vcfg(&vcfg); + // TMC51X0::VCfg_t vcfg; + // vcfg.a1 = m_cfg.motor_a1; + // vcfg.amax = m_cfg.motor_amax; + // vcfg.v1 = m_cfg.motor_v1; + // vcfg.dmax = m_cfg.motor_dmax; + // vcfg.d1 = m_cfg.motor_d1; + // vcfg.vstart = m_cfg.motor_vstart; + // vcfg.vstop = m_cfg.motor_vstop; + // vcfg.vmax = m_cfg.motor_default_velocity; + // m_stepM1->set_vcfg(&vcfg); + m_stepM1->stop(); // stepmotor_iglobalscaler if (m_state.enable) { @@ -285,7 +293,7 @@ int32_t StepMotorCtrlModule::step_motor_read_io_state(int32_t ioindex, int32_t* if (ioindex < 0 || ioindex >= m_nio) { return err::kstep_motor_ioindex_out_of_range; } - *state = m_iotable[ioindex].getState(); + *state = getIOState(ioindex) ? 1 : 0; return 0; } @@ -411,11 +419,13 @@ bool StepMotorCtrlModule::check_when_run() { } else if (state.uv_cp) { ZLOGE(TAG, "motor uv_cp when run"); dumpTMC5130Status(&devStatus); + module_detail_errorcode = *((int32_t*)&devStatus); setErrorFlag(err::kstep_motor_uv_cp); return false; } else if (state.drv_err) { ZLOGE(TAG, "motor drv_err when run"); setErrorFlag(err::kstep_motor_drv_err); + module_detail_errorcode = *((int32_t*)&devStatus); dumpTMC5130Status(&devStatus); return false; } @@ -430,16 +440,15 @@ bool StepMotorCtrlModule::exec_move_to_io_task(int32_t ioindex, int32_t directio direction = direction >= 0 ? 1 : -1; - ZGPIO* gpio = &m_iotable[ioindex]; if (!check_when_run()) return false; - if (!gpio->getState()) { + if (!getIOState(ioindex)) { ZLOGI(TAG, "---------STEP1-------- move to point"); rotate(direction, runToPointSpeed); bool xreach_io = false; while (!m_thread.getExitFlag()) { - if (gpio->getState()) { + if (getIOState(ioindex)) { xreach_io = true; m_stepM1->stop(); break; @@ -465,13 +474,13 @@ bool StepMotorCtrlModule::exec_move_to_io_task(int32_t ioindex, int32_t directio // 如果设备已经在零点，则反向移动一定距离远离零点 - if (gpio->getState()) { + if (getIOState(ioindex)) { ZLOGI(TAG, "---------STEP2-------- find edge"); rotate(-direction, lookPointVelocity); bool reach_edge = false; while (!m_thread.getExitFlag()) { - if (!gpio->getState()) { + if (!getIOState(ioindex)) { reach_edge = true; m_stepM1->stop(); break; @@ -494,13 +503,13 @@ bool StepMotorCtrlModule::exec_move_to_io_task(int32_t ioindex, int32_t directio if (!check_when_run()) return false; // 如果设备已经在零点，则反向移动一定距离远离零点 - if (!gpio->getState()) { + if (!getIOState(ioindex)) { ZLOGI(TAG, "---------STEP3-------- bak to gpio trigger edge"); rotate(direction, lookPointVelocity); bool reach_edge = false; while (!m_thread.getExitFlag()) { - if (gpio->getState()) { + if (getIOState(ioindex)) { reach_edge = true; m_stepM1->stop(); break; @@ -623,9 +632,6 @@ int32_t StepMotorCtrlModule::step_motor_easy_move_to_zero() { return err::kstep_motor_ioindex_out_of_range; } - if (m_iotable[0].isNull()) { - return err::kstep_motor_ioindex_out_of_range; - } return do_step_motor_easy_move_to_zero(); } @@ -648,9 +654,6 @@ int32_t StepMotorCtrlModule::step_motor_easy_move_to_end_point() { if (m_nio <= 1) { return err::kstep_motor_ioindex_out_of_range; } - if (m_iotable[1].isNull()) { - return err::kstep_motor_ioindex_out_of_range; - } 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) { @@ -793,9 +796,6 @@ int32_t StepMotorCtrlModule::do_step_motor_easy_move_to_io(int32_t ioindex, int3 if (ioindex < 0 || ioindex >= m_nio) { return err::kstep_motor_ioindex_out_of_range; } - if (m_iotable[ioindex].isNull()) { - return err::kstep_motor_ioindex_out_of_range; - } m_thread.stop(); module_status = 1; @@ -985,3 +985,15 @@ int32_t StepMotorCtrlModule::getspeed(int32_t speedlevel) { } return speed; } +bool StepMotorCtrlModule::getIOState(int32_t ioindex) { + if (ioindex < 0 || ioindex >= m_nio) { + return false; + } + if (ioindex == 0) { + return m_cfg.zero_io_mirror ? !m_iotable[ioindex].getState() : m_iotable[ioindex].getState(); + } else if (ioindex == 1) { + return m_cfg.limit_io_mirror ? !m_iotable[ioindex].getState() : m_iotable[ioindex].getState(); + } else { + return m_iotable[ioindex].getState(); + } +} 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 d578d74..f2cf14a 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 @@ -42,6 +42,8 @@ class StepMotorCtrlModule : public ZIModule { int32_t motor_dzero; // 零点偏移 int32_t pos_devi_tolerance; int32_t io_trigger_append_distance; + int32_t zero_io_mirror; + int32_t limit_io_mirror; } config_t; typedef struct { @@ -162,9 +164,6 @@ class StepMotorCtrlModule : public ZIModule { private: int32_t getspeed(int32_t speedlevel); - // int32_t read_enc_val(int32_t& enc_val); - // int32_t write_enc_val(int32_t enc_val); - - // virtual int32_t step_motor_set_enc_resolution(int32_t enc_resolution); + bool getIOState(int32_t ioindex); }; } // namespace iflytop \ No newline at end of file diff --git a/sdk/components/tmc/ic/ztmc5130.cpp b/sdk/components/tmc/ic/ztmc5130.cpp index 0416e1f..29b31e8 100644 --- a/sdk/components/tmc/ic/ztmc5130.cpp +++ b/sdk/components/tmc/ic/ztmc5130.cpp @@ -190,6 +190,7 @@ void TMC51X0::setXACTUAL(int32_t value) { rotate(0); writeInt(TMC5130_XACTUAL, to_motor_pos(value)); writeInt(TMC5130_XTARGET, to_motor_pos(value)); + writeInt(TMC5130_RAMPMODE, TMC5130_MODE_POSITION); // 切换回位置模式 } int32_t TMC51X0::getVACTUAL() { return to_user_pos(readInt(TMC5130_VACTUAL)); } void TMC51X0::setAcceleration(float accelerationpps2) { writeInt(TMC5130_AMAX, (int32_t)(to_motor_acc(accelerationpps2))); } // 设置最大加速度 @@ -246,6 +247,7 @@ Tmc5130RampStat TMC51X0::getTMC5130_RAMPSTAT2() { void TMC51X0::stop() { int mode = readInt(TMC5130_RAMPMODE); if (mode == TMC5130_MODE_POSITION) { + writeInt(TMC5130_TZEROWAIT, 0); writeInt(TMC5130_XTARGET, readInt(TMC5130_XACTUAL)); } else { rotate(0); diff --git a/usrc/a8000_protocol/protocol/reg_index.hpp b/usrc/a8000_protocol/protocol/reg_index.hpp index 46cd77f..ba7e769 100644 --- a/usrc/a8000_protocol/protocol/reg_index.hpp +++ b/usrc/a8000_protocol/protocol/reg_index.hpp @@ -217,6 +217,8 @@ typedef enum { kreg_step_motor_dzero_pos = 10191, // kreg_step_motor_pos_devi_tolerance = 10192, // 位置偏差容忍度 kreg_step_motor_io_trigger_append_distance = 10193, // 移动到IO时，会在原点触发的边沿，稍稍里面移动一点，用来保证IO稳定触发 + kreg_step_motor_zero_io_mirror = 10194, // 零点IO有效位是否翻转 + kreg_step_motor_limit_io_mirror = 10195, // 零点IO有效位是否翻转 /*********************************************************************************************************************** * step_motor * diff --git a/usrc/version.h b/usrc/version.h index 96e785d..4a3f824 100644 --- a/usrc/version.h +++ b/usrc/version.h @@ -1,2 +1,2 @@ #pragma once -#define APP_VERSION 1211 +#define APP_VERSION 1212