diff --git a/a8000_protocol b/a8000_protocol index 5fb4985..49fd379 160000 --- a/a8000_protocol +++ b/a8000_protocol @@ -1 +1 @@ -Subproject commit 5fb4985d0c6cae59ab4f00a1de65910938bca096 +Subproject commit 49fd379cc25952455fcf735c2e57f8937229aefb diff --git a/sdk/components/pipette_module/pipette_ctrl_module_v2.cpp b/sdk/components/pipette_module/pipette_ctrl_module_v2.cpp index d71d031..2d33c7b 100644 --- a/sdk/components/pipette_module/pipette_ctrl_module_v2.cpp +++ b/sdk/components/pipette_module/pipette_ctrl_module_v2.cpp @@ -200,14 +200,13 @@ int32_t PipetteModuleV2::module_xxx_reg(int32_t param_id, bool read, int32_t &va } m_config_update = false; return retval; - } - int32_t PipetteModuleV2::module_reset_reg() { +int32_t PipetteModuleV2::module_reset_reg() { ZLOGI(TAG, "module_reset_reg"); m_cfg = m_default_cfg; module_active_cfg(); return 0; - } +} /*********************************************************************************************************************** * EXT_API * @@ -823,6 +822,14 @@ int32_t PipetteModuleV2::bfcall(int32_t cmdid, uint8_t *param, int32_t len) { MODULE_BFCALL_DUMP_FN_CALL(cmdid, param32, paramNum); } + if (!m_zm->isMotorIcInitOk()) { + return err::kstep_motor_subic_init_fail; + } + + if (!m_zm->isOnline()) { + return err::kstep_motor_subic_offline; + } + if (cmdid == kpipette_zmotor_move_zero) goto check_motor_is_enable; if (cmdid == kpipette_zmotor_move_by) goto check_motor_is_enable; 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 1508db2..264a930 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 @@ -286,6 +286,11 @@ void StepMotorCtrlModule::setErrorFlag(int32_t ecode) { creg.module_errorcode = bool StepMotorCtrlModule::check_when_run() { // + if (!m_stepM1->isOnline()) { + creg.module_errorcode = err::kstep_motor_subic_offline; + return false; + } + if (m_state.debugmode) return true; if (m_cfg.motor_enable_enc != 0) { @@ -296,7 +301,7 @@ bool StepMotorCtrlModule::check_when_run() { vPortExitCritical(); - int32_t dm1 = abs(m1enc - m1pos); + int32_t dm1 = abs(m1enc - m1pos); m_state.lost_step = dm1; if (m_cfg.pos_devi_tolerance != 0) { if (dm1 > m_cfg.pos_devi_tolerance) { @@ -465,6 +470,14 @@ int32_t StepMotorCtrlModule::check_befor_run() { return err::kstep_motor_not_enable; } + if (!m_stepM1->isMotorIcInitOk()) { + return err::kstep_motor_subic_init_fail; + } + + if (!m_stepM1->isOnline()) { + return err::kstep_motor_subic_offline; + } + if (creg.module_errorcode == err::kstep_motor_subic_reset) { return creg.module_errorcode; } else if (creg.module_errorcode == err::kstep_motor_drv_err) { @@ -719,9 +732,13 @@ void StepMotorCtrlModule::checkdpos(int32_t 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); + int32_t ecode = check_befor_run(); + if (ecode != 0) return ecode; + if (m_state.has_move_to_zero == 0) { return err::kstep_motor_not_move_to_zero; } + m_thread.stop(); creg.module_status = 1; m_thread.start( diff --git a/sdk/components/tmc/ic/ztmc5130.cpp b/sdk/components/tmc/ic/ztmc5130.cpp index e942ad0..9b6abf5 100644 --- a/sdk/components/tmc/ic/ztmc5130.cpp +++ b/sdk/components/tmc/ic/ztmc5130.cpp @@ -30,6 +30,7 @@ void TMC51X0::initialize(cfg_t *cfg) { // m_port = config->m_port; m_registerAccessTable = &tmc5130_defaultRegisterAccess[0]; m_defaultRegisterResetState = &tmc5130_defaultRegisterResetState[0]; + if (cfg->csgpio != PinNull) { m_csnpin = new ZGPIO(); m_csnpin->initAsOutput(cfg->csgpio, ZGPIO::kMode_nopull, false, true); @@ -101,6 +102,22 @@ uint8_t TMC51X0::reset() { // zchip_clock_early_delayus(1000); SET_PIN(m_csnpin, true); + int icVersion = 0; + for (size_t i = 0; i < 50; i++) { + icVersion = readICVersion(); + if (icVersion == kTMC2160AndTMC5160 || icVersion == kTMC5130) { + motorICInitOk = true; + break; + } + HAL_Delay(100); + ZLOGI("TMC5130", "WAITING FOR MOTOR POWER UP"); + } + + if (!motorICInitOk) { + ZLOGE("TMC5130", "MOTOR INIT FAIL, DRIVER_IC_IS_OFFLINE"); + return; + } + stop(); enable(false); for (uint32_t add = 0; add < TMC5130_REGISTER_COUNT; add++) { @@ -132,7 +149,6 @@ uint8_t TMC51X0::reset() { return 0; } - int32_t TMC51X0::getXACTUAL() { return to_user_pos(readInt(TMC5130_XACTUAL)); } void TMC51X0::setXACTUAL(int32_t value) { /** @@ -148,6 +164,14 @@ void TMC51X0::setDeceleration(float accelerationpps2) { writeInt(TMC5130_DMAX void TMC51X0::setMotorShaft(bool reverse) { PRV_FIELD_WRITE(TMC5130_GCONF, TMC5130_SHAFT_MASK, TMC5130_SHAFT_SHIFT, reverse); } void TMC51X0::setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay) { writeInt(TMC5130_IHOLD_IRUN, (iholddelay << TMC5130_IHOLDDELAY_SHIFT) | (irun << TMC5130_IRUN_SHIFT) | (ihold << TMC5130_IHOLD_SHIFT)); } +bool TMC51X0::isOnline() { + uint32_t chipId = readICVersion(); + if (chipId == kTMC5130 || chipId == kTMC2160AndTMC5160) { + return true; + } + return false; +} + #define TMC5160_GLOBAL_SCALER 0x0B #define TMC5160_GLOBAL_SCALER_MASK 0xFF #define TMC5160_GLOBAL_SCALER_SHIFT 0 diff --git a/sdk/components/tmc/ic/ztmc5130.hpp b/sdk/components/tmc/ic/ztmc5130.hpp index 6f83490..c6b7c9f 100644 --- a/sdk/components/tmc/ic/ztmc5130.hpp +++ b/sdk/components/tmc/ic/ztmc5130.hpp @@ -21,13 +21,13 @@ namespace iflytop { * * 1. 6点加速控制只在位置模式下有效,在速度模式下无效 * 2. 速度模式下只有AMAX有效 - * - * - * + * + * + * * 速度的时间基准 t:t=2^24/fclk * 加速度的时间基准:ta^2:ta^2=2^24/(fclk)^2 - * - * + * + * * VSTOP 1...(2^18)-1, 1...262144-1, 1...307 * VSTART 1...(2^18)-1, 1...262144-1, 1...307 * VEL +-(2^23)-1 [µsteps / t] +-8388608 +-9830.4 @@ -115,11 +115,15 @@ class TMC51X0 : public IStepperMotor { int32_t m_enc_resolution = 0; // 编码器分辨率,默认只有在256细分的情况下有效 + bool motorICInitOk = false; + public: TMC51X0(/* args */); void initialize(cfg_t *cfg); + bool isMotorIcInitOk() { return motorICInitOk; } + virtual void enable(bool enable); virtual void rotate(int32_t velocity); @@ -132,6 +136,7 @@ class TMC51X0 : public IStepperMotor { virtual void setXACTUAL(int32_t value); // 设置当前位置 virtual int32_t getXACTUAL(); // 当前位置 virtual int32_t getVACTUAL(); // 当前速度 + virtual bool isOnline(); virtual void setAcceleration(float accelerationpps2); // 设置最大加速度 virtual void setDeceleration(float accelerationpps2); // 设置最大减速度 @@ -141,11 +146,10 @@ class TMC51X0 : public IStepperMotor { virtual void setScale(int32_t scale); virtual void setScaleDenominator(int32_t scale); - virtual void read_enc_val(int32_t &enc_val); - virtual void set_enc_val(int32_t enc_val); + virtual void read_enc_val(int32_t &enc_val); + virtual void set_enc_val(int32_t enc_val); virtual int32_t read_enc_val(); - virtual bool set_enc_resolution(int32_t enc_resolution); virtual void get_enc_resolution(int32_t *enc_resolution); diff --git a/sdk/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp b/sdk/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp index 3e3d565..0722c2d 100644 --- a/sdk/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp +++ b/sdk/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp @@ -64,6 +64,22 @@ int32_t XYRobotCtrlModule::do_public_check() { if (!m_state.is_enable) { return err::kxymotor_not_enable; } + + if (!m_stepM1->isMotorIcInitOk()) { + return err::kstep_motor_subic_init_fail; + } + + if (!m_stepM1->isOnline()) { + return err::kstep_motor_subic_offline; + } + if (!m_stepM2->isMotorIcInitOk()) { + return err::kstep_motor_subic_init_fail; + } + + if (!m_stepM2->isOnline()) { + return err::kstep_motor_subic_offline; + } + return 0; } int32_t XYRobotCtrlModule::module_active_cfg() {