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 71938a3..de8f138 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp @@ -578,18 +578,18 @@ void StepMotorCtrlModule::call_exec_status_cb(int32_t status, action_cb_status_t } void StepMotorCtrlModule::create_default_cfg(flash_config_t& cfg) { memset(&cfg, 0, sizeof(cfg)); - cfg.base_param.distance_scale = 1000; + cfg.base_param.distance_scale = 10000; cfg.base_param.ihold = 0; cfg.base_param.irun = 8; cfg.base_param.iholddelay = 100; - cfg.base_param.acc = 3000000; - cfg.base_param.dec = 3000000; - cfg.base_param.maxspeed = 500000; - - cfg.base_param.run_to_zero_move_to_zero_max_d = 51200 * 1000; - cfg.base_param.run_to_zero_leave_from_zero_max_d = 51200 * 10; - cfg.base_param.run_to_zero_speed = 100000; - cfg.base_param.run_to_zero_dec = 3000000; + cfg.base_param.acc = 300; + cfg.base_param.dec = 300; + cfg.base_param.maxspeed = 800; + + cfg.base_param.run_to_zero_move_to_zero_max_d = 10000 * 100; + cfg.base_param.run_to_zero_leave_from_zero_max_d = 10000 * 2; + cfg.base_param.run_to_zero_speed = 100; + cfg.base_param.run_to_zero_dec = 600; } int32_t StepMotorCtrlModule::flush() { diff --git a/components/tmc/ic/ztmc4361A.cpp b/components/tmc/ic/ztmc4361A.cpp index bdcb743..438ac57 100644 --- a/components/tmc/ic/ztmc4361A.cpp +++ b/components/tmc/ic/ztmc4361A.cpp @@ -141,8 +141,8 @@ void TMC4361A::setAcceleration(float accelerationpps2) { * AMAX [pps2] = AMAX / 237 ?? fCLK^2 */ - accelerationpps2 *= m_scale; - int32_t acc = (int32_t)accelerationpps2; + accelerationpps2 = to_motor_acc(accelerationpps2); + int32_t acc = (int32_t)accelerationpps2; writeInt(TMC4361A_AMAX, acc << 2); } void TMC4361A::setDeceleration(float accelerationpps2) { @@ -156,8 +156,8 @@ void TMC4361A::setDeceleration(float accelerationpps2) { * a[?v per clk_cycle]= AMAX / 2^37 * AMAX [pps2] = AMAX / 237 ?? fCLK^2 */ - accelerationpps2 *= m_scale; - int32_t acc = (int32_t)accelerationpps2; + accelerationpps2 = to_motor_acc(accelerationpps2); + int32_t acc = (int32_t)accelerationpps2; writeInt(TMC4361A_DMAX, acc << 2); } #define INIT_GPIO(m_pin, pin, ...) \ @@ -201,7 +201,7 @@ void TMC4361A::initialize(cfg_t *cfg) { driverIC_setIHOLD_IRUN(1, 3, 0); } -void TMC4361A::setScale(float scale) { m_scale = scale; } +void TMC4361A::setScale(int32_t scale) { m_scale = scale; } uint8_t TMC4361A::reset() { // Pulse the low-active hardware reset pin @@ -247,13 +247,13 @@ uint8_t TMC4361A::reset() { } uint8_t TMC4361A::restore() { return 1; } -int32_t TMC4361A::getXACTUAL() { return readInt(TMC4361A_XACTUAL) / m_scale; } -void TMC4361A::setXACTUAL(int32_t value) { writeInt(TMC4361A_XACTUAL, value * m_scale); } -int32_t TMC4361A::getVACTUAL() { return readInt(TMC4361A_VACTUAL) / m_scale; } -int32_t TMC4361A::getXTARGET() { return readInt(TMC4361A_X_TARGET) / m_scale; } -int32_t TMC4361A::getENC_POS() { return readInt(TMC4361A_ENC_POS) / m_scale; } -void TMC4361A::setENC_POS(int32_t value) { writeInt(TMC4361A_ENC_POS, value * m_scale); } -int32_t TMC4361A::getENC_POS_DEV() { return readInt(TMC4361A_ENC_POS_DEV_RD) / m_scale; } +int32_t TMC4361A::getXACTUAL() { return to_user_pos(readInt(TMC4361A_XACTUAL)); } +void TMC4361A::setXACTUAL(int32_t value) { writeInt(TMC4361A_XACTUAL, to_motor_pos(value)); } +int32_t TMC4361A::getVACTUAL() { return to_user_pos(readInt(TMC4361A_VACTUAL)); } +int32_t TMC4361A::getXTARGET() { return to_user_pos(readInt(TMC4361A_X_TARGET)); } +int32_t TMC4361A::getENC_POS() { return to_user_pos(readInt(TMC4361A_ENC_POS)); } +void TMC4361A::setENC_POS(int32_t value) { writeInt(TMC4361A_ENC_POS, to_motor_pos(value)); } +int32_t TMC4361A::getENC_POS_DEV() { return to_user_pos(readInt(TMC4361A_ENC_POS_DEV_RD)); } void TMC4361A::enableIC(bool enable) { SET_PIN(m_ennPin, !enable); SET_PIN(m_driverIC_ennPin, !enable); @@ -266,15 +266,19 @@ int32_t tmc4361A_discardVelocityDecimals(int32_t value) { return value << 8; } void TMC4361A::rotate(int32_t velocity) { - velocity *= m_scale; + // velocity *= m_scale; + velocity = to_motor_vel(velocity); m_motor_mode = kvelmode; PRV_FIELD_WRITE(TMC4361A_RAMPMODE, TMC4361A_OPERATION_MODE_MASK, TMC4361A_OPERATION_MODE_SHIFT, 0); writeInt(TMC4361A_VMAX, tmc4361A_discardVelocityDecimals(velocity)); } void TMC4361A::stop() { rotate(0); } void TMC4361A::moveTo(int32_t position, uint32_t velocityMax) { - position *= m_scale; - velocityMax *= m_scale; + // position *= m_scale; + // velocityMax *= m_scale; + + position = to_motor_pos(position); + velocityMax = to_motor_vel(velocityMax); m_motor_mode = kposmode; PRV_FIELD_WRITE(TMC4361A_RAMPMODE, TMC4361A_OPERATION_MODE_MASK, TMC4361A_OPERATION_MODE_SHIFT, 1); @@ -387,6 +391,28 @@ void TMC4361A::setRightVirtualLimitSwitch(bool enable, int32_t position) { writeInt(TMC4361A_VIRT_STOP_RIGHT, position); } +int32_t TMC4361A::to_motor_acc(int32_t acc) { // + int32_t val = acc / 60.0 * 51200; + return val; +} +int32_t TMC4361A::to_motor_vel(int32_t vel) { // + int32_t val = vel / 60.0 * 51200; + return val; +} // rpm +int32_t TMC4361A::to_motor_pos(int32_t pos) { // + int32_t val = pos * 1.0 / m_scale * 51200.0; + return val; +} // + +int32_t TMC4361A::to_user_pos(int32_t pos) { // + int32_t val = pos / 51200.0 * m_scale; + return 0; +} // +int32_t TMC4361A::to_user_vel(int32_t vel) { // + int32_t val = vel * 60.0 / 51200.0; + return 0; +} + #endif #endif diff --git a/components/tmc/ic/ztmc4361A.hpp b/components/tmc/ic/ztmc4361A.hpp index c280371..97dc2fd 100644 --- a/components/tmc/ic/ztmc4361A.hpp +++ b/components/tmc/ic/ztmc4361A.hpp @@ -74,7 +74,7 @@ class TMC4361A : public IStepperMotor { motor_mode_t m_motor_mode = kvelmode; - float m_scale = 1.0; + int32_t m_scale = 10000; // 10000一圈 public: TMC4361A(/* args */); @@ -116,7 +116,7 @@ class TMC4361A : public IStepperMotor { virtual void setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay) { driverIC_setIHOLD_IRUN(ihold, irun, iholddelay); } virtual void setMotorShaft(bool reverse) { driverIC_setMotorShaft(reverse); } - virtual void setScale(float scale); + virtual void setScale(int32_t scale); // Left Virtual Limit Switch XACTUAL ≤ VIRT_STOP_LEFT 时触发 void setLeftVirtualLimitSwitch(bool enable, int32_t position); @@ -172,6 +172,14 @@ class TMC4361A : public IStepperMotor { int32_t driverIC_readInt(uint8_t address); void driverIC_setMotorShaft(bool reverse); uint32_t driverIC_readICVersion(); + + private: + int32_t to_motor_acc(int32_t acc); // rpm/s^2 + int32_t to_motor_vel(int32_t vel); // rpm + int32_t to_motor_pos(int32_t pos); // + + int32_t to_user_pos(int32_t pos); // + int32_t to_user_vel(int32_t vel); }; } // namespace iflytop diff --git a/components/tmc/ic/ztmc5130.cpp b/components/tmc/ic/ztmc5130.cpp index ff4541d..3c5d3d4 100644 --- a/components/tmc/ic/ztmc5130.cpp +++ b/components/tmc/ic/ztmc5130.cpp @@ -70,8 +70,8 @@ void TMC5130::initialize(cfg_t *cfg) { writeInt(TMC5130_VSTOP, 10); writeInt(TMC5130_TZEROWAIT, 1000); - setAcceleration(100000); - setDeceleration(100000); + setAcceleration(100); + setDeceleration(100); setIHOLD_IRUN(2, 10, 1); enableIC(true); } @@ -97,11 +97,11 @@ uint8_t TMC5130::reset() { } return 0; } -int32_t TMC5130::getXACTUAL() { return readInt(TMC5130_XACTUAL) / m_scale; } -void TMC5130::setXACTUAL(int32_t value) { writeInt(TMC5130_XACTUAL, value * m_scale); } -int32_t TMC5130::getVACTUAL() { return readInt(TMC5130_VACTUAL) / m_scale; } -void TMC5130::setAcceleration(float accelerationpps2) { writeInt(TMC5130_AMAX, (int32_t)(accelerationpps2 * m_scale)); } // 设置最大加速度 -void TMC5130::setDeceleration(float accelerationpps2) { writeInt(TMC5130_DMAX, (int32_t)(accelerationpps2 * m_scale)); } // 设置最大减速度 +int32_t TMC5130::getXACTUAL() { return to_user_pos(readInt(TMC5130_XACTUAL)); } +void TMC5130::setXACTUAL(int32_t value) { writeInt(TMC5130_XACTUAL, to_motor_pos(value)); } +int32_t TMC5130::getVACTUAL() { return to_user_pos(readInt(TMC5130_VACTUAL)); } +void TMC5130::setAcceleration(float accelerationpps2) { writeInt(TMC5130_AMAX, (int32_t)(to_motor_acc(accelerationpps2))); } // 设置最大加速度 +void TMC5130::setDeceleration(float accelerationpps2) { writeInt(TMC5130_DMAX, (int32_t)(to_motor_acc(accelerationpps2))); } // 设置最大减速度 void TMC5130::setMotorShaft(bool reverse) { PRV_FIELD_WRITE(TMC5130_GCONF, TMC5130_SHAFT_MASK, TMC5130_SHAFT_SHIFT, reverse); } void TMC5130::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)); } uint32_t TMC5130::readICVersion() { @@ -115,15 +115,18 @@ Tmc5130RampStat TMC5130::getTMC5130_RAMPSTAT2() { } void TMC5130::stop() { rotate(0); } void TMC5130::rotate(int32_t velocity) { - velocity *= m_scale; + // velocity *= m_scale; + to_motor_vel(velocity); writeInt(TMC5130_VMAX, abs(velocity)); writeInt(TMC5130_RAMPMODE, (velocity >= 0) ? TMC5130_MODE_VELPOS : TMC5130_MODE_VELNEG); } void TMC5130::right(int32_t velocity) { rotate(velocity); } void TMC5130::left(int32_t velocity) { rotate(-velocity); } void TMC5130::moveTo(int32_t position, uint32_t velocityMax) { - position *= m_scale; - velocityMax *= m_scale; + // position *= m_scale; + // velocityMax *= m_scale; + to_motor_pos(position); + to_motor_vel(velocityMax); writeInt(TMC5130_RAMPMODE, TMC5130_MODE_POSITION); writeInt(TMC5130_VMAX, velocityMax); writeInt(TMC5130_XTARGET, position); @@ -205,4 +208,26 @@ int32_t TMC5130::readInt(uint8_t address) { return ((uint32_t)data[1] << 24) | ((uint32_t)data[2] << 16) | (data[3] << 8) | data[4]; } -#endif \ No newline at end of file +#endif + +int32_t TMC5130::to_motor_acc(int32_t acc) { // + int32_t val = acc / 60.0 * 51200; + return val; +} +int32_t TMC5130::to_motor_vel(int32_t vel) { // + int32_t val = vel / 60.0 * 51200; + return val; +} // rpm +int32_t TMC5130::to_motor_pos(int32_t pos) { // + int32_t val = pos * 1.0 / m_scale * 51200.0; + return val; +} // + +int32_t TMC5130::to_user_pos(int32_t pos) { // + int32_t val = pos / 51200.0 * m_scale; + return 0; +} // +int32_t TMC5130::to_user_vel(int32_t vel) { // + int32_t val = vel * 60.0 / 51200.0; + return 0; +} \ No newline at end of file diff --git a/components/tmc/ic/ztmc5130.hpp b/components/tmc/ic/ztmc5130.hpp index a4b0df1..1770d57 100644 --- a/components/tmc/ic/ztmc5130.hpp +++ b/components/tmc/ic/ztmc5130.hpp @@ -76,10 +76,10 @@ class TMC5130 : public IStepperMotor { cfg_t m_cfg; - ZGPIO *m_csnpin = NULL; - ZGPIO *m_ennpin = NULL; + ZGPIO *m_csnpin = NULL; + ZGPIO *m_ennpin = NULL; ZGPIO *m_spi_mode_select_gpio = NULL; - SPI_HandleTypeDef *m_hspi = NULL; + SPI_HandleTypeDef *m_hspi = NULL; // uint8_t m_channel; uint32_t m_lastCallPeriodicJobTick; @@ -149,6 +149,13 @@ class TMC5130 : public IStepperMotor { private: uint32_t haspassedms(uint32_t now, uint32_t last); + + int32_t to_motor_acc(int32_t acc); // rpm/s^2 + int32_t to_motor_vel(int32_t vel); // rpm + int32_t to_motor_pos(int32_t pos); // + + int32_t to_user_pos(int32_t pos); // + int32_t to_user_vel(int32_t vel); }; } // namespace iflytop #endif \ No newline at end of file