project_boditech_vidas_a8000_v3
/
a8000_subboard_sdk
2
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

update

master
zhaohe 1 year ago
parent
90ea2d6ded
commit
64fcdac4e9
6 changed files with 266 additions and 92 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 169
      components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
  2. 3
      components/step_motor_ctrl_module/step_motor_ctrl_module.hpp
  3. 132
      components/tmc/ic/ztmc4361A.cpp
  4. 3
      components/tmc/ic/ztmc4361A.hpp
  5. 43
      components/zcancmder/zcan_protocol_parser.cpp
  6. 8
      components/zcancmder/zcan_protocol_parser.hpp

169
components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
View File

@ -37,6 +37,11 @@ void StepMotorCtrlModule::initialize(int moduleid, IStepperMotor* stepM, ZGPIO i
auto tmc4361status = tmc4361motor->getTMC4361AMotorStatus(); auto tmc4361status = tmc4361motor->getTMC4361AMotorStatus();
auto tmc2160Status = tmc4361motor->getTMC2160MotorDriverStatus(); auto tmc2160Status = tmc4361motor->getTMC2160MotorDriverStatus();
auto tmc2160State = tmc4361motor->getTMC2160MotorGstate(); auto tmc2160State = tmc4361motor->getTMC2160MotorGstate();
dump(tmc4361state);
dump(tmc4361status);
dump(tmc2160Status);
dump(tmc2160State);
} }
} }
void StepMotorCtrlModule::create_default_cfg(config_t& cfg) { 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_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_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_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: default:
return err::kmodule_not_find_config_index; return err::kmodule_not_find_config_index;
@ -246,67 +252,111 @@ void StepMotorCtrlModule::after_motor_move() {
} }
void StepMotorCtrlModule::dumpTMC5130Status(TMC51X0::DevStatusReg_t* status) { 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) { 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) { 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) { void StepMotorCtrlModule::setErrorFlag(int32_t ecode, uint32_t motorStatue) {
creg.module_errorcode = ecode; creg.module_errorcode = ecode;
@ -315,6 +365,11 @@ void StepMotorCtrlModule::setErrorFlag(int32_t ecode, uint32_t motorStatue) {
bool StepMotorCtrlModule::check_when_run() { bool StepMotorCtrlModule::check_when_run() {
// //
if (m_state.debugmode) {
return true;
}
TMC51X0* tmc5130 = dynamic_cast<TMC51X0*>(m_stepM1); TMC51X0* tmc5130 = dynamic_cast<TMC51X0*>(m_stepM1);
if (tmc5130) { if (tmc5130) {
auto state = tmc5130->getGState(); auto state = tmc5130->getGState();

3
components/step_motor_ctrl_module/step_motor_ctrl_module.hpp
View File

@ -35,13 +35,14 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor {
int32_t after_move_pos; int32_t after_move_pos;
int32_t before_move_pos; int32_t before_move_pos;
int32_t enable; int32_t enable;
int32_t debugmode;
} state_t; } state_t;
private: private:
IStepperMotor* m_stepM1 = nullptr; IStepperMotor* m_stepM1 = nullptr;
int m_id = 0; int m_id = 0;
config_t m_cfg; config_t m_cfg;
state_t m_state;
state_t m_state = {0};
ZGPIO* m_iotable = nullptr; ZGPIO* m_iotable = nullptr;
int m_nio = 0; int m_nio = 0;

132
components/tmc/ic/ztmc4361A.cpp
View File

@ -194,6 +194,78 @@ void TMC4361A::initialize(cfg_t *cfg) {
void TMC4361A::setScale(int32_t scale) { m_scale = scale; } void TMC4361A::setScale(int32_t scale) { m_scale = scale; }
void TMC4361A::setScaleDenominator(int32_t scale) { m_scale_deceleration = 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() { uint8_t TMC4361A::reset() {
// Pulse the low-active hardware reset pin // Pulse the low-active hardware reset pin
@ -211,35 +283,37 @@ uint8_t TMC4361A::reset() {
* @brief 重置芯片镜像寄存?? * @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; return 1;
} }
uint8_t TMC4361A::restore() { return 1; } uint8_t TMC4361A::restore() { return 1; }

3
components/tmc/ic/ztmc4361A.hpp
View File

@ -140,6 +140,9 @@ class TMC4361A : public IStepperMotor {
int32_t to_user_pos(int32_t pos); int32_t to_user_pos(int32_t pos);
int32_t to_user_vel(int32_t vel); int32_t to_user_vel(int32_t vel);
uint32_t haspassedms(uint32_t now, uint32_t last); uint32_t haspassedms(uint32_t now, uint32_t last);
void init();
}; };
} // namespace iflytop } // namespace iflytop

43
components/zcancmder/zcan_protocol_parser.cpp
View File

@ -34,9 +34,15 @@ void ZCanProtocolParser::initialize(IZCanReceiver* cancmder) {
REGFN(step_motor_active_cfg); REGFN(step_motor_active_cfg);
REGFN(step_motor_stop); REGFN(step_motor_stop);
REGFN(step_motor_read_io_state); REGFN(step_motor_read_io_state);
REGFN(step_motor_read_tmc5130_status);
REGFN(step_motor_easy_move_to_end_point); 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_enable);
REGFN(mini_servo_read_pos); REGFN(mini_servo_read_pos);
REGFN(mini_servo_active_cfg); 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); 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) { int32_t ZCanProtocolParser::step_motor_read_tmc5130_status(cmdcontxt_t* cxt) {
CHECK_AND_GET_MODULE(0); CHECK_AND_GET_MODULE(0);
int32_t* ack = (int32_t*)cxt->ackbuf; int32_t* ack = (int32_t*)cxt->ackbuf;
cxt->acklen = 4; cxt->acklen = 4;
return module->step_motor_read_tmc5130_status(&ack[0]); 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); 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 #undef MODULE_CLASS

8
components/zcancmder/zcan_protocol_parser.hpp
View File

@ -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_easy_move_to_io(cmdcontxt_t* cxt);
static int32_t step_motor_active_cfg(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_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_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_enable(cmdcontxt_t* cxt);
static int32_t mini_servo_read_pos(cmdcontxt_t* cxt); static int32_t mini_servo_read_pos(cmdcontxt_t* cxt);

Write Preview
Loading…
Cancel
Save