From 6d491f649cce31294f24accdfa0541acf5908243 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Wed, 26 Jun 2024 09:44:33 +0800 Subject: [PATCH] update --- .../step_motor_ctrl_module.cpp | 220 +++++++-------------- .../step_motor_ctrl_module.hpp | 25 ++- components/tmc/ic/ztmc5130.cpp | 75 +++++++ components/tmc/ic/ztmc5130.hpp | 20 +- components/zcancmder/zcan_protocol_parser.cpp | 38 ++-- components/zcancmder/zcan_protocol_parser.hpp | 24 +-- 6 files changed, 202 insertions(+), 200 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 951ec38..bb2ce1f 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp @@ -4,7 +4,6 @@ #include #include "a8000_protocol\protocol.hpp" -#include "sdk/components/tmc/ic/ztmc4361A.hpp" #include "sdk\components\flash\znvs.hpp" #include "sdk\components\tmc\ic\ztmc5130.hpp" using namespace iflytop; @@ -30,19 +29,6 @@ void StepMotorCtrlModule::initialize(int moduleid, TMC51X0* stepM, ZGPIO iotable if (tmcmotor) { tmcmotor->getGState(); // 读取状态，清空下复位标识 } - - TMC4361A* tmc4361motor = dynamic_cast(m_stepM1); - if (tmc4361motor) { - auto tmc4361state = tmc4361motor->getTMC4361AMotorEventState(); - auto tmc4361status = tmc4361motor->getTMC4361AMotorStatus(); - auto tmc2160Status = tmc4361motor->getTMC2160MotorDriverStatus(); - auto tmc2160State = tmc4361motor->getTMC2160MotorGstate(); - - dump(tmc4361state); - dump(tmc4361status); - dump(tmc2160Status); - dump(tmc2160State); - } } void StepMotorCtrlModule::create_default_cfg(config_t& cfg) { memset(&cfg, 0, sizeof(cfg)); @@ -62,12 +48,42 @@ void StepMotorCtrlModule::create_default_cfg(config_t& cfg) { int32_t StepMotorCtrlModule::module_xxx_reg(int32_t param_id, bool read, int32_t& val) { int32_t suc = pri_module_xxx_reg(param_id, read, val); if (suc != 0) return suc; + if (!read) { + switch (param_id) { + case kreg_step_motor_shaft: + case kreg_step_motor_one_circle_pulse: + case kreg_step_motor_one_circle_pulse_denominator: + case kreg_step_motor_ihold: + case kreg_step_motor_irun: + case kreg_step_motor_iholddelay: + case kreg_step_motor_max_d: + case kreg_step_motor_min_d: + case kreg_step_motor_iglobalscaler: + case kreg_step_motor_in_debug_mode: + case kreg_step_motor_vstart: + case kreg_step_motor_a1: + case kreg_step_motor_amax: + case kreg_step_motor_v1: + case kreg_step_motor_dmax: + case kreg_step_motor_d1: + case kreg_step_motor_vstop: + case kreg_step_motor_tzerowait: + case kreg_step_motor_enc_resolution: + step_motor_active_cfg(); + break; + default: + break; + } + } return suc; } int32_t StepMotorCtrlModule::pri_module_xxx_reg(int32_t param_id, bool read, int32_t& val) { switch (param_id) { MODULE_COMMON_PROCESS_REG_CB(); PROCESS_REG(kreg_step_motor_pos, inter_get_pos(val), ACTION_NONE); + PROCESS_REG(kreg_step_motor_enc_val, read_enc_val(val), write_enc_val(val)); + PROCESS_REG(kreg_step_motor_denc_val, REG_GET(m_state.denc), ACTION_NONE); + PROCESS_REG(kreg_step_motor_dpos, REG_GET(m_state.dpos), ACTION_NONE); PROCESS_REG(kreg_step_motor_shaft, REG_GET(m_cfg.motor_shaft), REG_SET(m_cfg.motor_shaft)); PROCESS_REG(kreg_step_motor_one_circle_pulse, REG_GET(m_cfg.motor_one_circle_pulse), REG_SET(m_cfg.motor_one_circle_pulse)); @@ -91,6 +107,8 @@ int32_t StepMotorCtrlModule::pri_module_xxx_reg(int32_t param_id, bool read, int PROCESS_REG(kreg_step_motor_d1, REG_GET(m_cfg.motor_d1), REG_SET(m_cfg.motor_d1)); PROCESS_REG(kreg_step_motor_vstop, REG_GET(m_cfg.motor_vstop), REG_SET(m_cfg.motor_vstop)); PROCESS_REG(kreg_step_motor_tzerowait, REG_GET(m_cfg.motor_tzerowait), REG_SET(m_cfg.motor_tzerowait)); + PROCESS_REG(kreg_step_motor_enc_resolution, REG_GET(m_cfg.motor_enc_resolution), REG_SET(m_cfg.motor_enc_resolution)); + PROCESS_REG(kreg_step_motor_enable_enc, REG_GET(m_cfg.motor_enable_enc_resolution), REG_SET(m_cfg.motor_enable_enc_resolution)); default: return err::kmodule_not_find_reg; @@ -121,6 +139,7 @@ int32_t StepMotorCtrlModule::step_motor_easy_set_current_pos(int32_t pos) { int32_t motor_pos = 0; inter_forward_kinematics(pos, motor_pos); m_stepM1->setXACTUAL(motor_pos); + m_stepM1->set_enc_val(motor_pos); return 0; } @@ -141,6 +160,7 @@ int32_t StepMotorCtrlModule::step_motor_active_cfg() { m_stepM1->set_d1(m_cfg.motor_d1); m_stepM1->set_vstop(m_cfg.motor_vstop); m_stepM1->set_tzerowait(m_cfg.motor_tzerowait); + m_stepM1->set_enc_resolution(m_cfg.motor_enc_resolution); // stepmotor_iglobalscaler if (m_state.enable) { @@ -177,38 +197,23 @@ int32_t StepMotorCtrlModule::step_motor_read_tmc5130_state(int32_t* status) { } return 0; }; -int32_t StepMotorCtrlModule::step_motor_read_tmc4361a_status(int32_t* status) { - TMC4361A* tmc4361 = dynamic_cast(m_stepM1); - if (tmc4361) { - auto devStatus = tmc4361->getTMC4361AMotorStatus(); - memcpy(status, &devStatus, sizeof(int32_t)); - } - return 0; -}; -int32_t StepMotorCtrlModule::step_motor_read_tmc4361a_state(int32_t* gstate) { - TMC4361A* tmc4361 = dynamic_cast(m_stepM1); - if (tmc4361) { - auto state = tmc4361->getTMC4361AMotorEventState(); - memcpy(gstate, &state, sizeof(int32_t)); - } - return 0; -}; -int32_t StepMotorCtrlModule::step_motor_read_tmc2160_status(int32_t* status) { - TMC4361A* tmc4361 = dynamic_cast(m_stepM1); - if (tmc4361) { - auto devStatus = tmc4361->getTMC2160MotorDriverStatus(); - memcpy(status, &devStatus, sizeof(int32_t)); + +int32_t StepMotorCtrlModule::step_motor_set_subdevice_reg(int32_t reg_addr, int32_t reg_val) { + TMC51X0* tmc5130 = dynamic_cast(m_stepM1); + if (tmc5130) { + tmc5130->writeInt(reg_addr, reg_val); + return 0; } - return 0; -}; -int32_t StepMotorCtrlModule::step_motor_read_tmc2160_state(int32_t* status) { - TMC4361A* tmc4361 = dynamic_cast(m_stepM1); - if (tmc4361) { - auto state = tmc4361->getTMC2160MotorGstate(); - memcpy(status, &state, sizeof(int32_t)); + return err::kcmd_not_support; +} +int32_t StepMotorCtrlModule::step_motor_get_subdevice_reg(int32_t reg_addr, int32_t* reg_val) { + TMC51X0* tmc5130 = dynamic_cast(m_stepM1); + if (tmc5130) { + *reg_val = tmc5130->readInt(reg_addr); + return 0; } - return 0; -}; + return err::kcmd_not_support; +} int32_t StepMotorCtrlModule::step_motor_read_io_index_in_stm32(int32_t ioindex, int32_t* index_in_stm32) { if (ioindex < 0 || ioindex >= m_nio) { @@ -235,15 +240,18 @@ int StepMotorCtrlModule::inter_get_pos(int32_t& x) { } void StepMotorCtrlModule::befor_motor_move() { // m_state.before_move_pos = m_stepM1->getXACTUAL(); - creg.module_status = 1; + m_state.before_move_enc = m_stepM1->read_enc_val(); + creg.module_status = 1; creg.module_errorcode = 0; - step_motor_active_cfg(); + // m_stepM1->enable(1); } void StepMotorCtrlModule::after_motor_move() { m_state.after_move_pos = m_stepM1->getXACTUAL(); + m_state.after_move_enc = m_stepM1->read_enc_val(); m_state.dpos = m_state.after_move_pos - m_state.before_move_pos; + m_state.denc = m_state.after_move_enc - m_state.before_move_enc; if (creg.module_errorcode != 0) { creg.module_status = 2; } @@ -264,109 +272,28 @@ void StepMotorCtrlModule::dumpTMC5130Status(TMC51X0::DevStatusReg_t* status) { ZLOGI(TAG, "olb: %d", status->olb); ZLOGI(TAG, "stst: %d", status->stst); } -void StepMotorCtrlModule::dump(TMC2160MotorDriverStatus_t val) { - ZLOGI(TAG, "------------ dump TMC2160MotorDriverStatus_t ---------------"); - ZLOGI(TAG, "sg_result: %d", val.sg_result); - ZLOGI(TAG, "s2vsa: %d", val.s2vsa); - ZLOGI(TAG, "s2vsb: %d", val.s2vsb); - ZLOGI(TAG, "stealth: %d", val.stealth); - ZLOGI(TAG, "fsactive: %d", val.fsactive); - ZLOGI(TAG, "cs_actual: %d", val.cs_actual); - ZLOGI(TAG, "reserved2: %d", val.reserved2); - ZLOGI(TAG, "stallguard: %d", val.stallguard); - ZLOGI(TAG, "ot: %d", val.ot); - ZLOGI(TAG, "otpw: %d", val.otpw); - ZLOGI(TAG, "s2ga: %d", val.s2ga); - ZLOGI(TAG, "s2gb: %d", val.s2gb); - ZLOGI(TAG, "ola: %d", val.ola); - ZLOGI(TAG, "olb: %d", val.olb); - ZLOGI(TAG, "stst: %d", val.stst); -} -void StepMotorCtrlModule::dump(TMC4361AMotorStatus_t val) { - ZLOGI(TAG, "------------ dump TMC4361AMotorStatus_t ---------------"); - - ZLOGI(TAG, "TARGET_REACHED_F: %d", val.TARGET_REACHED_F); - ZLOGI(TAG, "POS_COMP_REACHED_F: %d", val.POS_COMP_REACHED_F); - ZLOGI(TAG, "VEL_REACHED_F: %d", val.VEL_REACHED_F); - ZLOGI(TAG, "VEL_STATE_F: %d", val.VEL_STATE_F); - ZLOGI(TAG, "RAMP_STATE_F: %d", val.RAMP_STATE_F); - ZLOGI(TAG, "STOPL_ACTIVE_F: %d", val.STOPL_ACTIVE_F); - ZLOGI(TAG, "STOPR_ACTIVE_F: %d", val.STOPR_ACTIVE_F); - ZLOGI(TAG, "VSTOPL_ACTIVE_F: %d", val.VSTOPL_ACTIVE_F); - ZLOGI(TAG, "VSTOPR_ACTIVE_F: %d", val.VSTOPR_ACTIVE_F); - ZLOGI(TAG, "ACTIVE_STALL_F: %d", val.ACTIVE_STALL_F); - ZLOGI(TAG, "HOME_ERROR_F: %d", val.HOME_ERROR_F); - ZLOGI(TAG, "FS_ACTIVE_F: %d", val.FS_ACTIVE_F); - ZLOGI(TAG, "ENC_FAIL_F: %d", val.ENC_FAIL_F); - ZLOGI(TAG, "N_ACTIVE_F: %d", val.N_ACTIVE_F); - ZLOGI(TAG, "ENC_LATCH_F: %d", val.ENC_LATCH_F); - ZLOGI(TAG, "MULTI_CYCLE_FAIL_F: %d", val.MULTI_CYCLE_FAIL_F); - ZLOGI(TAG, "___: %d", val.___); - ZLOGI(TAG, "CL_FIT_F: %d", val.CL_FIT_F); - ZLOGI(TAG, "SERIAL_ENC_FLAGS: %d", val.SERIAL_ENC_FLAGS); - ZLOGI(TAG, "SG: %d", val.SG); - ZLOGI(TAG, "OT: %d", val.OT); - ZLOGI(TAG, "OTPW: %d", val.OTPW); - ZLOGI(TAG, "S2GA: %d", val.S2GA); - ZLOGI(TAG, "S2GB: %d", val.S2GB); - ZLOGI(TAG, "OLA: %d", val.OLA); - ZLOGI(TAG, "OLB: %d", val.OLB); - ZLOGI(TAG, "STST: %d", val.STST); -} -void StepMotorCtrlModule::dump(TMC2160MotorGstate_t val) { - ZLOGI(TAG, "------------ dump TMC2160MotorGstate_t ---------------"); - - ZLOGI(TAG, "reset: %d", val.reset); - ZLOGI(TAG, "drv_err: %d", val.drv_err); - ZLOGI(TAG, "uv_cp: %d", val.uv_cp); -} -void StepMotorCtrlModule::dump(TMC4361AMotorEventState_t val) { - ZLOGI(TAG, "------------ dump TMC4361AMotorEventState_t ---------------"); - - ZLOGI(TAG, "TARGET_REACHED : %d", val.TARGET_REACHED); - ZLOGI(TAG, "POS_COMP_REACHED : %d", val.POS_COMP_REACHED); - ZLOGI(TAG, "VEL_REACHED : %d", val.VEL_REACHED); - ZLOGI(TAG, "VEL_STATE_EQ_0 : %d", val.VEL_STATE_EQ_0); - ZLOGI(TAG, "VEL_STATE_EQ_1 : %d", val.VEL_STATE_EQ_1); - ZLOGI(TAG, "VEL_STATE_EQ_2 : %d", val.VEL_STATE_EQ_2); - ZLOGI(TAG, "RAMP_STATE_EQ_0 : %d", val.RAMP_STATE_EQ_0); - ZLOGI(TAG, "RAMP_STATE_EQ_1 : %d", val.RAMP_STATE_EQ_1); - ZLOGI(TAG, "RAMP_STATE_EQ_2 : %d", val.RAMP_STATE_EQ_2); - ZLOGI(TAG, "MAX_PHASE_TRAP : %d", val.MAX_PHASE_TRAP); - ZLOGI(TAG, "FROZEN : %d", val.FROZEN); - ZLOGI(TAG, "STOPL : %d", val.STOPL); - ZLOGI(TAG, "STOPR : %d", val.STOPR); - ZLOGI(TAG, "VSTOPL_ACTIVE : %d", val.VSTOPL_ACTIVE); - ZLOGI(TAG, "VSTOPR_ACTIVE : %d", val.VSTOPR_ACTIVE); - ZLOGI(TAG, "HOME_ERROR : %d", val.HOME_ERROR); - ZLOGI(TAG, "XLATCH_DONE : %d", val.XLATCH_DONE); - ZLOGI(TAG, "FS_ACTIVE : %d", val.FS_ACTIVE); - ZLOGI(TAG, "ENC_FAIL : %d", val.ENC_FAIL); - ZLOGI(TAG, "N_ACTIVE : %d", val.N_ACTIVE); - ZLOGI(TAG, "ENC_DONE : %d", val.ENC_DONE); - ZLOGI(TAG, "SER_ENC_DATA_FAIL : %d", val.SER_ENC_DATA_FAIL); - ZLOGI(TAG, "___ : %d", val.___); - ZLOGI(TAG, "SER_DATA_DONE : %d", val.SER_DATA_DONE); - ZLOGI(TAG, "SERIAL_ENC_Flags : %d", val.SERIAL_ENC_Flags); - ZLOGI(TAG, "COVER_DONE : %d", val.COVER_DONE); - ZLOGI(TAG, "ENC_VEL0 : %d", val.ENC_VEL0); - ZLOGI(TAG, "CL_MAX : %d", val.CL_MAX); - ZLOGI(TAG, "CL_FIT : %d", val.CL_FIT); - ZLOGI(TAG, "STOP_ON_STALL : %d", val.STOP_ON_STALL); - ZLOGI(TAG, "MOTOR_EV : %d", val.MOTOR_EV); - ZLOGI(TAG, "RST_EV : %d", val.RST_EV); -} - -void StepMotorCtrlModule::setErrorFlag(int32_t ecode) { - creg.module_errorcode = ecode; -} -bool StepMotorCtrlModule::check_when_run() { - // +void StepMotorCtrlModule::setErrorFlag(int32_t ecode) { creg.module_errorcode = ecode; } - if (m_state.debugmode) { - return true; +int32_t StepMotorCtrlModule::read_enc_val(int32_t& enc_val) { + auto tmc5130 = dynamic_cast(m_stepM1); + if (tmc5130) { + tmc5130->read_enc_val(enc_val); + return 0; + } + return err::kcmd_not_support; +} +int32_t StepMotorCtrlModule::write_enc_val(int32_t enc_val) { + auto tmc5130 = dynamic_cast(m_stepM1); + if (tmc5130) { + tmc5130->set_enc_val(enc_val); + return 0; } + return err::kcmd_not_support; +} +bool StepMotorCtrlModule::check_when_run() { + // + if (m_state.debugmode) return true; TMC51X0* tmc5130 = dynamic_cast(m_stepM1); if (tmc5130 && tmc5130->isTMC5130()) { @@ -671,6 +598,7 @@ int32_t StepMotorCtrlModule::do_step_motor_easy_move_to_zero() { exec_move_to_io_task(0, -1); after_motor_move(); m_stepM1->setXACTUAL(0); + m_stepM1->set_enc_val(0); }, [this]() { m_stepM1->stop(); }); return 0; diff --git a/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp b/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp index 7de0ada..db90d8f 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp @@ -30,12 +30,17 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor { int32_t motor_d1; int32_t motor_vstop; int32_t motor_tzerowait; + int32_t motor_enc_resolution; + int32_t motor_enable_enc_resolution; } config_t; typedef struct { int32_t dpos; + int32_t denc; int32_t after_move_pos; int32_t before_move_pos; + int32_t after_move_enc; + int32_t before_move_enc; int32_t enable; int32_t debugmode; } state_t; @@ -55,7 +60,7 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor { void initialize(int moduleid, TMC51X0* stepM, ZGPIO iotable[], int nio, config_t* cfg); static void create_default_cfg(config_t& cfg); - config_t* get_cfg() { return &m_cfg; } + config_t* get_cfg() { return &m_cfg; } /*********************************************************************************************************************** * ZIModule * @@ -89,10 +94,12 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor { virtual int32_t step_motor_read_tmc5130_status(int32_t* errorflag) override; virtual int32_t step_motor_read_tmc5130_state(int32_t* status) override; - virtual int32_t step_motor_read_tmc4361a_status(int32_t* status) override; - virtual int32_t step_motor_read_tmc4361a_state(int32_t* gstate) override; - virtual int32_t step_motor_read_tmc2160_status(int32_t* status) override; - virtual int32_t step_motor_read_tmc2160_state(int32_t* status) override; + + // virtual int32_t step_motor_read_enc_val(int32_t* enc_val) override; + // virtual int32_t step_motor_set_enc_resolution(int32_t enc_resolution) override; + // virtual int32_t step_motor_get_enc_resolution(int32_t* enc_resolution) override; + virtual int32_t step_motor_set_subdevice_reg(int32_t reg_addr, int32_t reg_val) override; + virtual int32_t step_motor_get_subdevice_reg(int32_t reg_addr, int32_t* reg_val) override; virtual int32_t step_motor_read_io_index_in_stm32(int32_t ioindex, int32_t* index_in_stm32) override; @@ -132,9 +139,9 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor { void dumpTMC5130Status(TMC51X0::DevStatusReg_t* status); void setErrorFlag(int32_t ecode); - void dump(TMC2160MotorDriverStatus_t val); - void dump(TMC4361AMotorStatus_t val); - void dump(TMC2160MotorGstate_t val); - void dump(TMC4361AMotorEventState_t val); + 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); }; } // namespace iflytop \ No newline at end of file diff --git a/components/tmc/ic/ztmc5130.cpp b/components/tmc/ic/ztmc5130.cpp index 651dfa6..b17adb5 100644 --- a/components/tmc/ic/ztmc5130.cpp +++ b/components/tmc/ic/ztmc5130.cpp @@ -327,4 +327,79 @@ void TMC51X0::readMotorState(bool *stoped, int32_t *error) { *stoped = isReachTarget(); } +void TMC51X0::read_enc_val(int32_t &enc_val) { + enc_val = to_user_pos(readInt(TMC5130_XENC)); + if (m_enc_resolution < 0) { + enc_val = -enc_val; + } +} +int32_t TMC51X0::read_enc_val() { + int32_t enc_val; + read_enc_val(enc_val); + return enc_val; +} + +void TMC51X0::set_enc_val(int32_t enc_val) { + int32_t enc_set_val = to_motor_pos(enc_val); + if (m_enc_resolution < 0) { + enc_set_val = -enc_set_val; + } + writeInt(TMC5130_XENC, enc_set_val); +} +bool TMC51X0::set_enc_resolution(int32_t enc_resolution) { + /** + * @brief + * + * 1.假设电机是256细分 + * 2.假设TMC5130_ENC_CONST是十进制模式 + * 3.只支持指定分辨率的编码器 + */ + + int32_t enc_resolution_tmp = enc_resolution * 4; + int16_t enc_const_integral = 0; + int16_t enc_const_fractional = 0; + + switch (abs(enc_resolution_tmp)) { + case 1000: + enc_const_integral = 51; + enc_const_fractional = 0.2 * 10000; + break; + case 1024: + enc_const_integral = 50; + enc_const_fractional = 0 * 10000; + break; + case 4000: + enc_const_integral = 12; + enc_const_fractional = 0.8 * 10000; + break; + case 4096: + enc_const_integral = 12; + enc_const_fractional = 0.5 * 10000; + break; + case 16384: + enc_const_integral = 3; + enc_const_fractional = 0.125 * 10000; + break; + case 0: + m_enc_resolution = 0; + return true; + + default: + return false; + break; + } + + m_enc_resolution = enc_resolution; + uint32_t setval = 0; + uint8_t *psetval = (uint8_t *)&setval; + + memcpy(psetval + 2, &enc_const_integral, 2); + memcpy(psetval, &enc_const_fractional, 2); + + writeInt(TMC5130_ENCMODE, 0x1 << 10); + writeInt(TMC5130_ENC_CONST, setval); + return true; +} +void TMC51X0::get_enc_resolution(int32_t *enc_resolution) { *enc_resolution = m_enc_resolution; } + #endif diff --git a/components/tmc/ic/ztmc5130.hpp b/components/tmc/ic/ztmc5130.hpp index 7802888..5b07988 100644 --- a/components/tmc/ic/ztmc5130.hpp +++ b/components/tmc/ic/ztmc5130.hpp @@ -79,17 +79,17 @@ class TMC51X0 : public IStepperMotor { typedef struct { SPI_HandleTypeDef *spi; - Pin_t csgpio = PinNull; // - Pin_t ennPin = PinNull; // + Pin_t csgpio = PinNull; // + Pin_t ennPin = PinNull; // } cfg_t; protected: cfg_t m_cfg; - ZGPIO *m_csnpin = NULL; - ZGPIO *m_ennpin = NULL; - SPI_HandleTypeDef *m_hspi = NULL; + ZGPIO *m_csnpin = NULL; + ZGPIO *m_ennpin = NULL; + SPI_HandleTypeDef *m_hspi = NULL; // uint8_t m_channel; uint32_t m_lastCallPeriodicJobTick; @@ -104,6 +104,8 @@ class TMC51X0 : public IStepperMotor { mres_type_t m_MRES = kmres_256; double m_onecirclepulse = 51200; + int32_t m_enc_resolution = 0; // 编码器分辨率，默认只有在256细分的情况下有效 + public: TMC51X0(/* args */); @@ -130,6 +132,14 @@ 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 int32_t read_enc_val(); + + + virtual bool set_enc_resolution(int32_t enc_resolution); + virtual void get_enc_resolution(int32_t *enc_resolution); + virtual bool isReachTarget(); // 是否到达目标位置 virtual bool isStoped() { return isReachTarget(); } virtual void setNoAccLimit(bool enable) override; diff --git a/components/zcancmder/zcan_protocol_parser.cpp b/components/zcancmder/zcan_protocol_parser.cpp index db9ed30..a116f97 100644 --- a/components/zcancmder/zcan_protocol_parser.cpp +++ b/components/zcancmder/zcan_protocol_parser.cpp @@ -38,11 +38,9 @@ void ZCanProtocolParser::initialize(IZCanReceiver* cancmder) { REGFN(step_motor_read_tmc5130_status); REGFN(step_motor_read_tmc5130_state); - REGFN(step_motor_read_tmc4361a_status); - REGFN(step_motor_read_tmc4361a_state); - REGFN(step_motor_read_tmc2160_status); - REGFN(step_motor_read_tmc2160_state); REGFN(step_motor_read_io_index_in_stm32); + REGFN(step_motor_set_subdevice_reg); + REGFN(step_motor_get_subdevice_reg); REGFN(mini_servo_enable); REGFN(mini_servo_read_pos); @@ -318,35 +316,23 @@ int32_t ZCanProtocolParser::step_motor_read_tmc5130_state(cmdcontxt_t* cxt) { cxt->acklen = 4; return module->step_motor_read_tmc5130_state(&ack[0]); } -int32_t ZCanProtocolParser::step_motor_read_tmc4361a_status(cmdcontxt_t* cxt) { - CHECK_AND_GET_MODULE(0); - int32_t* ack = (int32_t*)cxt->ackbuf; - cxt->acklen = 4; - return module->step_motor_read_tmc4361a_status(&ack[0]); -} -int32_t ZCanProtocolParser::step_motor_read_tmc4361a_state(cmdcontxt_t* cxt) { - CHECK_AND_GET_MODULE(0); - int32_t* ack = (int32_t*)cxt->ackbuf; - cxt->acklen = 4; - return module->step_motor_read_tmc4361a_state(&ack[0]); -} -int32_t ZCanProtocolParser::step_motor_read_tmc2160_status(cmdcontxt_t* cxt) { - CHECK_AND_GET_MODULE(0); +int32_t ZCanProtocolParser::step_motor_read_io_index_in_stm32(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(1); int32_t* ack = (int32_t*)cxt->ackbuf; cxt->acklen = 4; - return module->step_motor_read_tmc2160_status(&ack[0]); + return module->step_motor_read_io_index_in_stm32(cxt->params[0], ack); } -int32_t ZCanProtocolParser::step_motor_read_tmc2160_state(cmdcontxt_t* cxt) { - CHECK_AND_GET_MODULE(0); - int32_t* ack = (int32_t*)cxt->ackbuf; - cxt->acklen = 4; - return module->step_motor_read_tmc2160_state(&ack[0]); + +int32_t ZCanProtocolParser::step_motor_set_subdevice_reg(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(2); + return module->step_motor_set_subdevice_reg(cxt->params[0], cxt->params[1]); } -int32_t ZCanProtocolParser::step_motor_read_io_index_in_stm32(cmdcontxt_t* cxt) { + +int32_t ZCanProtocolParser::step_motor_get_subdevice_reg(cmdcontxt_t* cxt) { CHECK_AND_GET_MODULE(1); int32_t* ack = (int32_t*)cxt->ackbuf; cxt->acklen = 4; - return module->step_motor_read_io_index_in_stm32(cxt->params[0], ack); + return module->step_motor_get_subdevice_reg(cxt->params[0], ack); } #undef MODULE_CLASS diff --git a/components/zcancmder/zcan_protocol_parser.hpp b/components/zcancmder/zcan_protocol_parser.hpp index 09fd677..b96d1f6 100644 --- a/components/zcancmder/zcan_protocol_parser.hpp +++ b/components/zcancmder/zcan_protocol_parser.hpp @@ -74,11 +74,9 @@ class ZCanProtocolParser : public IZCanReceiverListener { CMDFN(step_motor_easy_move_to_end_point); CMDFN(step_motor_read_tmc5130_status); CMDFN(step_motor_read_tmc5130_state); - CMDFN(step_motor_read_tmc4361a_status); - CMDFN(step_motor_read_tmc4361a_state); - CMDFN(step_motor_read_tmc2160_status); - CMDFN(step_motor_read_tmc2160_state); CMDFN(step_motor_read_io_index_in_stm32); + CMDFN(step_motor_set_subdevice_reg); + CMDFN(step_motor_get_subdevice_reg); CMDFN(mini_servo_enable); CMDFN(mini_servo_read_pos); @@ -155,16 +153,14 @@ class ZCanProtocolParser : public IZCanReceiverListener { CMDFN(a8000_idcard_reader_read_raw); - -CMDFN(plate_code_scaner_push_card_and_scan); -CMDFN(plate_code_scaner_stop_scan); -CMDFN(plate_code_scaner_read_result); -CMDFN(plate_code_scaner_read_result_point_num); -CMDFN(plate_code_scaner_read_code); -CMDFN(plate_code_scaner_adc_readraw); -CMDFN(plate_code_scaner_open_laser); -CMDFN(plate_code_scaner_close_laser); - + CMDFN(plate_code_scaner_push_card_and_scan); + CMDFN(plate_code_scaner_stop_scan); + CMDFN(plate_code_scaner_read_result); + CMDFN(plate_code_scaner_read_result_point_num); + CMDFN(plate_code_scaner_read_code); + CMDFN(plate_code_scaner_adc_readraw); + CMDFN(plate_code_scaner_open_laser); + CMDFN(plate_code_scaner_close_laser); }; } // namespace iflytop \ No newline at end of file