diff --git a/a8000_protocol b/a8000_protocol index 4ed8685..9ec492b 160000 --- a/a8000_protocol +++ b/a8000_protocol @@ -1 +1 @@ -Subproject commit 4ed86854f7c2a6906d17b922d9af85677fb31033 +Subproject commit 9ec492b5981174c6cc2db3dbfeeb6e9c68bc0a42 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 551b50b..3231c7f 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 @@ -43,6 +43,7 @@ void StepMotorCtrlModule::create_default_cfg(config_t& cfg) { cfg.motor_run_to_zero_speed = 100; cfg.motor_look_zero_edge_speed = 100; cfg.io_trigger_append_distance = 0; + cfg.stepmotor_mres = 0; } /*********************************************************************************************************************** @@ -57,6 +58,7 @@ int32_t StepMotorCtrlModule::module_xxx_reg(int32_t param_id, bool read, int32_t case kreg_step_motor_shaft: case kreg_step_motor_one_circle_pulse: case kreg_step_motor_one_circle_pulse_denominator: + case kreg_step_motor_mres: case kreg_step_motor_ihold: case kreg_step_motor_irun: case kreg_step_motor_iholddelay: @@ -110,6 +112,7 @@ int32_t StepMotorCtrlModule::pri_module_xxx_reg(int32_t param_id, bool read, int PROCESS_REG(kreg_step_motor_max_d, REG_GET(m_cfg.max_d), REG_SET(m_cfg.max_d)); PROCESS_REG(kreg_step_motor_min_d, REG_GET(m_cfg.min_d), REG_SET(m_cfg.min_d)); PROCESS_REG(kreg_step_motor_iglobalscaler, REG_GET(m_cfg.stepmotor_iglobalscaler), REG_SET(m_cfg.stepmotor_iglobalscaler)); + PROCESS_REG(kreg_step_motor_mres, REG_GET(m_cfg.stepmotor_mres), REG_SET(m_cfg.stepmotor_mres)); PROCESS_REG(kreg_step_motor_in_debug_mode, REG_GET(m_state.debugmode), REG_SET(m_state.debugmode)); PROCESS_REG(kreg_step_motor_vstart, REG_GET(m_cfg.motor_vstart), REG_SET(m_cfg.motor_vstart)); @@ -164,6 +167,7 @@ int32_t StepMotorCtrlModule::step_motor_easy_set_current_pos(int32_t pos) { retu int32_t StepMotorCtrlModule::step_motor_active_cfg() { ZLOGI(TAG, "m%d motor_active_cfg", m_id); m_stepM1->enable(false); + m_stepM1->setMRES((mres_type_t)m_cfg.stepmotor_mres); m_stepM1->setIHOLD_IRUN(m_cfg.stepmotor_ihold, m_cfg.stepmotor_irun, m_cfg.stepmotor_iholddelay); m_stepM1->setScale(m_cfg.motor_one_circle_pulse); m_stepM1->setScaleDenominator(m_cfg.motor_one_circle_pulse_denominator); 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 f884403..29b0148 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 @@ -19,6 +19,7 @@ class StepMotorCtrlModule : public ZIModule { int32_t stepmotor_irun; int32_t stepmotor_iholddelay; int32_t stepmotor_iglobalscaler; + int32_t stepmotor_mres; int32_t motor_default_velocity; int32_t min_d; int32_t max_d; diff --git a/sdk/components/tmc/ic/ztmc5130.cpp b/sdk/components/tmc/ic/ztmc5130.cpp index 05e4e89..0bf9828 100644 --- a/sdk/components/tmc/ic/ztmc5130.cpp +++ b/sdk/components/tmc/ic/ztmc5130.cpp @@ -49,28 +49,30 @@ void TMC51X0::initialize(cfg_t *cfg) { void TMC51X0::setMRES(mres_type_t value) { PRV_FIELD_WRITE(TMC5130_CHOPCONF, TMC5130_MRES_MASK, TMC5130_MRES_SHIFT, value); - m_MRES = value; - if (m_MRES == kmres_256) { + if (value == kmres_256) { m_onecirclepulse = 51200; - } else if (m_MRES == kmres_128) { + } else if (value == kmres_128) { m_onecirclepulse = 25600; - } else if (m_MRES == kmres_64) { + } else if (value == kmres_64) { m_onecirclepulse = 12800; - } else if (m_MRES == kmres_32) { + } else if (value == kmres_32) { m_onecirclepulse = 6400; - } else if (m_MRES == kmres_16) { + } else if (value == kmres_16) { m_onecirclepulse = 3200; - } else if (m_MRES == kmres_8) { + } else if (value == kmres_8) { m_onecirclepulse = 1600; - } else if (m_MRES == kmres_4) { + } else if (value == kmres_4) { m_onecirclepulse = 800; - } else if (m_MRES == kmres_2) { + } else if (value == kmres_2) { m_onecirclepulse = 400; - } else if (m_MRES == kmres_1) { + } else if (value == kmres_1) { m_onecirclepulse = 200; } else { ZASSERT(false); + m_onecirclepulse = 51200; + value = kmres_256; } + m_MRES = value; } void TMC51X0::setNoAccLimit(bool enable) { @@ -329,7 +331,7 @@ int32_t TMC51X0::to_user_vel(int32_t vel) { // } void TMC51X0::readMotorState(bool *stoped, int32_t *error) { - auto state = getGState(); + auto state = getGState(); // auto devStatus = getDevStatus(); if (state.reset) { *error = (int32_t)err::kstep_motor_subic_reset; diff --git a/usrc/subboards/subboard30_shake_module/subboard30_shake_module.cpp b/usrc/subboards/subboard30_shake_module/subboard30_shake_module.cpp index 5a35235..1f01ff6 100644 --- a/usrc/subboards/subboard30_shake_module/subboard30_shake_module.cpp +++ b/usrc/subboards/subboard30_shake_module/subboard30_shake_module.cpp @@ -84,5 +84,19 @@ void Subboard30ShakeModule::initialize() { GService::inst()->getZCanProtocolParser()->registerModule(&module); } + + { + static MiniServoCtrlModule::config_t cfg = {0}; + static MiniServoCtrlModule module; + int subid = 8; + int subidInBus = 5; + + feiteservomotor_bus.regServo(subidInBus, feite::kscs009, false); + + module.create_default_config(cfg); + module.initialize(getmoduleId(subid), &feiteservomotor_bus, subidInBus, &cfg); + GService::inst()->getZCanProtocolParser()->registerModule(&module); + } + #endif } diff --git a/usrc/version.h b/usrc/version.h index e52f06f..4c0a70d 100644 --- a/usrc/version.h +++ b/usrc/version.h @@ -1,2 +1,2 @@ #pragma once -#define PC_VERSION 1006 +#define PC_VERSION 1008