From a87b981a3df20078510c8da415a3817b1bc74f67 Mon Sep 17 00:00:00 2001 From: Alex Voinea Date: Wed, 13 Oct 2021 15:11:30 +0200 Subject: [PATCH] Homing initial --- src/config/axis.h | 1 + src/config/config.h | 9 ++++++--- src/hal/tmc2130.cpp | 21 ++++++++++----------- src/hal/tmc2130.h | 10 +++++++--- src/main.cpp | 3 +++ src/modules/idler.cpp | 4 ++++ src/modules/idler.h | 1 + src/modules/motion.cpp | 2 +- src/modules/motion.h | 6 +++--- src/modules/movable_base.cpp | 3 ++- src/modules/movable_base.h | 5 +++-- src/modules/selector.cpp | 4 ++++ src/modules/selector.h | 1 + 13 files changed, 46 insertions(+), 24 deletions(-) diff --git a/src/config/axis.h b/src/config/axis.h index 9469b7a..7df69b0 100644 --- a/src/config/axis.h +++ b/src/config/axis.h @@ -29,6 +29,7 @@ struct AxisConfig { uint8_t iHold; ///< holding current bool stealth; ///< Default to Stealth mode long double stepsPerUnit; ///< steps per unit + int8_t sg_thrs; /// @todo 7bit two's complement for the sg_thrs }; /// List of available axes diff --git a/src/config/config.h b/src/config/config.h index 0c31c47..06a440d 100644 --- a/src/config/config.h +++ b/src/config/config.h @@ -99,6 +99,7 @@ static constexpr AxisConfig pulley = { .iHold = 0, /// 17mA in SpreadCycle, freewheel in StealthChop .stealth = false, .stepsPerUnit = (200 * 2 / 19.147274), + .sg_thrs = 3, }; /// Pulley motion limits @@ -120,6 +121,7 @@ static constexpr AxisConfig selector = { .iHold = 5, /// 99mA .stealth = false, .stepsPerUnit = (200 * 8 / 8.), + .sg_thrs = 3, }; /// Selector motion limits @@ -168,6 +170,7 @@ static constexpr AxisConfig idler = { .iHold = 23, /// 398mA .stealth = false, .stepsPerUnit = (200 * 16 / 360.), + .sg_thrs = 8, }; /// Idler motion limits @@ -198,10 +201,10 @@ static constexpr U_deg_s idlerFeedrate = 200._deg_s; // TMC2130 setup -static constexpr int8_t tmc2130_sg_thrs = 3; // @todo 7bit two's complement for the sg_thrs -static_assert(tmc2130_sg_thrs >= -64 && tmc2130_sg_thrs <= 63, "tmc2130_sg_thrs out of range"); +// static constexpr int8_t tmc2130_sg_thrs = 3; +// static_assert(tmc2130_sg_thrs >= -64 && tmc2130_sg_thrs <= 63, "tmc2130_sg_thrs out of range"); -static constexpr uint32_t tmc2130_coolStepThreshold = 400; ///< step-based 20bit uint +static constexpr uint32_t tmc2130_coolStepThreshold = 5000; ///< step-based 20bit uint static_assert(tmc2130_coolStepThreshold <= 0xfffff, "tmc2130_coolStepThreshold out of range"); static constexpr uint32_t tmc2130_PWM_AMPL = 240; diff --git a/src/hal/tmc2130.cpp b/src/hal/tmc2130.cpp index 7be2765..e11c741 100644 --- a/src/hal/tmc2130.cpp +++ b/src/hal/tmc2130.cpp @@ -2,10 +2,14 @@ #include "tmc2130.h" #include "../config/config.h" +#include "debug.h" + namespace hal { namespace tmc2130 { bool TMC2130::Init(const MotorParams ¶ms, const MotorCurrents ¤ts, MotorMode mode) { + sg_filter_threshold = (1 << (8 - params.mRes)) - 1; + gpio::Init(params.csPin, gpio::GPIO_InitTypeDef(gpio::Mode::output, gpio::Level::high)); gpio::Init(params.sgPin, gpio::GPIO_InitTypeDef(gpio::Mode::input, gpio::Pull::up)); gpio::Init(params.stepPin, gpio::GPIO_InitTypeDef(gpio::Mode::output, gpio::Level::low)); @@ -39,7 +43,7 @@ bool TMC2130::Init(const MotorParams ¶ms, const MotorCurrents ¤ts, Mot WriteRegister(params, Registers::TPOWERDOWN, 0); ///Stallguard parameters - static constexpr uint32_t tmc2130_coolConf = (((uint32_t)config::tmc2130_sg_thrs) << 16U); + uint32_t tmc2130_coolConf = (((uint32_t)params.sg_thrs) << 16U); WriteRegister(params, Registers::COOLCONF, tmc2130_coolConf); WriteRegister(params, Registers::TCOOLTHRS, config::tmc2130_coolStepThreshold); @@ -78,15 +82,10 @@ void TMC2130::SetCurrents(const MotorParams ¶ms, const MotorCurrents ¤ void TMC2130::SetEnabled(const MotorParams ¶ms, bool enabled) { hal::shr16::shr16.SetTMCEnabled(params.idx, enabled); if (this->enabled != enabled) - ClearStallguard(params); + ClearStallguard(); this->enabled = enabled; } -void TMC2130::ClearStallguard(const MotorParams ¶ms) { - // @@TODO maximum resolution right now is x256/4 (uint8_t / 4) - sg_counter = 4 * (1 << (8 - params.mRes)) - 1; /// one electrical full step (4 steps when fullstepping) -} - bool TMC2130::CheckForErrors(const MotorParams ¶ms) { uint32_t GSTAT = ReadRegister(params, Registers::GSTAT); uint32_t DRV_STATUS = ReadRegister(params, Registers::DRV_STATUS); @@ -115,11 +114,11 @@ void TMC2130::WriteRegister(const MotorParams ¶ms, Registers reg, uint32_t d } void TMC2130::Isr(const MotorParams ¶ms) { - if (sg_counter) { + if (sg_filter_counter) { if (SampleDiag(params)) - sg_counter--; - else if (sg_counter < (4 * (1 << (8 - params.mRes)) - 1)) - sg_counter++; + sg_filter_counter--; + else if (sg_filter_counter < sg_filter_threshold) + sg_filter_counter++; } } diff --git a/src/hal/tmc2130.h b/src/hal/tmc2130.h index 3c4e17e..2bd73e8 100644 --- a/src/hal/tmc2130.h +++ b/src/hal/tmc2130.h @@ -25,6 +25,7 @@ struct MotorParams { gpio::GPIO_pin stepPin; ///< step pin gpio::GPIO_pin sgPin; ///< stallguard pin config::MRes mRes; ///< microstep resolution + int8_t sg_thrs; }; struct MotorCurrents { @@ -114,10 +115,12 @@ public: } inline bool Stalled() const { - return sg_counter == 0; + return sg_filter_counter == 0; } - void ClearStallguard(const MotorParams ¶ms); + inline void ClearStallguard() { + sg_filter_counter = sg_filter_threshold; + } /// Should be called periodically from main loop. Maybe not all the time. Once every 10 ms is probably enough bool CheckForErrors(const MotorParams ¶ms); @@ -149,7 +152,8 @@ private: ErrorFlags errorFlags; bool enabled = false; - uint8_t sg_counter = 0; + uint8_t sg_filter_threshold; + uint8_t sg_filter_counter = 0; }; } // namespace tmc2130 diff --git a/src/main.cpp b/src/main.cpp index 63f0244..160dbac 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -145,6 +145,9 @@ void setup() { mu::cdc.Init(); + ms::selector.Home(); + mi::idler.Home(); + _delay_ms(100); /// Turn off all leds diff --git a/src/modules/idler.cpp b/src/modules/idler.cpp index 747c4ff..1bc0866 100644 --- a/src/modules/idler.cpp +++ b/src/modules/idler.cpp @@ -21,6 +21,10 @@ void Idler::PlanHomingMove() { dbg_logic_P(PSTR("Plan Homing Idler")); } +void Idler::FinishHoming() { + mm::motion.SetPosition(mm::Idler, 0); +} + Idler::OperationResult Idler::Disengage() { if (state == Moving) { dbg_logic_P(PSTR("Moving --> Disengage refused")); diff --git a/src/modules/idler.h b/src/modules/idler.h index b903ee8..a5d17e3 100644 --- a/src/modules/idler.h +++ b/src/modules/idler.h @@ -52,6 +52,7 @@ public: protected: virtual void PrepareMoveToPlannedSlot() override; virtual void PlanHomingMove() override; + virtual void FinishHoming() override; private: /// direction of travel - engage/disengage diff --git a/src/modules/motion.cpp b/src/modules/motion.cpp index 51fbb0e..0fe0fa7 100644 --- a/src/modules/motion.cpp +++ b/src/modules/motion.cpp @@ -43,7 +43,7 @@ bool Motion::StallGuard(Axis axis) { } void Motion::StallGuardReset(Axis axis) { - axisData[axis].drv.ClearStallguard(axisParams[axis].params); + axisData[axis].drv.ClearStallguard(); } void Motion::PlanMoveTo(Axis axis, pos_t pos, steps_t feed_rate, steps_t end_rate) { diff --git a/src/modules/motion.h b/src/modules/motion.h index dabcf5d..9f3e050 100644 --- a/src/modules/motion.h +++ b/src/modules/motion.h @@ -42,7 +42,7 @@ static AxisParams axisParams[NUM_AXIS] = { // Pulley { .name = 'P', - .params = { .spi = hal::spi::TmcSpiBus, .idx = Pulley, .dirOn = config::pulley.dirOn, .csPin = PULLEY_CS_PIN, .stepPin = PULLEY_STEP_PIN, .sgPin = PULLEY_SG_PIN, .mRes = config::pulley.mRes }, + .params = { .spi = hal::spi::TmcSpiBus, .idx = Pulley, .dirOn = config::pulley.dirOn, .csPin = PULLEY_CS_PIN, .stepPin = PULLEY_STEP_PIN, .sgPin = PULLEY_SG_PIN, .mRes = config::pulley.mRes, .sg_thrs = config::pulley.sg_thrs }, .currents = { .vSense = config::pulley.vSense, .iRun = config::pulley.iRun, .iHold = config::pulley.iHold }, .mode = DefaultMotorMode(config::pulley), .jerk = unitToSteps(config::pulleyLimits.jerk), @@ -51,7 +51,7 @@ static AxisParams axisParams[NUM_AXIS] = { // Selector { .name = 'S', - .params = { .spi = hal::spi::TmcSpiBus, .idx = Selector, .dirOn = config::selector.dirOn, .csPin = SELECTOR_CS_PIN, .stepPin = SELECTOR_STEP_PIN, .sgPin = SELECTOR_SG_PIN, .mRes = config::selector.mRes }, + .params = { .spi = hal::spi::TmcSpiBus, .idx = Selector, .dirOn = config::selector.dirOn, .csPin = SELECTOR_CS_PIN, .stepPin = SELECTOR_STEP_PIN, .sgPin = SELECTOR_SG_PIN, .mRes = config::selector.mRes, .sg_thrs = config::selector.sg_thrs }, .currents = { .vSense = config::selector.vSense, .iRun = config::selector.iRun, .iHold = config::selector.iHold }, .mode = DefaultMotorMode(config::selector), .jerk = unitToSteps(config::selectorLimits.jerk), @@ -60,7 +60,7 @@ static AxisParams axisParams[NUM_AXIS] = { // Idler { .name = 'I', - .params = { .spi = hal::spi::TmcSpiBus, .idx = Idler, .dirOn = config::idler.dirOn, .csPin = IDLER_CS_PIN, .stepPin = IDLER_STEP_PIN, .sgPin = IDLER_SG_PIN, .mRes = config::idler.mRes }, + .params = { .spi = hal::spi::TmcSpiBus, .idx = Idler, .dirOn = config::idler.dirOn, .csPin = IDLER_CS_PIN, .stepPin = IDLER_STEP_PIN, .sgPin = IDLER_SG_PIN, .mRes = config::idler.mRes, .sg_thrs = config::idler.sg_thrs }, .currents = { .vSense = config::idler.vSense, .iRun = config::idler.iRun, .iHold = config::idler.iHold }, .mode = DefaultMotorMode(config::idler), .jerk = unitToSteps(config::idlerLimits.jerk), diff --git a/src/modules/movable_base.cpp b/src/modules/movable_base.cpp index b7a616f..139ad1d 100644 --- a/src/modules/movable_base.cpp +++ b/src/modules/movable_base.cpp @@ -8,13 +8,13 @@ namespace motion { void MovableBase::PlanHome(config::Axis axis) { // switch to normal mode on this axis - state = Homing; mm::motion.InitAxis(axis); mm::motion.SetMode(axis, mm::Normal); mm::motion.StallGuardReset(axis); // plan move at least as long as the axis can go from one side to the other PlanHomingMove(); // mm::motion.PlanMove(axis, delta, 1000); + state = Homing; } MovableBase::OperationResult MovableBase::InitMovement(config::Axis axis) { @@ -44,6 +44,7 @@ void MovableBase::PerformHome(config::Axis axis) { // we have reached the end of the axis - homed ok mm::motion.AbortPlannedMoves(axis, true); mm::motion.SetMode(axis, mg::globals.MotorsStealth() ? mm::Stealth : mm::Normal); + FinishHoming(); state = Ready; } else if (mm::motion.QueueEmpty(axis)) { // we ran out of planned moves but no StallGuard event has occurred - homing failed diff --git a/src/modules/movable_base.h b/src/modules/movable_base.h index d196357..2619634 100644 --- a/src/modules/movable_base.h +++ b/src/modules/movable_base.h @@ -27,8 +27,8 @@ public: inline constexpr MovableBase() : state(Ready) - , plannedSlot(0) - , currentSlot(0) {} + , plannedSlot(-1) + , currentSlot(-1) {} /// virtual ~MovableBase(); intentionally disabled, see description in logic::CommandBase @@ -60,6 +60,7 @@ protected: virtual void PrepareMoveToPlannedSlot() = 0; virtual void PlanHomingMove() = 0; + virtual void FinishHoming() = 0; OperationResult InitMovement(config::Axis axis); diff --git a/src/modules/selector.cpp b/src/modules/selector.cpp index e127d8c..f874605 100644 --- a/src/modules/selector.cpp +++ b/src/modules/selector.cpp @@ -21,6 +21,10 @@ void Selector::PlanHomingMove() { dbg_logic_P(PSTR("Plan Homing Selector")); } +void Selector::FinishHoming() { + mm::motion.SetPosition(mm::Selector, mm::unitToSteps(config::selectorLimits.lenght)); +} + Selector::OperationResult Selector::MoveToSlot(uint8_t slot) { if (state == Moving) { dbg_logic_P(PSTR("Moving --> Selector refused")); diff --git a/src/modules/selector.h b/src/modules/selector.h index cb95d98..2b741b1 100644 --- a/src/modules/selector.h +++ b/src/modules/selector.h @@ -41,6 +41,7 @@ public: protected: virtual void PrepareMoveToPlannedSlot() override; virtual void PlanHomingMove() override; + virtual void FinishHoming() override; private: };