diff --git a/src/application.cpp b/src/application.cpp index d80e07f..23e6b99 100644 --- a/src/application.cpp +++ b/src/application.cpp @@ -13,6 +13,7 @@ #include "logic/cut_filament.h" #include "logic/eject_filament.h" #include "logic/home.h" +#include "logic/hw_sanity.h" #include "logic/load_filament.h" #include "logic/move_selector.h" #include "logic/no_command.h" diff --git a/src/hal/tmc2130.cpp b/src/hal/tmc2130.cpp index eda42d1..597b512 100644 --- a/src/hal/tmc2130.cpp +++ b/src/hal/tmc2130.cpp @@ -6,6 +6,8 @@ namespace hal { namespace tmc2130 { +static constexpr uint8_t TOFF_DEFAULT = 3U, TOFF_MASK = 0xFU; + bool TMC2130::Init(const MotorParams ¶ms, const MotorCurrents ¤ts, MotorMode mode) { // sg_filter_threshold = (1 << (8 - params.mRes)); sg_filter_threshold = 2; @@ -26,7 +28,7 @@ bool TMC2130::Init(const MotorParams ¶ms, const MotorCurrents ¤ts, Mot errorFlags.reset_flag = false; ///apply chopper parameters - const uint32_t chopconf = (uint32_t)(3U & 0x0FU) << 0U //toff + const uint32_t chopconf = (uint32_t)(TOFF_DEFAULT & TOFF_MASK) << 0U //toff | (uint32_t)(5U & 0x07U) << 4U //hstrt | (uint32_t)(1U & 0x0FU) << 7U //hend | (uint32_t)(2U & 0x03U) << 15U //tbl @@ -71,6 +73,15 @@ void TMC2130::SetMode(const MotorParams ¶ms, MotorMode mode) { WriteRegister(params, Registers::TPWMTHRS, (mode == Stealth) ? 70 : 0xFFFF0); // @@TODO should be configurable } +void TMC2130::SetBridgeOutput(const MotorParams ¶ms, bool bOn) { + uint32_t chopconf = ReadRegister(params, Registers::CHOPCONF); + chopconf &= ~((uint32_t)TOFF_MASK); + if (bOn) { + chopconf |= TOFF_DEFAULT; + } + WriteRegister(params, Registers::CHOPCONF, chopconf); +} + void TMC2130::SetCurrents(const MotorParams ¶ms, const MotorCurrents ¤ts) { uint32_t ihold_irun = (uint32_t)(currents.iHold & 0x1F) << 0 //ihold | (uint32_t)(currents.iRun & 0x1F) << 8 //irun diff --git a/src/hal/tmc2130.h b/src/hal/tmc2130.h index 30b7f25..7624e57 100644 --- a/src/hal/tmc2130.h +++ b/src/hal/tmc2130.h @@ -89,6 +89,9 @@ public: /// Set the current motor mode void SetMode(const MotorParams ¶ms, MotorMode mode); + /// Disables the output by setting or clearing CHOPCONF's TOFF. + void SetBridgeOutput(const MotorParams ¶ms, bool on); + /// Set the current motor currents void SetCurrents(const MotorParams ¶ms, const MotorCurrents ¤ts); @@ -111,11 +114,22 @@ public: hal::shr16::shr16.SetTMCDir(params.idx, dir ^ params.dirOn); } + /// Set direction, raw value (i.e. ignore Params). + static inline void SetRawDir(const MotorParams ¶ms, bool dir) { + // Also cancels the inversion in SetTMCDir + hal::shr16::shr16.SetTMCDir(params.idx, !dir); + } + /// Step the motor static inline void Step(const MotorParams ¶ms) { gpio::TogglePin(params.stepPin); // assumes DEDGE } + /// Set step to an explicit state + static inline void SetStep(const MotorParams ¶ms, bool state) { + gpio::WritePin(params.stepPin, (state ? gpio::Level::high : gpio::Level::low)); + } + /// Return SG state static inline bool SampleDiag(const MotorParams ¶ms) { return gpio::ReadPin(params.sgPin) == gpio::Level::low; diff --git a/src/logic/CMakeLists.txt b/src/logic/CMakeLists.txt index 295dcb0..215dc63 100644 --- a/src/logic/CMakeLists.txt +++ b/src/logic/CMakeLists.txt @@ -6,6 +6,7 @@ target_sources( feed_to_bondtech.cpp feed_to_finda.cpp home.cpp + hw_sanity.cpp load_filament.cpp move_selector.cpp no_command.cpp diff --git a/src/logic/error_codes.h b/src/logic/error_codes.h index f47b105..34abaca 100644 --- a/src/logic/error_codes.h +++ b/src/logic/error_codes.h @@ -100,5 +100,12 @@ enum class ErrorCode : uint_fast16_t { /// - E49280 Selector TMC driver /// - E49408 Idler TMC driver /// - E49600 All 3 TMC driver - TMC_OVER_TEMPERATURE_ERROR = 0xC000 + TMC_OVER_TEMPERATURE_ERROR = 0xC000, + + /// TMC driver - IO pins are unreliable. While in theory it's recoverable, in practice it most likely + /// means your hardware is borked (we can't command the drivers reliably via STEP/EN/DIR due to electrical + /// issues or hardware fault. Possible "fixable" cause is undervoltage on the 5v logic line. + /// Unfixable possible cause: bad or cracked solder joints on the PCB, failed shift register, failed driver. + MMU_SOLDERING_NEEDS_ATTENTION = 0xC200, + }; diff --git a/src/logic/hw_sanity.cpp b/src/logic/hw_sanity.cpp new file mode 100644 index 0000000..f39ad79 --- /dev/null +++ b/src/logic/hw_sanity.cpp @@ -0,0 +1,175 @@ +/// @file hw_sanity.cpp +#include "hw_sanity.h" +#include "../modules/globals.h" +#include "../modules/motion.h" +#include "../modules/leds.h" +#include "../modules/timebase.h" + +namespace logic { + +// Copy-pasta from command_base, they're inline +// so this shouldn't affect code size. +inline ErrorCode &operator|=(ErrorCode &a, ErrorCode b) { + return a = (ErrorCode)((uint16_t)a | (uint16_t)b); +} + +using Axis = config::Axis; +using TMC2130 = hal::tmc2130::TMC2130; + +static constexpr uint8_t LED_WAIT_MS = 50U; +static constexpr uint8_t TEST_PASSES = 3U; +static_assert(TEST_PASSES < 32); // Would overflow counters + +HWSanity hwSanity; + +bool HWSanity::Reset(uint8_t param) { + state = ProgressCode::HWTestBegin; + error = ErrorCode::RUNNING; + axis = config::Axis::Idler; + fault_masks[0] = 0; + fault_masks[1] = 0; + fault_masks[2] = 0; + return true; +} + +enum pin_bits { + BIT_STEP = 0b001, + BIT_DIR = 0b010, + BIT_ENA = 0b100, +}; + +void HWSanity::SetFaultDisplay(uint8_t slot, uint8_t mask) { + ml::Mode red_mode = ml::off, green_mode = ml::off; + if (mask & BIT_STEP) { + green_mode = ml::on; + } + if (mask & BIT_DIR) { + red_mode = ml::on; + } + if (mask & BIT_ENA) { + green_mode = green_mode ? ml::blink0 : ml::on; + red_mode = red_mode ? ml::blink0 : ml::on; + } + ml::leds.SetMode(slot, ml::green, green_mode); + ml::leds.SetMode(slot, ml::red, red_mode); +} + +void HWSanity::PrepareAxis(config::Axis axis) { + mm::motion.InitAxis(axis); + mm::motion.SetMode(axis, mm::Normal); + // Clear TOFF so the motors don't actually step during the test. + mm::motion.MMU_NEEDS_ATTENTION_DriverForAxis(axis).SetBridgeOutput(mm::axisParams[axis].params, false); +} + +bool HWSanity::StepInner() { + switch (state) { + case ProgressCode::HWTestBegin: + error = ErrorCode::RUNNING; + test_step = 0; + state = ProgressCode::HWTestIdler; + break; + case ProgressCode::HWTestIdler: + axis = config::Axis::Idler; + ml::leds.SetPairButOffOthers(3, ml::on, ml::off); + state = ProgressCode::HWTestExec; + next_state = ProgressCode::HWTestSelector; + PrepareAxis(axis); + break; + case ProgressCode::HWTestSelector: + axis = config::Axis::Selector; + ml::leds.SetPairButOffOthers(3, ml::off, ml::on); + state = ProgressCode::HWTestExec; + next_state = ProgressCode::HWTestPulley; + PrepareAxis(axis); + break; + case ProgressCode::HWTestPulley: + axis = config::Axis::Pulley; + ml::leds.SetPairButOffOthers(3, ml::on, ml::on); + state = ProgressCode::HWTestExec; + next_state = ProgressCode::HWTestCleanup; + PrepareAxis(axis); + break; + // The main test loop for a given axis. + case ProgressCode::HWTestDisplay: + // Hold for a few ms while we display the last step result. + if (!mt::timebase.Elapsed(wait_start, LED_WAIT_MS)) { + break; + } else { + state = ProgressCode::HWTestExec; + // display done, reset LEDs. + for (uint8_t i = 0; i < 6; i++) { + ml::leds.SetMode(i, ml::off); + } + } + [[fallthrough]]; + case ProgressCode::HWTestExec: { + auto params = mm::axisParams[axis].params; + auto &driver = mm::motion.MMU_NEEDS_ATTENTION_DriverForAxis(axis); + if (test_step < (TEST_PASSES * 8)) // 8 combos per axis + { + uint8_t set_state = test_step % 8; + // The order of the bits here is roughly the same as that of IOIN. + driver.SetRawDir(params, set_state & BIT_DIR); + driver.SetStep(params, set_state & BIT_STEP); + driver.SetEnabled(params, set_state & BIT_ENA); + uint32_t drv_ioin = driver.ReadRegister(params, hal::tmc2130::TMC2130::Registers::IOIN); + // Compose IOIN to look like set_state. + drv_ioin = (drv_ioin & 0b11) | ((drv_ioin & 0b10000) ? 0 : 4); // Note the logic inversion for ENA readback! + uint8_t bit_errs = (drv_ioin ^ set_state); + // Set the LEDs. Note RED is index 0 in the enum, so we want the expression FALSE if there's an error. + ml::leds.SetMode(0, static_cast((bit_errs & BIT_STEP) == 0), ml::on); + ml::leds.SetMode(1, static_cast((bit_errs & BIT_DIR) == 0), ml::on); + ml::leds.SetMode(2, static_cast((bit_errs & BIT_ENA) == 0), ml::on); + // Capture the error for later. + fault_masks[axis] |= bit_errs; + // Enter the wait state: + wait_start = mt::timebase.Millis(); + das_blinken_state = das_blinken_state ? ml::off : ml::on; + ml::leds.SetMode(4, ml::green, das_blinken_state); + state = ProgressCode::HWTestDisplay; + // Next iteration. + test_step++; + } else { + // This pass is complete. Move on to the next motor or cleanup. + driver.SetBridgeOutput(params, true); + test_step = 0; + state = next_state; + } + } break; + case ProgressCode::HWTestCleanup: + if (fault_masks[0] || fault_masks[1] || fault_masks[2]) { + // error, display it and return the code. + state = ProgressCode::ErrHwTestFailed; + error = ErrorCode::MMU_SOLDERING_NEEDS_ATTENTION; + uint8_t mask = fault_masks[Axis::Idler]; + if (mask) { + SetFaultDisplay(0, mask); + } + mask = fault_masks[Axis::Pulley]; + if (mask) { + SetFaultDisplay(2, mask); + } + mask = fault_masks[Axis::Selector]; + if (mask) { + SetFaultDisplay(1, mask); + } + ml::leds.SetMode(3, ml::red, ml::off); + ml::leds.SetMode(3, ml::green, ml::off); + ml::leds.SetMode(4, ml::red, ml::on); + ml::leds.SetMode(4, ml::green, ml::off); + return true; + } else { + ml::leds.SetPairButOffOthers(0, ml::off, ml::off); + FinishedOK(); + } + case ProgressCode::OK: + return true; + default: // we got into an unhandled state, better report it + state = ProgressCode::ERRInternal; + error = ErrorCode::INTERNAL; + return true; + } + return false; +} + +} // namespace logic diff --git a/src/logic/hw_sanity.h b/src/logic/hw_sanity.h new file mode 100644 index 0000000..a97a71e --- /dev/null +++ b/src/logic/hw_sanity.h @@ -0,0 +1,57 @@ +/// @file hw_sanity.h +#pragma once +#include +#include "command_base.h" +#include "../config/axis.h" +#include "../modules/leds.h" + +namespace logic { + +/// @brief Performs a sanity check of the hardware at reset/boot. Checks the following: +/// - TMC drivers using their IOIN registers (DIR/STEP/DRV_EN) +/// - ... +/// - Profit! + +class HWSanity : public CommandBase { +public: + inline HWSanity() + : CommandBase() {} + + /// Restart the automaton + bool Reset(uint8_t param) override; + + /// @returns true if the state machine finished its job, false otherwise. + /// LED indicators during the test execution: + /// Slots 1-3: Pin states for STEP, DIR, and ENA - G: Set value matches readback, R: Readback disagrees. + /// Slot 4: Axis under test - G: Idler, R: Selector, RG: Pully. + /// Slot 5: G: Blinking to indicate test progression. R: Solid to indicate completed test w/ fault. + /// Indicators at test end (fault condition): + /// Slots 1-3 now indicate pins for Idler/Selector/Pulley, respectively: + /// - Off: No faults detected. + /// - G: STEP fault + /// - R: DIR fault + /// - RG: EN fault. + /// - Blinking R/G: Multiple fault, e.g both an EN fault together with STEP and/or DIR. + /// Slot 4: Reserved + /// Slot 5: R: Solid + bool StepInner() override; + +private: + // Shared code fault display setup for each axis/slot + static void SetFaultDisplay(uint8_t slot, uint8_t mask); + + // Prepares an axis for testing by initializing it and turning off the output. + static void PrepareAxis(config::Axis axis); + + uint8_t test_step = 0; + config::Axis axis; + uint8_t fault_masks[3] = { 0 }; + ProgressCode next_state = ProgressCode::HWTestBegin; + uint16_t wait_start = 0; + ml::Mode das_blinken_state = ml::off; +}; + +/// The one and only instance of hwSanity state machine in the FW +extern HWSanity hwSanity; + +} // namespace logic diff --git a/src/logic/progress_codes.h b/src/logic/progress_codes.h index 7dd2e19..5c6c814 100644 --- a/src/logic/progress_codes.h +++ b/src/logic/progress_codes.h @@ -41,5 +41,14 @@ enum class ProgressCode : uint_fast8_t { FeedingToFSensor, // P28 + HWTestBegin, // P29 + HWTestIdler, // P30 + HWTestSelector, // P31 + HWTestPulley, // P32 + HWTestCleanup, // P33 + HWTestExec, // P34 + HWTestDisplay, // P35 + ErrHwTestFailed, // P36 + Empty = 0xff // dummy empty state }; diff --git a/src/main.cpp b/src/main.cpp index 3039277..af90d79 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -27,6 +27,7 @@ #include "application.h" +#include "logic/hw_sanity.h" #include "logic/no_command.h" /// One-time setup of HW and SW components @@ -112,6 +113,19 @@ static void setup2() { } ml::leds.Step(); + // Prep hardware sanity: + logic::hwSanity.Reset(0); + + while (!logic::hwSanity.StepInner()) { + ml::leds.Step(); + } + + if (logic::hwSanity.Error() != ErrorCode::OK) { + // forward the issue to the logic startup handler. + logic::noCommand.SetInitError(logic::hwSanity.Error()); + return; + } + // Idler and Selector decide whether homing is possible/safe mi::idler.Init(); ms::selector.Init(); @@ -141,14 +155,17 @@ void Panic(ErrorCode ec) { /// The idea behind the Step* routines is to keep each automaton non-blocking allowing for some “concurrency”. /// Some FW components will leverage ISR to do their stuff (UART, motor stepping?, etc.) void loop() { + mb::buttons.Step(); ml::leds.Step(); - mf::finda.Step(); - mfs::fsensor.Step(); - mi::idler.Step(); - ms::selector.Step(); - mpu::pulley.Step(); - mui::userInput.Step(); + if (logic::hwSanity.Error() == ErrorCode::OK) { + mf::finda.Step(); + mfs::fsensor.Step(); + mi::idler.Step(); + ms::selector.Step(); + mpu::pulley.Step(); + mui::userInput.Step(); + } hal::cpu::Step(); mu::cdc.Step(); diff --git a/src/modules/motion.h b/src/modules/motion.h index 6724fed..7a0df15 100644 --- a/src/modules/motion.h +++ b/src/modules/motion.h @@ -344,6 +344,13 @@ public: return axisData[axis].drv; } + /// @returns the (non-const) TMC2130 driver associated with the particular axis. + /// Do not use unless you know exactly what you're doing, (i.e., nothing else can possibly be using + /// the axis. Currently the only valid usage is in the hw sanity module. + inline hal::tmc2130::TMC2130 &MMU_NEEDS_ATTENTION_DriverForAxis(Axis axis) { + return axisData[axis].drv; + } + /// @returns the controller associated with the particular axis inline const pulse_gen::PulseGen &CtrlForAxis(Axis axis) const { return axisData[axis].ctrl;