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 d76b413..0262d00 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp @@ -37,6 +37,11 @@ void StepMotorCtrlModule::initialize(int moduleid, IStepperMotor* stepM, ZGPIO i 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) { @@ -104,6 +109,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_in_debug_mode, REG_GET(m_state.debugmode), REG_SET(m_state.debugmode)); default: return err::kmodule_not_find_config_index; @@ -246,67 +252,111 @@ void StepMotorCtrlModule::after_motor_move() { } void StepMotorCtrlModule::dumpTMC5130Status(TMC51X0::DevStatusReg_t* status) { - ZLOGE(TAG, "sg_result: %d", status->sg_result); - ZLOGE(TAG, "fsactive: %d", status->fsactive); - ZLOGE(TAG, "cs_actual: %d", status->cs_actual); - ZLOGE(TAG, "stallguard: %d", status->stallguard); - ZLOGE(TAG, "ot: %d", status->ot); - ZLOGE(TAG, "otpw: %d", status->otpw); - ZLOGE(TAG, "s2ga: %d", status->s2ga); - ZLOGE(TAG, "s2gb: %d", status->s2gb); - ZLOGE(TAG, "ola: %d", status->ola); - ZLOGE(TAG, "olb: %d", status->olb); - ZLOGE(TAG, "stst: %d", status->stst); + ZLOGI(TAG, "------------ dump TMC51X0 DevStatusReg_t ---------------"); + ZLOGI(TAG, "sg_result: %d", status->sg_result); + ZLOGI(TAG, "fsactive: %d", status->fsactive); + ZLOGI(TAG, "cs_actual: %d", status->cs_actual); + ZLOGI(TAG, "stallguard: %d", status->stallguard); + ZLOGI(TAG, "ot: %d", status->ot); + ZLOGI(TAG, "otpw: %d", status->otpw); + ZLOGI(TAG, "s2ga: %d", status->s2ga); + ZLOGI(TAG, "s2gb: %d", status->s2gb); + ZLOGI(TAG, "ola: %d", status->ola); + ZLOGI(TAG, "olb: %d", status->olb); + ZLOGI(TAG, "stst: %d", status->stst); } void StepMotorCtrlModule::dump(TMC2160MotorDriverStatus_t val) { - ZLOGE(TAG, "sg_result: %d", val.sg_result); - ZLOGE(TAG, "s2vsa: %d", val.s2vsa); - ZLOGE(TAG, "s2vsb: %d", val.s2vsb); - ZLOGE(TAG, "stealth: %d", val.stealth); - ZLOGE(TAG, "fsactive: %d", val.fsactive); - ZLOGE(TAG, "cs_actual: %d", val.cs_actual); - ZLOGE(TAG, "reserved2: %d", val.reserved2); - ZLOGE(TAG, "stallguard: %d", val.stallguard); - ZLOGE(TAG, "ot: %d", val.ot); - ZLOGE(TAG, "otpw: %d", val.otpw); - ZLOGE(TAG, "s2ga: %d", val.s2ga); - ZLOGE(TAG, "s2gb: %d", val.s2gb); - ZLOGE(TAG, "ola: %d", val.ola); - ZLOGE(TAG, "olb: %d", val.olb); - ZLOGE(TAG, "stst: %d", val.stst); + 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) { - - ZLOGE(TAG, "TARGET_REACHED_F: %d", val.TARGET_REACHED_F); - ZLOGE(TAG, "POS_COMP_REACHED_F: %d", val.POS_COMP_REACHED_F); - ZLOGE(TAG, "VEL_REACHED_F: %d", val.VEL_REACHED_F); - ZLOGE(TAG, "VEL_STATE_F: %d", val.VEL_STATE_F); - ZLOGE(TAG, "RAMP_STATE_F: %d", val.RAMP_STATE_F); - ZLOGE(TAG, "STOPL_ACTIVE_F: %d", val.STOPL_ACTIVE_F); - ZLOGE(TAG, "STOPR_ACTIVE_F: %d", val.STOPR_ACTIVE_F); - ZLOGE(TAG, "VSTOPL_ACTIVE_F: %d", val.VSTOPL_ACTIVE_F); - ZLOGE(TAG, "VSTOPR_ACTIVE_F: %d", val.VSTOPR_ACTIVE_F); - ZLOGE(TAG, "ACTIVE_STALL_F: %d", val.ACTIVE_STALL_F); - ZLOGE(TAG, "HOME_ERROR_F: %d", val.HOME_ERROR_F); - ZLOGE(TAG, "FS_ACTIVE_F: %d", val.FS_ACTIVE_F); - ZLOGE(TAG, "ENC_FAIL_F: %d", val.ENC_FAIL_F); - ZLOGE(TAG, "N_ACTIVE_F: %d", val.N_ACTIVE_F); - ZLOGE(TAG, "ENC_LATCH_F: %d", val.ENC_LATCH_F); - ZLOGE(TAG, "MULTI_CYCLE_FAIL_F: %d", val.MULTI_CYCLE_FAIL_F); - ZLOGE(TAG, "___: %d", val.___); - ZLOGE(TAG, "CL_FIT_F: %d", val.CL_FIT_F); - ZLOGE(TAG, "SERIAL_ENC_FLAGS: %d", val.SERIAL_ENC_FLAGS); - ZLOGE(TAG, "SG: %d", val.SG); - ZLOGE(TAG, "OT: %d", val.OT); - ZLOGE(TAG, "OTPW: %d", val.OTPW); - ZLOGE(TAG, "S2GA: %d", val.S2GA); - ZLOGE(TAG, "S2GB: %d", val.S2GB); - ZLOGE(TAG, "OLA: %d", val.OLA); - ZLOGE(TAG, "OLB: %d", val.OLB); - ZLOGE(TAG, "STST: %d", val.STST); -} -void StepMotorCtrlModule::dump(TMC2160MotorGstate_t val) {} -void StepMotorCtrlModule::dump(TMC4361AMotorEventState_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, uint32_t motorStatue) { creg.module_errorcode = ecode; @@ -315,6 +365,11 @@ void StepMotorCtrlModule::setErrorFlag(int32_t ecode, uint32_t motorStatue) { bool StepMotorCtrlModule::check_when_run() { // + + if (m_state.debugmode) { + return true; + } + TMC51X0* tmc5130 = dynamic_cast(m_stepM1); if (tmc5130) { auto state = tmc5130->getGState(); 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 fb346bb..6691699 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp @@ -35,13 +35,14 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor { int32_t after_move_pos; int32_t before_move_pos; int32_t enable; + int32_t debugmode; } state_t; private: IStepperMotor* m_stepM1 = nullptr; int m_id = 0; config_t m_cfg; - state_t m_state; + state_t m_state = {0}; ZGPIO* m_iotable = nullptr; int m_nio = 0; diff --git a/components/tmc/ic/ztmc4361A.cpp b/components/tmc/ic/ztmc4361A.cpp index e517f4d..6e52d81 100644 --- a/components/tmc/ic/ztmc4361A.cpp +++ b/components/tmc/ic/ztmc4361A.cpp @@ -194,6 +194,78 @@ void TMC4361A::initialize(cfg_t *cfg) { void TMC4361A::setScale(int32_t scale) { m_scale = scale; } void TMC4361A::setScaleDenominator(int32_t scale) { m_scale_deceleration = scale; } +void TMC4361A::init() { + tmc4361_writeInt(TMC4361A_INPUT_FILT_CONF, 0x540022); // + tmc4361_writeInt(TMC4361A_SPIOUT_CONF, 0x8440000d); // + tmc4361_writeInt(TMC4361A_GENERAL_CONF, 0x7026); // + tmc4361_writeInt(TMC4361A_STP_LENGTH_ADD, 0x60004); // direct-a, direct-bow + + driverIC_writeInt(0x81, 0x07); + driverIC_writeInt(0x80, 0x12200); + driverIC_writeInt(0x90, 0x00071600); + driverIC_writeInt(0x91, 0x0a); + driverIC_writeInt(0x94, 200); + driverIC_writeInt(0x95, 50); + driverIC_writeInt(0xEE, 0x70025); + driverIC_writeInt(0xF0, 0x404FF); + driverIC_writeInt(0xEC, 0x03010223); + driverIC_writeInt(0xED, 0x100a220); + + tmc4361_writeInt(0x24, 0); + tmc4361_writeInt(0x21, 0); + tmc4361_writeInt(0x37, 0); + + driverIC_writeInt(0x6D, 0x6d); + driverIC_writeInt(0x6c, 0); + +#if 0 + tmc4361_writeInt(0x54, 4000); // Encoder resolution = 4000/rev + tmc4361_writeInt(0x1C, 0xff00ff); // CL_BETA=CL_GAMMA=255 + tmc4361_writeInt(0x5C, 0x10000); // cl_p = 1.0 + + // cl calibration: + tmc4361_writeInt(0x20, 0x06); // hold+pos mode + tmc4361_writeInt(0x24, 0x100000); // slow vel + tmc4361_writeInt(0x07, 0x400000); // turn on cl + tmc4361_writeInt(0x37, 0x80); // move to FS position + tmc4361_writeInt(0x07, 0x1400000); // turn on cl calibration + osDelay(100); + tmc4361_writeInt(0x07, 0x400000); // turn off cl calibration + tmc4361_writeInt(0x24, 0); // v=0 +#endif + + // S-shaped Ramps setup: + tmc4361_writeInt(0x20, 0x06); // s-ramp + pos mode + tmc4361_writeInt(0x24, 0x31A8000); // vmax=800kpps + tmc4361_writeInt(0x28, 30000); // amax + tmc4361_writeInt(0x29, 15000); // dmax + tmc4361_writeInt(0x2D, 0x100); // bow1 + tmc4361_writeInt(0x2E, 0x100); // bow2 + tmc4361_writeInt(0x2F, 0x100); // bow3 + tmc4361_writeInt(0x30, 0x100); // bow4 + +// cl operation setup: +#if 0 + tmc4361_writeInt(0x60, 250000); // emf_vmin = + tmc4361_writeInt(0x61, 450000); // emf_vadd = -> emf_vmax = + tmc4361_writeInt(0x62, 0xffffff); // emf_vel0_timer + tmc4361_writeInt(0x63, 0x2000864); // enc vel filter settings + + tmc4361_writeInt(0x5F, 5); // cl_tolerance=20 + tmc4361_writeInt(0x5C, 0x10000); // cl_p=1.0 + tmc4361_writeInt(0x5A, 3000); // cl_vlimit_p=200 + tmc4361_writeInt(0x5B, 300); // cl_vlimit_i=50 + tmc4361_writeInt(0x5D, 1000); // cl_vlimit_diclip=1000 + tmc4361_writeInt(0x5E, 200000); // cl_vlimit=200kpps + + tmc4361_writeInt(0x06, 0x32ff64); // cl scaling values + tmc4361_writeInt(0x18, 600); // cl_upscale=1000 + tmc4361_writeInt(0x19, 200000); // cl_dnscale=10000 + + tmc4361_writeInt(0x07, 0xa400000); // cl with gamma correction and vlimit + tmc4361_writeInt(0x05, 0x80); // cl scaling on +#endif +} uint8_t TMC4361A::reset() { // Pulse the low-active hardware reset pin @@ -211,35 +283,37 @@ uint8_t TMC4361A::reset() { * @brief 重置芯片镜像寄存?? * */ - for (uint32_t add = 0; add < TMC4361A_REGISTER_COUNT; add++) { - if (!TMC_IS_RESETTABLE(m_registerAccessTable[add])) { - continue; - } - tmc4361_writeInt(add, m_defaultRegisterResetState[add]); - } - - uint8_t driver, dataLength; - uint32_t value; - // Setup SPI - switch (m_driver_ic_type) { - case IC_TMC2130: - case IC_TMC2160: - driver = 0x0C; - dataLength = 0; - break; - case IC_TMC2660: - driver = 0x0B; - dataLength = 0; - break; - default: - driver = 0x0F; - dataLength = 40; - break; - } - value = 0x44400040 | (dataLength << 13) | (driver << 0); - tmc4361_writeInt(TMC4361A_SPIOUT_CONF, value); - tmc4361_writeInt(TMC4361A_CURRENT_CONF, 0x00000003); - tmc4361_writeInt(TMC4361A_SCALE_VALUES, 0x00000000); + // for (uint32_t add = 0; add < TMC4361A_REGISTER_COUNT; add++) { + // if (!TMC_IS_RESETTABLE(m_registerAccessTable[add])) { + // continue; + // } + // tmc4361_writeInt(add, m_defaultRegisterResetState[add]); + // } + + init(); + + // uint8_t driver, dataLength; + // uint32_t value; + // // Setup SPI + // switch (m_driver_ic_type) { + // case IC_TMC2130: + // case IC_TMC2160: + // driver = 0x0C; + // dataLength = 0; + // break; + // case IC_TMC2660: + // driver = 0x0B; + // dataLength = 0; + // break; + // default: + // driver = 0x0F; + // dataLength = 40; + // break; + // } + // value = 0x44400040 | (dataLength << 13) | (driver << 0); + // tmc4361_writeInt(TMC4361A_SPIOUT_CONF, value); + // tmc4361_writeInt(TMC4361A_CURRENT_CONF, 0x00000003); + // tmc4361_writeInt(TMC4361A_SCALE_VALUES, 0x00000000); return 1; } uint8_t TMC4361A::restore() { return 1; } diff --git a/components/tmc/ic/ztmc4361A.hpp b/components/tmc/ic/ztmc4361A.hpp index 52b114c..07fbc8d 100644 --- a/components/tmc/ic/ztmc4361A.hpp +++ b/components/tmc/ic/ztmc4361A.hpp @@ -140,6 +140,9 @@ class TMC4361A : public IStepperMotor { int32_t to_user_pos(int32_t pos); int32_t to_user_vel(int32_t vel); uint32_t haspassedms(uint32_t now, uint32_t last); + + void init(); + }; } // namespace iflytop diff --git a/components/zcancmder/zcan_protocol_parser.cpp b/components/zcancmder/zcan_protocol_parser.cpp index 987c6bf..f9f5ef8 100644 --- a/components/zcancmder/zcan_protocol_parser.cpp +++ b/components/zcancmder/zcan_protocol_parser.cpp @@ -34,9 +34,15 @@ void ZCanProtocolParser::initialize(IZCanReceiver* cancmder) { REGFN(step_motor_active_cfg); REGFN(step_motor_stop); REGFN(step_motor_read_io_state); - REGFN(step_motor_read_tmc5130_status); REGFN(step_motor_easy_move_to_end_point); + 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(mini_servo_enable); REGFN(mini_servo_read_pos); REGFN(mini_servo_active_cfg); @@ -255,15 +261,46 @@ int32_t ZCanProtocolParser::step_motor_read_io_state(cmdcontxt_t* cxt) { return module->step_motor_read_io_state(cxt->params[0], ack); } +int32_t ZCanProtocolParser::step_motor_easy_move_to_end_point(cmdcontxt_t* cxt) { + CHECK_AND_GET_MODULE(0); + return module->step_motor_easy_move_to_end_point(); +} + int32_t ZCanProtocolParser::step_motor_read_tmc5130_status(cmdcontxt_t* cxt) { CHECK_AND_GET_MODULE(0); int32_t* ack = (int32_t*)cxt->ackbuf; cxt->acklen = 4; return module->step_motor_read_tmc5130_status(&ack[0]); } -int32_t ZCanProtocolParser::step_motor_easy_move_to_end_point(cmdcontxt_t* cxt) { +int32_t ZCanProtocolParser::step_motor_read_tmc5130_state(cmdcontxt_t* cxt) { CHECK_AND_GET_MODULE(0); - return module->step_motor_easy_move_to_end_point(); + int32_t* ack = (int32_t*)cxt->ackbuf; + 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* ack = (int32_t*)cxt->ackbuf; + cxt->acklen = 4; + return module->step_motor_read_tmc2160_status(&ack[0]); +} +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]); } #undef MODULE_CLASS diff --git a/components/zcancmder/zcan_protocol_parser.hpp b/components/zcancmder/zcan_protocol_parser.hpp index c7da80e..5d69777 100644 --- a/components/zcancmder/zcan_protocol_parser.hpp +++ b/components/zcancmder/zcan_protocol_parser.hpp @@ -68,9 +68,13 @@ class ZCanProtocolParser : public IZCanReceiverListener { static int32_t step_motor_easy_move_to_io(cmdcontxt_t* cxt); static int32_t step_motor_active_cfg(cmdcontxt_t* cxt); static int32_t step_motor_read_io_state(cmdcontxt_t* cxt); - static int32_t step_motor_read_tmc5130_status(cmdcontxt_t* cxt); static int32_t step_motor_easy_move_to_end_point(cmdcontxt_t* cxt); - + static int32_t step_motor_read_tmc5130_status(cmdcontxt_t* cxt); + static int32_t step_motor_read_tmc5130_state(cmdcontxt_t* cxt); + static int32_t step_motor_read_tmc4361a_status(cmdcontxt_t* cxt); + static int32_t step_motor_read_tmc4361a_state(cmdcontxt_t* cxt); + static int32_t step_motor_read_tmc2160_status(cmdcontxt_t* cxt); + static int32_t step_motor_read_tmc2160_state(cmdcontxt_t* cxt); static int32_t mini_servo_enable(cmdcontxt_t* cxt); static int32_t mini_servo_read_pos(cmdcontxt_t* cxt);