From c65c1d90d353825c24d64e11a85b16d0cd55cdf7 Mon Sep 17 00:00:00 2001 From: Mitch Bradley Date: Thu, 3 Oct 2024 10:36:35 -1000 Subject: [PATCH] Make a lot of stuff static --- FluidNC/src/GCode.cpp | 4 +- FluidNC/src/Kinematics/Cartesian.cpp | 6 +-- FluidNC/src/Kinematics/CoreXY.cpp | 8 ++-- FluidNC/src/Kinematics/ParallelDelta.cpp | 4 +- FluidNC/src/Kinematics/WallPlotter.cpp | 6 +-- FluidNC/src/Limits.cpp | 10 ++--- FluidNC/src/Machine/Axes.cpp | 25 ++++++++----- FluidNC/src/Machine/Axes.h | 38 +++++++++---------- FluidNC/src/Machine/Homing.cpp | 26 ++++++------- FluidNC/src/Machine/LimitPin.cpp | 10 +++-- FluidNC/src/Machine/MachineConfig.h | 2 +- FluidNC/src/Main.cpp | 4 +- FluidNC/src/MotionControl.cpp | 12 +++--- FluidNC/src/Motors/Dynamixel2.cpp | 4 +- FluidNC/src/Motors/MotorDriver.cpp | 6 ++- FluidNC/src/Motors/RcServo.cpp | 2 +- FluidNC/src/Motors/TrinamicBase.cpp | 4 +- FluidNC/src/Motors/UnipolarMotor.cpp | 10 +++-- FluidNC/src/NutsBolts.cpp | 10 ++--- FluidNC/src/OLED.cpp | 2 +- FluidNC/src/Pin.h | 2 +- FluidNC/src/Pins/ChannelPinDetail.cpp | 14 +++++-- FluidNC/src/Pins/DebugPinDetail.cpp | 4 +- FluidNC/src/Pins/ErrorPinDetail.cpp | 20 +++++++--- FluidNC/src/Planner.cpp | 4 +- FluidNC/src/ProcessSettings.cpp | 14 +++---- FluidNC/src/Protocol.cpp | 18 ++++----- FluidNC/src/Report.cpp | 8 ++-- FluidNC/src/Stepper.cpp | 34 ++++++----------- FluidNC/src/Stepping.cpp | 8 ++++ FluidNC/src/Stepping.h | 47 +++++++++++++----------- FluidNC/src/System.cpp | 6 +-- FluidNC/src/WebUI/WifiConfig.cpp | 8 ++-- 33 files changed, 205 insertions(+), 175 deletions(-) diff --git a/FluidNC/src/GCode.cpp b/FluidNC/src/GCode.cpp index ab64bc093..2fca261d6 100644 --- a/FluidNC/src/GCode.cpp +++ b/FluidNC/src/GCode.cpp @@ -268,7 +268,7 @@ Error gc_execute_line(char* line) { bool nonmodalG38 = false; // Used for G38.6-9 bool isWaitOnInputDigital = false; - auto n_axis = config->_axes->_numberAxis; + auto n_axis = Axes::_numberAxis; float coord_data[MAX_N_AXIS]; // Used by WCO-related commands uint8_t pValue; // Integer value of P word @@ -1602,7 +1602,7 @@ Error gc_execute_line(char* line) { // NOTE: Pass zero spindle speed for all restricted laser motions. if (!disableLaser) { pl_data->spindle_speed = gc_state.spindle_speed; // Record data for planner use. - } // else { pl_data->spindle_speed = 0.0; } // Initialized as zero already. + } // else { pl_data->spindle_speed = 0.0; } // Initialized as zero already. // [5. Select tool ]: NOT SUPPORTED. Only tracks tool value. // gc_state.tool = gc_block.values.t; // [M6. Change tool ]: diff --git a/FluidNC/src/Kinematics/Cartesian.cpp b/FluidNC/src/Kinematics/Cartesian.cpp index 00b1318bd..b318f5dc5 100644 --- a/FluidNC/src/Kinematics/Cartesian.cpp +++ b/FluidNC/src/Kinematics/Cartesian.cpp @@ -12,7 +12,7 @@ namespace Kinematics { // Initialize the machine position void Cartesian::init_position() { - auto n_axis = config->_axes->_numberAxis; + auto n_axis = Axes::_numberAxis; for (size_t axis = 0; axis < n_axis; axis++) { set_motor_steps(axis, 0); // Set to zeros } @@ -178,7 +178,7 @@ namespace Kinematics { void Cartesian::constrain_jog(float* target, plan_line_data_t* pl_data, float* position) { auto axes = config->_axes; - auto n_axis = config->_axes->_numberAxis; + auto n_axis = Axes::_numberAxis; float* current_position = get_mpos(); MotorMask lim_pin_state = limits_get_state(); @@ -237,7 +237,7 @@ namespace Kinematics { bool Cartesian::invalid_line(float* cartesian) { auto axes = config->_axes; - auto n_axis = config->_axes->_numberAxis; + auto n_axis = Axes::_numberAxis; for (int axis = 0; axis < n_axis; axis++) { float coordinate = cartesian[axis]; diff --git a/FluidNC/src/Kinematics/CoreXY.cpp b/FluidNC/src/Kinematics/CoreXY.cpp index 76815c75d..7a531b9f3 100644 --- a/FluidNC/src/Kinematics/CoreXY.cpp +++ b/FluidNC/src/Kinematics/CoreXY.cpp @@ -33,8 +33,8 @@ namespace Kinematics { log_info("Kinematic system: " << name()); // A limit switch on either axis stops both motors - config->_axes->_axis[X_AXIS]->_motors[0]->limitOtherAxis(Y_AXIS); - config->_axes->_axis[Y_AXIS]->_motors[0]->limitOtherAxis(X_AXIS); + Axes::_axis[X_AXIS]->_motors[0]->limitOtherAxis(Y_AXIS); + Axes::_axis[Y_AXIS]->_motors[0]->limitOtherAxis(X_AXIS); } bool CoreXY::canHome(AxisMask axisMask) { @@ -93,7 +93,7 @@ namespace Kinematics { bool CoreXY::cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) { // log_debug("cartesian_to_motors position (" << position[X_AXIS] << "," << position[Y_AXIS] << ") target (" << target[X_AXIS] << "," << target[Y_AXIS] << ")"); - auto n_axis = config->_axes->_numberAxis; + auto n_axis = Axes::_numberAxis; float motors[n_axis]; transform_cartesian_to_motors(motors, target); @@ -136,7 +136,7 @@ namespace Kinematics { motors[X_AXIS] = (_x_scaler * cartesian[X_AXIS]) + cartesian[Y_AXIS]; motors[Y_AXIS] = (_x_scaler * cartesian[X_AXIS]) - cartesian[Y_AXIS]; - auto n_axis = config->_axes->_numberAxis; + auto n_axis = Axes::_numberAxis; for (size_t axis = Z_AXIS; axis < n_axis; axis++) { motors[axis] = cartesian[axis]; } diff --git a/FluidNC/src/Kinematics/ParallelDelta.cpp b/FluidNC/src/Kinematics/ParallelDelta.cpp index cd4be2822..e5e34d81b 100644 --- a/FluidNC/src/Kinematics/ParallelDelta.cpp +++ b/FluidNC/src/Kinematics/ParallelDelta.cpp @@ -85,7 +85,7 @@ namespace Kinematics { log_info("Kinematic system:" << name() << " soft_limits:" << _softLimits); auto axes = config->_axes; - auto n_axis = config->_axes->_numberAxis; + auto n_axis = Axes::_numberAxis; // warn about axis soft limits for (int axis = 0; axis < n_axis; axis++) { @@ -293,7 +293,7 @@ namespace Kinematics { auto axes = config->_axes; auto n_axis = axes->_numberAxis; - config->_axes->set_disable(false); + Axes::set_disable(false); // TODO deal with non kinematic axes above Z for (int axis = 0; axis < 3; axis++) { diff --git a/FluidNC/src/Kinematics/WallPlotter.cpp b/FluidNC/src/Kinematics/WallPlotter.cpp index 10e5a09f3..dc66e5f86 100644 --- a/FluidNC/src/Kinematics/WallPlotter.cpp +++ b/FluidNC/src/Kinematics/WallPlotter.cpp @@ -26,7 +26,7 @@ namespace Kinematics { xy_to_lengths(0, 0, zero_left, zero_right); last_motor_segment_end[0] = zero_left; last_motor_segment_end[1] = zero_right; - auto n_axis = config->_axes->_numberAxis; + auto n_axis = Axes::_numberAxis; for (size_t axis = Z_AXIS; axis < n_axis; axis++) { last_motor_segment_end[axis] = 0.0; } @@ -36,7 +36,7 @@ namespace Kinematics { // Initialize the machine position void WallPlotter::init_position() { - auto n_axis = config->_axes->_numberAxis; + auto n_axis = Axes::_numberAxis; for (size_t axis = 0; axis < n_axis; axis++) { set_motor_steps(axis, 0); // Set to zeros } @@ -66,7 +66,7 @@ namespace Kinematics { float dx, dy, dz; // segment distances in each cartesian axis uint32_t segment_count; // number of segments the move will be broken in to. - auto n_axis = config->_axes->_numberAxis; + auto n_axis = Axes::_numberAxis; float total_cartesian_distance = vector_distance(position, target, n_axis); if (total_cartesian_distance == 0) { diff --git a/FluidNC/src/Limits.cpp b/FluidNC/src/Limits.cpp index e9594fd42..3c363de0c 100644 --- a/FluidNC/src/Limits.cpp +++ b/FluidNC/src/Limits.cpp @@ -47,16 +47,16 @@ MotorMask limits_get_state() { bool limits_startup_check() { // return true if there is a hard limit error. MotorMask lim_pin_state = limits_get_state(); if (lim_pin_state) { - auto n_axis = config->_axes->_numberAxis; + auto n_axis = Axes::_numberAxis; for (size_t axis = 0; axis < n_axis; axis++) { for (size_t motor = 0; motor < 2; motor++) { if (bitnum_is_true(lim_pin_state, Machine::Axes::motor_bit(axis, motor))) { - log_warn("Active limit switch on " << config->_axes->axisName(axis) << " axis motor " << motor); + log_warn("Active limit switch on " << Axes::axisName(axis) << " axis motor " << motor); } } } } - return (config->_start->_checkLimits && (config->_axes->hardLimitMask() & lim_pin_state)); + return (config->_start->_checkLimits && (Axes::hardLimitMask() & lim_pin_state)); } // Called only from Kinematics canHome() methods, hence from states allowing homing @@ -99,7 +99,7 @@ void limit_error() { } float limitsMaxPosition(size_t axis) { - auto axisConfig = config->_axes->_axis[axis]; + auto axisConfig = Axes::_axis[axis]; auto homing = axisConfig->_homing; auto mpos = homing ? homing->_mpos : 0; auto maxtravel = axisConfig->_maxTravel; @@ -108,7 +108,7 @@ float limitsMaxPosition(size_t axis) { } float limitsMinPosition(size_t axis) { - auto axisConfig = config->_axes->_axis[axis]; + auto axisConfig = Axes::_axis[axis]; auto homing = axisConfig->_homing; auto mpos = homing ? homing->_mpos : 0; auto maxtravel = axisConfig->_maxTravel; diff --git a/FluidNC/src/Machine/Axes.cpp b/FluidNC/src/Machine/Axes.cpp index b16068890..0fff03c1d 100644 --- a/FluidNC/src/Machine/Axes.cpp +++ b/FluidNC/src/Machine/Axes.cpp @@ -20,14 +20,19 @@ namespace Machine { bool Axes::disabled = false; - Axes::Axes() : _axis() { - for (int i = 0; i < MAX_N_AXIS; ++i) { - _axis[i] = nullptr; - } - } + Pin Axes::_sharedStepperDisable; + Pin Axes::_sharedStepperReset; + + uint32_t Axes::_homing_runs = 2; // Number of Approach/Pulloff cycles + + int Axes::_numberAxis = 0; + + Axis* Axes::_axis[MAX_N_AXIS] = { nullptr }; + + Axes::Axes() {} void Axes::init() { - log_info("Axis count " << config->_axes->_numberAxis); + log_info("Axis count " << Axes::_numberAxis); if (_sharedStepperDisable.defined()) { _sharedStepperDisable.setAttr(Pin::Attr::Output); @@ -72,8 +77,8 @@ namespace Machine { if (!disable && disabled) { disabled = false; - if (config->_stepping->_disableDelayUsecs) { // wait for the enable delay - delay_us(config->_stepping->_disableDelayUsecs); + if (Stepping::_disableDelayUsecs) { // wait for the enable delay + delay_us(Stepping::_disableDelayUsecs); } } } @@ -167,7 +172,7 @@ namespace Machine { // Some small helpers to find the axis index and axis motor index for a given motor. This // is helpful for some motors that need this info, as well as debug information. - size_t Axes::findAxisIndex(const MotorDrivers::MotorDriver* const driver) const { + size_t Axes::findAxisIndex(const MotorDrivers::MotorDriver* const driver) { for (int i = 0; i < _numberAxis; ++i) { for (int j = 0; j < Axis::MAX_MOTORS_PER_AXIS; ++j) { if (_axis[i] != nullptr && _axis[i]->hasMotor(driver)) { @@ -180,7 +185,7 @@ namespace Machine { return SIZE_MAX; } - size_t Axes::findAxisMotor(const MotorDrivers::MotorDriver* const driver) const { + size_t Axes::findAxisMotor(const MotorDrivers::MotorDriver* const driver) { for (int i = 0; i < _numberAxis; ++i) { if (_axis[i] != nullptr && _axis[i]->hasMotor(driver)) { for (int j = 0; j < Axis::MAX_MOTORS_PER_AXIS; ++j) { diff --git a/FluidNC/src/Machine/Axes.h b/FluidNC/src/Machine/Axes.h index 91201f45f..54003f835 100644 --- a/FluidNC/src/Machine/Axes.h +++ b/FluidNC/src/Machine/Axes.h @@ -31,26 +31,26 @@ namespace Machine { static bool disabled; - Pin _sharedStepperDisable; - Pin _sharedStepperReset; + static Pin _sharedStepperDisable; + static Pin _sharedStepperReset; - uint32_t _homing_runs = 2; // Number of Approach/Pulloff cycles + static uint32_t _homing_runs; // Number of Approach/Pulloff cycles - inline char axisName(int index) { return index < MAX_N_AXIS ? _names[index] : '?'; } // returns axis letter + static inline char axisName(int index) { return index < MAX_N_AXIS ? _names[index] : '?'; } // returns axis letter static inline size_t motor_bit(size_t axis, size_t motor) { return motor ? axis + 16 : axis; } static inline AxisMask motors_to_axes(MotorMask motors) { return (motors & 0xffff) | (motors >> 16); } static inline MotorMask axes_to_motors(AxisMask axes) { return axes | (axes << 16); } - int _numberAxis = 0; - Axis* _axis[MAX_N_AXIS]; + static int _numberAxis; + static Axis* _axis[MAX_N_AXIS]; // Some small helpers to find the axis index and axis motor number for a given motor. This // is helpful for some motors that need this info, as well as debug information. - size_t findAxisIndex(const MotorDrivers::MotorDriver* const motor) const; - size_t findAxisMotor(const MotorDrivers::MotorDriver* const motor) const; + static size_t findAxisIndex(const MotorDrivers::MotorDriver* const motor); + static size_t findAxisMotor(const MotorDrivers::MotorDriver* const motor); - MotorMask hardLimitMask(); + static MotorMask hardLimitMask(); inline bool hasHardLimits() const { for (int axis = 0; axis < _numberAxis; ++axis) { @@ -66,23 +66,23 @@ namespace Machine { return false; } - void init(); + static void init(); // These are used during homing cycles. // The return value is a bitmask of axes that can home - MotorMask set_homing_mode(AxisMask homing_mask, bool isHoming); + static MotorMask set_homing_mode(AxisMask homing_mask, bool isHoming); - void set_disable(int axis, bool disable); - void set_disable(bool disable); - void step(uint8_t step_mask, uint8_t dir_mask); - void unstep(); - void config_motors(); + static void set_disable(int axis, bool disable); + static void set_disable(bool disable); + static void step(uint8_t step_mask, uint8_t dir_mask); + static void unstep(); + static void config_motors(); - std::string maskToNames(AxisMask mask); + static std::string maskToNames(AxisMask mask); - bool namesToMask(const char* names, AxisMask& mask); + static bool namesToMask(const char* names, AxisMask& mask); - std::string motorMaskToNames(MotorMask mask); + static std::string motorMaskToNames(MotorMask mask); // Configuration helpers: void group(Configuration::HandlerBase& handler) override; diff --git a/FluidNC/src/Machine/Homing.cpp b/FluidNC/src/Machine/Homing.cpp index 8e54abd21..470c200fe 100644 --- a/FluidNC/src/Machine/Homing.cpp +++ b/FluidNC/src/Machine/Homing.cpp @@ -70,7 +70,7 @@ namespace Machine { void Homing::startMove(AxisMask axisMask, MotorMask motors, Phase phase, uint32_t& settle_ms) { float rate; - float target[config->_axes->_numberAxis]; + float target[Axes::_numberAxis]; axisVector(_phaseAxes, _phaseMotors, _phase, target, rate, _settling_ms); plan_line_data_t plan_data = {}; @@ -318,7 +318,7 @@ namespace Machine { return; } - log_debug("Homing limited" << config->_axes->motorMaskToNames(limited)); + log_debug("Homing limited" << Axes::motorMaskToNames(limited)); bool stop = config->_kinematics->limitReached(_phaseAxes, _phaseMotors, limited); @@ -329,7 +329,7 @@ namespace Machine { Stepper::reset(); // Stop moving if (_phaseAxes) { - log_debug("Homing replan with " << config->_axes->maskToNames(_phaseAxes)); + log_debug("Homing replan with " << Axes::maskToNames(_phaseAxes)); config->_kinematics->releaseMotors(_phaseAxes, _phaseMotors); @@ -380,21 +380,21 @@ namespace Machine { _cycleAxes = _remainingCycles.front(); _remainingCycles.pop(); - log_debug("Homing Cycle " << config->_axes->maskToNames(_cycleAxes)); + log_debug("Homing Cycle " << Axes::maskToNames(_cycleAxes)); _cycleAxes &= Machine::Axes::homingMask; - _cycleMotors = config->_axes->set_homing_mode(_cycleAxes, true); + _cycleMotors = Axes::set_homing_mode(_cycleAxes, true); _phase = Phase::PrePulloff; - _runs = config->_axes->_homing_runs; + _runs = Axes::_homing_runs; runPhase(); } void Homing::fail(ExecAlarm alarm) { Stepper::reset(); // Stop moving send_alarm(alarm); - config->_axes->set_homing_mode(_cycleAxes, false); // tell motors homing is done...failed - config->_axes->set_disable(config->_stepping->_idleMsecs != 255); + Axes::set_homing_mode(_cycleAxes, false); // tell motors homing is done...failed + Axes::set_disable(Stepping::_idleMsecs != 255); } bool Homing::needsPulloff2(MotorMask motors) { @@ -453,7 +453,7 @@ namespace Machine { #if 0 static std::string axisNames(AxisMask axisMask) { std::string retval = ""; - auto n_axis = config->_axes->_numberAxis; + auto n_axis = Axes::_numberAxis; for (size_t axis = 0; axis < n_axis; axis++) { if (bitnum_is_true(axisMask, axis)) { retval += Machine::Axes::_names[axis]; @@ -479,9 +479,9 @@ namespace Machine { } // Find any cycles that set the m_pos without motion - auto n_axis = config->_axes->_numberAxis; + auto n_axis = Axes::_numberAxis; for (int axis = X_AXIS; axis < n_axis; axis++) { - auto homing = config->_axes->_axis[axis]->_homing; + auto homing = Axes::_axis[axis]->_homing; if (homing && homing->_cycle == set_mpos_only) { if (axisMask == 0 || axisMask & 1 << axis) { float* mpos = get_mpos(); @@ -529,9 +529,9 @@ namespace Machine { AxisMask Homing::axis_mask_from_cycle(int cycle) { AxisMask axisMask = 0; - auto n_axis = config->_axes->_numberAxis; + auto n_axis = Axes::_numberAxis; for (int axis = 0; axis < n_axis; axis++) { - auto axisConfig = config->_axes->_axis[axis]; + auto axisConfig = Axes::_axis[axis]; auto homing = axisConfig->_homing; if (homing && homing->_cycle == cycle) { set_bitnum(axisMask, axis); diff --git a/FluidNC/src/Machine/LimitPin.cpp b/FluidNC/src/Machine/LimitPin.cpp index dd5eeaf9d..f2b4b96e7 100644 --- a/FluidNC/src/Machine/LimitPin.cpp +++ b/FluidNC/src/Machine/LimitPin.cpp @@ -37,7 +37,7 @@ namespace Machine { // Set a bitmap with bits to represent the axis and which motors are affected // The bitmap looks like CBAZYX..cbazyx where motor0 motors are in the lower bits _bitmask = 1 << Axes::motor_bit(axis, motor); - _legend = config->_axes->motorMaskToNames(_bitmask); + _legend = Axes::motorMaskToNames(_bitmask); _legend += " "; _legend += sDir; _legend += " Limit"; @@ -87,7 +87,11 @@ namespace Machine { // Make this switch act like an axis level switch. Both motors will report the same // This should be called from a higher level object, that has the logic to figure out // if this belongs to a dual motor, single switch axis - void LimitPin::makeDualMask() { _bitmask = Axes::axes_to_motors(Axes::motors_to_axes(_bitmask)); } + void LimitPin::makeDualMask() { + _bitmask = Axes::axes_to_motors(Axes::motors_to_axes(_bitmask)); + } - void LimitPin::setExtraMotorLimit(int axis, int motorNum) { _pExtraLimited = &config->_axes->_axis[axis]->_motors[motorNum]->_limited; } + void LimitPin::setExtraMotorLimit(int axis, int motorNum) { + _pExtraLimited = &config->_axes->_axis[axis]->_motors[motorNum]->_limited; + } } diff --git a/FluidNC/src/Machine/MachineConfig.h b/FluidNC/src/Machine/MachineConfig.h index 278a9150e..9aa847799 100644 --- a/FluidNC/src/Machine/MachineConfig.h +++ b/FluidNC/src/Machine/MachineConfig.h @@ -120,7 +120,7 @@ extern Machine::MachineConfig* config; template void copyAxes(T* dest, T* src) { - auto n_axis = config->_axes->_numberAxis; + auto n_axis = Axes::_numberAxis; for (size_t axis = 0; axis < n_axis; axis++) { dest[axis] = src[axis]; } diff --git a/FluidNC/src/Main.cpp b/FluidNC/src/Main.cpp index 3a356840a..b620578d3 100644 --- a/FluidNC/src/Main.cpp +++ b/FluidNC/src/Main.cpp @@ -90,7 +90,7 @@ void setup() { } } - config->_stepping->init(); // Configure stepper interrupt timers + Stepping::init(); // Configure stepper interrupt timers plan_init(); @@ -98,7 +98,7 @@ void setup() { config->_userInputs->init(); - config->_axes->init(); + Axes::init(); config->_control->init(); diff --git a/FluidNC/src/MotionControl.cpp b/FluidNC/src/MotionControl.cpp index a03904892..243b9532f 100644 --- a/FluidNC/src/MotionControl.cpp +++ b/FluidNC/src/MotionControl.cpp @@ -145,7 +145,7 @@ void mc_arc(float* target, float radii[2] = { -offset[axis_0], -offset[axis_1] }; float rt[2] = { target[axis_0] - center[0], target[axis_1] - center[1] }; - auto n_axis = config->_axes->_numberAxis; + auto n_axis = Axes::_numberAxis; float previous_position[n_axis] = { 0.0 }; for (size_t i = 0; i < n_axis; i++) { @@ -295,7 +295,7 @@ GCUpdatePos mc_probe_cycle(float* target, plan_line_data_t* pl_data, bool away, return GCUpdatePos::None; // Return if system reset has been issued. } - config->_stepping->beginLowLatency(); + Stepping::beginLowLatency(); // Initialize probing control variables probe_succeeded = false; // Re-initialize probe history before beginning cycle. @@ -305,7 +305,7 @@ GCUpdatePos mc_probe_cycle(float* target, plan_line_data_t* pl_data, bool away, if (config->_probe->tripped()) { send_alarm(ExecAlarm::ProbeFailInitial); protocol_execute_realtime(); - config->_stepping->endLowLatency(); + Stepping::endLowLatency(); return GCUpdatePos::None; // Nothing else to do but bail. } // Setup and queue probing motion. Auto cycle-start should not start the cycle. @@ -317,12 +317,12 @@ GCUpdatePos mc_probe_cycle(float* target, plan_line_data_t* pl_data, bool away, do { protocol_execute_realtime(); if (sys.abort) { - config->_stepping->endLowLatency(); + Stepping::endLowLatency(); return GCUpdatePos::None; // Check for system abort } } while (!state_is(State::Idle)); - config->_stepping->endLowLatency(); + Stepping::endLowLatency(); // Probing cycle complete! // Set state variables and error out, if the probe failed and cycle with error is enabled. @@ -355,7 +355,7 @@ GCUpdatePos mc_probe_cycle(float* target, plan_line_data_t* pl_data, bool away, motor_steps_to_mpos(probe_contact, probe_steps); coords[gc_state.modal.coord_select]->get(coord_data); // get a copy of the current coordinate offsets - auto n_axis = config->_axes->_numberAxis; + auto n_axis = Axes::_numberAxis; for (int axis = 0; axis < n_axis; axis++) { // find the axis specified. There should only be one. if (offsetAxis & (1 << axis)) { coord_data[axis] = probe_contact[axis] - offset; diff --git a/FluidNC/src/Motors/Dynamixel2.cpp b/FluidNC/src/Motors/Dynamixel2.cpp index 63a837b0b..33ffb77f1 100644 --- a/FluidNC/src/Motors/Dynamixel2.cpp +++ b/FluidNC/src/Motors/Dynamixel2.cpp @@ -171,7 +171,7 @@ namespace MotorDrivers { return false; } - auto axisConfig = config->_axes->_axis[_axis_index]; + auto axisConfig = Axes::_axis[_axis_index]; auto homing = axisConfig->_homing; auto mpos = homing ? homing->_mpos : 0; set_motor_steps(_axis_index, mpos_to_steps(mpos, _axis_index)); @@ -237,7 +237,7 @@ namespace MotorDrivers { if (data_len == 15) { uint32_t dxl_position = _rx_message[9] | (_rx_message[10] << 8) | (_rx_message[11] << 16) | (_rx_message[12] << 24); - auto axis = config->_axes->_axis[_axis_index]; + auto axis = Axes::_axis[_axis_index]; uint32_t pos_min_steps = mpos_to_steps(limitsMinPosition(_axis_index), _axis_index); uint32_t pos_max_steps = mpos_to_steps(limitsMaxPosition(_axis_index), _axis_index); diff --git a/FluidNC/src/Motors/MotorDriver.cpp b/FluidNC/src/Motors/MotorDriver.cpp index 47b5aaf37..3c7988f2d 100644 --- a/FluidNC/src/Motors/MotorDriver.cpp +++ b/FluidNC/src/Motors/MotorDriver.cpp @@ -27,12 +27,14 @@ namespace MotorDrivers { std::string MotorDriver::axisName() const { - return std::string(1, config->_axes->axisName(axis_index())) + (dual_axis_index() ? "2" : "") + " Axis"; + return std::string(1, Axes::axisName(axis_index())) + (dual_axis_index() ? "2" : "") + " Axis"; } void MotorDriver::debug_message() {} - bool MotorDriver::test() { return true; }; // true = OK + bool MotorDriver::test() { + return true; + }; // true = OK size_t MotorDriver::axis_index() const { Assert(config != nullptr && config->_axes != nullptr, "Expected machine to be configured before this is called."); diff --git a/FluidNC/src/Motors/RcServo.cpp b/FluidNC/src/Motors/RcServo.cpp index 2d99aeb7f..4243ad46e 100644 --- a/FluidNC/src/Motors/RcServo.cpp +++ b/FluidNC/src/Motors/RcServo.cpp @@ -82,7 +82,7 @@ namespace MotorDrivers { return false; if (isHoming) { - auto axisConfig = config->_axes->_axis[_axis_index]; + auto axisConfig = Axes::_axis[_axis_index]; auto homing = axisConfig->_homing; auto mpos = homing ? homing->_mpos : 0; set_motor_steps(_axis_index, mpos_to_steps(mpos, _axis_index)); diff --git a/FluidNC/src/Motors/TrinamicBase.cpp b/FluidNC/src/Motors/TrinamicBase.cpp index 3a2da168e..502ba859c 100644 --- a/FluidNC/src/Motors/TrinamicBase.cpp +++ b/FluidNC/src/Motors/TrinamicBase.cpp @@ -32,11 +32,11 @@ namespace MotorDrivers { // This is used to set the stallguard window from the homing speed. // The percent is the offset on the window uint32_t TrinamicBase::calc_tstep(int percent) { - auto axisConfig = config->_axes->_axis[axis_index()]; + auto axisConfig = Axes::_axis[axis_index()]; auto homing = axisConfig->_homing; auto homingFeedrate = homing ? homing->_feedRate : 200.0; - double tstep = homingFeedrate / 60.0 * config->_axes->_axis[axis_index()]->_stepsPerMm * (256.0 / _microsteps); + double tstep = homingFeedrate / 60.0 * Axes::_axis[axis_index()]->_stepsPerMm * (256.0 / _microsteps); tstep = fclk / tstep * percent / 100.0; return static_cast(tstep); diff --git a/FluidNC/src/Motors/UnipolarMotor.cpp b/FluidNC/src/Motors/UnipolarMotor.cpp index 8b190e6a3..f1b14dde9 100644 --- a/FluidNC/src/Motors/UnipolarMotor.cpp +++ b/FluidNC/src/Motors/UnipolarMotor.cpp @@ -15,8 +15,8 @@ namespace MotorDrivers { } void UnipolarMotor::config_message() { - log_info(" " << name() << " Ph0:" << _pin_phase0.name() << " Ph1:" << _pin_phase1.name() - << " Ph2:" << _pin_phase2.name() << " Ph3:" << _pin_phase3.name()); + log_info(" " << name() << " Ph0:" << _pin_phase0.name() << " Ph1:" << _pin_phase1.name() << " Ph2:" << _pin_phase2.name() + << " Ph3:" << _pin_phase3.name()); } void IRAM_ATTR UnipolarMotor::set_disable(bool disable) { @@ -29,7 +29,9 @@ namespace MotorDrivers { _enabled = !disable; } - void IRAM_ATTR UnipolarMotor::set_direction(bool dir) { _dir = dir; } + void IRAM_ATTR UnipolarMotor::set_direction(bool dir) { + _dir = dir; + } void IRAM_ATTR UnipolarMotor::step() { uint8_t _phase[8] = { 0, 0, 0, 0, 0, 0, 0, 0 }; // temporary phase values...all start as off @@ -116,7 +118,7 @@ namespace MotorDrivers { _pin_phase1.write(_phase[1]); _pin_phase2.write(_phase[2]); _pin_phase3.write(_phase[3]); - config->_stepping->startPulseTimer(); + Stepping::startPulseTimer(); } // Configuration registration diff --git a/FluidNC/src/NutsBolts.cpp b/FluidNC/src/NutsBolts.cpp index 41207b473..4cbd48e97 100644 --- a/FluidNC/src/NutsBolts.cpp +++ b/FluidNC/src/NutsBolts.cpp @@ -151,7 +151,7 @@ void scale_vector(float* v, float scale, size_t n) { } float convert_delta_vector_to_unit_vector(float* v) { - auto n_axis = config->_axes->_numberAxis; + auto n_axis = Axes::_numberAxis; float magnitude = vector_length(v, n_axis); scale_vector(v, 1.0f / magnitude, n_axis); return magnitude; @@ -161,9 +161,9 @@ const float secPerMinSq = 60.0 * 60.0; // Seconds Per Minute Squared, for accel float limit_acceleration_by_axis_maximum(float* unit_vec) { float limit_value = SOME_LARGE_VALUE; - auto n_axis = config->_axes->_numberAxis; + auto n_axis = Axes::_numberAxis; for (size_t idx = 0; idx < n_axis; idx++) { - auto axisSetting = config->_axes->_axis[idx]; + auto axisSetting = Axes::_axis[idx]; if (unit_vec[idx] != 0) { // Avoid divide by zero. limit_value = MIN(limit_value, fabsf(axisSetting->_acceleration / unit_vec[idx])); } @@ -177,9 +177,9 @@ float limit_acceleration_by_axis_maximum(float* unit_vec) { float limit_rate_by_axis_maximum(float* unit_vec) { float limit_value = SOME_LARGE_VALUE; - auto n_axis = config->_axes->_numberAxis; + auto n_axis = Axes::_numberAxis; for (size_t idx = 0; idx < n_axis; idx++) { - auto axisSetting = config->_axes->_axis[idx]; + auto axisSetting = Axes::_axis[idx]; if (unit_vec[idx] != 0) { // Avoid divide by zero. limit_value = MIN(limit_value, fabsf(axisSetting->_maxRate / unit_vec[idx])); } diff --git a/FluidNC/src/OLED.cpp b/FluidNC/src/OLED.cpp index 7974f3faf..e5b8901e0 100644 --- a/FluidNC/src/OLED.cpp +++ b/FluidNC/src/OLED.cpp @@ -147,7 +147,7 @@ void OLED::show_dro(const float* axes, bool isMpos, bool* limits) { return; } - auto n_axis = config->_axes->_numberAxis; + auto n_axis = Axes::_numberAxis; char axisVal[20]; show(limitLabelLayout, "L"); diff --git a/FluidNC/src/Pin.h b/FluidNC/src/Pin.h index b9bc9aa4f..9d6cf1d2d 100644 --- a/FluidNC/src/Pin.h +++ b/FluidNC/src/Pin.h @@ -95,7 +95,7 @@ class Pin { inline Pin(Pin&& o) : _detail(nullptr) { std::swap(_detail, o._detail); } inline Pin& operator=(const Pin& o) = delete; - inline Pin& operator =(Pin&& o) { + inline Pin& operator=(Pin&& o) { std::swap(_detail, o._detail); return *this; } diff --git a/FluidNC/src/Pins/ChannelPinDetail.cpp b/FluidNC/src/Pins/ChannelPinDetail.cpp index db944487f..84972c423 100644 --- a/FluidNC/src/Pins/ChannelPinDetail.cpp +++ b/FluidNC/src/Pins/ChannelPinDetail.cpp @@ -34,7 +34,9 @@ namespace Pins { s += std::to_string(high); _channel->out(s, "SET:"); } - int ChannelPinDetail::read() { return _value; } + int ChannelPinDetail::read() { + return _value; + } void ChannelPinDetail::setAttr(PinAttributes attr) { _attributes = _attributes | attr; @@ -61,8 +63,10 @@ namespace Pins { _channel->setAttr(_index, _attributes.has(Pins::PinAttributes::Input) ? &this->_value : nullptr, s, "INI:"); } - PinAttributes ChannelPinDetail::getAttr() const { return _attributes; } - std::string ChannelPinDetail::toString() { + PinAttributes ChannelPinDetail::getAttr() const { + return _attributes; + } + std::string ChannelPinDetail::toString() { std::string s = _channel->name(); s += "."; s += std::to_string(_index); @@ -78,5 +82,7 @@ namespace Pins { return s; } - void ChannelPinDetail::registerEvent(EventPin* obj) { _channel->registerEvent(_index, obj); } + void ChannelPinDetail::registerEvent(EventPin* obj) { + _channel->registerEvent(_index, obj); + } } diff --git a/FluidNC/src/Pins/DebugPinDetail.cpp b/FluidNC/src/Pins/DebugPinDetail.cpp index d32f7f9f7..19f205bd2 100644 --- a/FluidNC/src/Pins/DebugPinDetail.cpp +++ b/FluidNC/src/Pins/DebugPinDetail.cpp @@ -58,7 +58,9 @@ namespace Pins { _implementation->setAttr(value); } - PinAttributes DebugPinDetail::getAttr() const { return _implementation->getAttr(); } + PinAttributes DebugPinDetail::getAttr() const { + return _implementation->getAttr(); + } void DebugPinDetail::CallbackHandler::handle(void* arg) { auto handler = static_cast(arg); diff --git a/FluidNC/src/Pins/ErrorPinDetail.cpp b/FluidNC/src/Pins/ErrorPinDetail.cpp index 32a390c43..984e789be 100644 --- a/FluidNC/src/Pins/ErrorPinDetail.cpp +++ b/FluidNC/src/Pins/ErrorPinDetail.cpp @@ -8,11 +8,15 @@ namespace Pins { ErrorPinDetail::ErrorPinDetail(std::string_view descr) : PinDetail(0), _description(descr) {} - PinCapabilities ErrorPinDetail::capabilities() const { return PinCapabilities::Error; } + PinCapabilities ErrorPinDetail::capabilities() const { + return PinCapabilities::Error; + } #ifdef ESP32 - void ErrorPinDetail::write(int high) { log_error("Cannot write to pin " << _description.c_str() << ". The config is incorrect."); } - int ErrorPinDetail::read() { + void ErrorPinDetail::write(int high) { + log_error("Cannot write to pin " << _description.c_str() << ". The config is incorrect."); + } + int ErrorPinDetail::read() { log_error("Cannot read from pin " << _description.c_str() << ". The config is incorrect."); return false; } @@ -21,8 +25,10 @@ namespace Pins { } #else - void ErrorPinDetail::write(int high) { Assert(false, "Cannot write to an error pin."); } - int ErrorPinDetail::read() { + void ErrorPinDetail::write(int high) { + Assert(false, "Cannot write to an error pin."); + } + int ErrorPinDetail::read() { Assert(false, "Cannot read from an error pin."); return false; } @@ -31,7 +37,9 @@ namespace Pins { #endif - PinAttributes ErrorPinDetail::getAttr() const { return PinAttributes::None; } + PinAttributes ErrorPinDetail::getAttr() const { + return PinAttributes::None; + } std::string ErrorPinDetail::toString() { std::string s("ERROR_PIN (for "); diff --git a/FluidNC/src/Planner.cpp b/FluidNC/src/Planner.cpp index d7e99a366..54747b653 100644 --- a/FluidNC/src/Planner.cpp +++ b/FluidNC/src/Planner.cpp @@ -317,13 +317,13 @@ bool plan_buffer_line(float* target, plan_line_data_t* pl_data) { get_motor_steps(position_steps); } else { if (!block->is_jog && Homing::unhomed_axes()) { - log_info("Unhomed axes: " << config->_axes->maskToNames(Homing::unhomed_axes())); + log_info("Unhomed axes: " << Axes::maskToNames(Homing::unhomed_axes())); send_alarm(ExecAlarm::Unhomed); return false; } copyAxes(position_steps, pl.position); } - auto n_axis = config->_axes->_numberAxis; + auto n_axis = Axes::_numberAxis; for (size_t idx = 0; idx < n_axis; idx++) { // Calculate target position in absolute steps, number of steps for each axis, and determine max step events. // Also, compute individual axes distance for move and prep unit vector calculations. diff --git a/FluidNC/src/ProcessSettings.cpp b/FluidNC/src/ProcessSettings.cpp index 9bf29e37f..ce796cb56 100644 --- a/FluidNC/src/ProcessSettings.cpp +++ b/FluidNC/src/ProcessSettings.cpp @@ -289,7 +289,7 @@ static Error disable_alarm_lock(const char* value, AuthenticationLevel auth_leve return err; } Homing::set_all_axes_homed(); - config->_kinematics->releaseMotors(config->_axes->motorMask, config->_axes->hardLimitMask()); + config->_kinematics->releaseMotors(Axes::motorMask, Axes::hardLimitMask()); report_feedback_message(Message::AlarmUnlock); set_state(State::Idle); } @@ -379,10 +379,10 @@ static Error cmd_log_verbose(const char* value, AuthenticationLevel auth_level, static Error home(AxisMask axisMask, Channel& out) { if (axisMask != Machine::Homing::AllCycles) { // if not AllCycles we need to make sure the cycle is not prohibited // if there is a cycle it is the axis from $H - auto n_axis = config->_axes->_numberAxis; + auto n_axis = Axes::_numberAxis; for (int axis = 0; axis < n_axis; axis++) { if (bitnum_is_true(axisMask, axis)) { - auto axisConfig = config->_axes->_axis[axis]; + auto axisConfig = Axes::_axis[axis]; auto homing = axisConfig->_homing; auto homing_allowed = homing && homing->_allow_single_axis; if (!homing_allowed) @@ -449,7 +449,7 @@ static Error home_all(const char* value, AuthenticationLevel auth_level, Channel return retval; } } - if (!config->_axes->namesToMask(value, requestedAxes)) { + if (!Axes::namesToMask(value, requestedAxes)) { return Error::InvalidValue; } } @@ -636,7 +636,7 @@ static Error motor_control(const char* value, bool disable) { } if (!value || *value == '\0') { log_info((disable ? "Dis" : "En") << "abling all motors"); - config->_axes->set_disable(disable); + Axes::set_disable(disable); return Error::Ok; } @@ -647,7 +647,7 @@ static Error motor_control(const char* value, bool disable) { return Error::InvalidStatement; } - for (int i = 0; i < config->_axes->_numberAxis; i++) { + for (int i = 0; i < Axes::_numberAxis; i++) { char axisName = axes->axisName(i); if (strchr(value, axisName) || strchr(value, tolower(axisName))) { @@ -666,7 +666,7 @@ static Error motor_enable(const char* value, AuthenticationLevel auth_level, Cha } static Error motors_init(const char* value, AuthenticationLevel auth_level, Channel& out) { - config->_axes->config_motors(); + Axes::config_motors(); return Error::Ok; } diff --git a/FluidNC/src/Protocol.cpp b/FluidNC/src/Protocol.cpp index 0cee4c87d..ea42de908 100644 --- a/FluidNC/src/Protocol.cpp +++ b/FluidNC/src/Protocol.cpp @@ -239,7 +239,7 @@ static void check_startup_state() { report_error_message(Message::ConfigAlarmLock); } else { // Perform some machine checks to make sure everything is good to go. - if (config->_start->_checkLimits && config->_axes->hasHardLimits()) { + if (config->_start->_checkLimits && Axes::hasHardLimits()) { if (limits_get_state()) { sys.state = State::Alarm; // Ensure alarm state is active. report_error_message(Message::CheckLimits); @@ -319,7 +319,7 @@ void protocol_main_loop() { if (idleEndTime && (getCpuTicks() - idleEndTime) > 0) { idleEndTime = 0; // - config->_axes->set_disable(true); + Axes::set_disable(true); } uint32_t newHeapSize = xPortGetFreeHeapSize(); if (newHeapSize < heapLowWater) { @@ -740,23 +740,23 @@ static void protocol_do_cycle_start() { void protocol_disable_steppers() { if (state_is(State::Homing)) { // Leave steppers enabled while homing - config->_axes->set_disable(false); + Axes::set_disable(false); return; } if (state_is(State::Sleep) || state_is(State::Alarm)) { // Disable steppers immediately in sleep or alarm state - config->_axes->set_disable(true); + Axes::set_disable(true); return; } - if (config->_stepping->_idleMsecs == 255) { + if (Stepping::_idleMsecs == 255) { // Leave steppers enabled if configured for "stay enabled" - config->_axes->set_disable(false); + Axes::set_disable(false); return; } // Otherwise, schedule stepper disable in a few milliseconds // unless a disable time has already been scheduled if (idleEndTime == 0) { - idleEndTime = usToEndTicks(config->_stepping->_idleMsecs * 1000); + idleEndTime = usToEndTicks(Stepping::_idleMsecs * 1000); // idleEndTime 0 means that a stepper disable is not scheduled. so if we happen to // land on 0 as an end time, just push it back by one microsecond to get off 0. if (idleEndTime == 0) { @@ -835,7 +835,7 @@ static void protocol_do_late_reset() { config->_coolant->stop(); protocol_disable_steppers(); - config->_stepping->reset(); + Stepping::reset(); // turn off all User I/O immediately config->_userOutputs->all_off(); @@ -1086,7 +1086,7 @@ static void protocol_do_limit(void* arg) { limit->isHard()) { mc_critical(ExecAlarm::HardLimit); } - log_debug("Limit switch tripped for " << config->_axes->axisName(limit->_axis) << " motor " << limit->_motorNum); + log_debug("Limit switch tripped for " << Axes::axisName(limit->_axis) << " motor " << limit->_motorNum); } static void protocol_do_fault_pin(void* arg) { if (inMotionState() || state_is(State::Idle) || state_is(State::Hold) || state_is(State::SafetyDoor)) { diff --git a/FluidNC/src/Report.cpp b/FluidNC/src/Report.cpp index 2eced1eed..c5644b394 100644 --- a/FluidNC/src/Report.cpp +++ b/FluidNC/src/Report.cpp @@ -82,7 +82,7 @@ static const int axesStringLen = coordStringLen * MAX_N_AXIS; // Sends the axis values to the output channel static std::string report_util_axis_values(const float* axis_value) { std::ostringstream msg; - auto n_axis = config->_axes->_numberAxis; + auto n_axis = Axes::_numberAxis; for (size_t idx = 0; idx < n_axis; idx++) { int decimals; float value = axis_value[idx]; @@ -434,7 +434,7 @@ void addPinReport(char* status, char pinLetter) { void mpos_to_wpos(float* position) { float* wco = get_wco(); - auto n_axis = config->_axes->_numberAxis; + auto n_axis = Axes::_numberAxis; for (int idx = 0; idx < n_axis; idx++) { position[idx] -= wco[idx]; } @@ -483,11 +483,11 @@ void report_recompute_pin_string() { MotorMask lim_pin_state = limits_get_state(); if (lim_pin_state) { - auto n_axis = config->_axes->_numberAxis; + auto n_axis = Axes::_numberAxis; for (size_t axis = 0; axis < n_axis; axis++) { if (bitnum_is_true(lim_pin_state, Machine::Axes::motor_bit(axis, 0)) || bitnum_is_true(lim_pin_state, Machine::Axes::motor_bit(axis, 1))) { - report_pin_string += config->_axes->axisName(axis); + report_pin_string += Axes::axisName(axis); } } } diff --git a/FluidNC/src/Stepper.cpp b/FluidNC/src/Stepper.cpp index 6d62fc604..6c44bd6e0 100644 --- a/FluidNC/src/Stepper.cpp +++ b/FluidNC/src/Stepper.cpp @@ -24,7 +24,7 @@ static bool awake = false; // Stores the planner block Bresenham algorithm execution data for the segments in the segment // buffer. Normally, this buffer is partially in-use, but, for the worst case scenario, it will -// never exceed the number of accessible stepper buffer segments (config->_stepping->_segments-1). +// never exceed the number of accessible stepper buffer segments (Stepping::_segments-1). // NOTE: This data is copied from the prepped planner blocks so that the planner blocks may be // discarded when entirely consumed and completed by the segment buffer. Also, AMASS alters this // data for its own use. @@ -54,11 +54,11 @@ void Stepper::init() { if (st_block_buffer) { delete[] st_block_buffer; } - st_block_buffer = new st_block_t[config->_stepping->_segments - 1]; + st_block_buffer = new st_block_t[Stepping::_segments - 1]; if (segment_buffer) { delete[] segment_buffer; } - segment_buffer = new segment_t[config->_stepping->_segments]; + segment_buffer = new segment_t[Stepping::_segments]; } // Stepper ISR data struct. Contains the running data for the main stepper ISR. @@ -201,7 +201,7 @@ bool IRAM_ATTR Stepper::pulse_func() { if (!awake) { return false; } - auto n_axis = config->_axes->_numberAxis; + auto n_axis = Axes::_numberAxis; config->_axes->step(st.step_outbits, st.dir_outbits); @@ -212,7 +212,7 @@ bool IRAM_ATTR Stepper::pulse_func() { // Initialize new step segment and load number of steps to execute st.exec_segment = &segment_buffer[segment_buffer_tail]; // Initialize step segment timing per step and load number of steps to execute. - config->_stepping->setTimerPeriod(st.exec_segment->isrPeriod); + Stepping::setTimerPeriod(st.exec_segment->isrPeriod); st.step_count = st.exec_segment->n_step; // NOTE: Can sometimes be zero when moving slow. // If the new segment starts a new planner block, initialize stepper variables and counters. // NOTE: When the segment data index changes, this indicates a new planner block. @@ -247,18 +247,6 @@ bool IRAM_ATTR Stepper::pulse_func() { return false; // Nothing to do but exit. } } -#if 0 - // Check probing state. - if (probing && config->_probe->tripped()) { - probing = false; - auto axes = config->_axes; - for (int axis = 0; axis < n_axis; axis++) { - auto m = axes->_axis[axis]->_motors[0]; - probe_steps[axis] = m ? m->_steps : 0; - } - protocol_send_event_from_ISR(&motionCancelEvent); - } -#endif // Reset step out bits. st.step_outbits = 0; @@ -275,7 +263,7 @@ bool IRAM_ATTR Stepper::pulse_func() { if (st.step_count == 0) { // Segment is complete. Discard current segment and advance segment indexing. st.exec_segment = NULL; - segment_buffer_tail = segment_buffer_tail >= (config->_stepping->_segments - 1) ? 0 : segment_buffer_tail + 1; + segment_buffer_tail = segment_buffer_tail >= (Stepping::_segments - 1) ? 0 : segment_buffer_tail + 1; } config->_axes->unstep(); @@ -291,10 +279,10 @@ void Stepper::wake_up() { // Cancel any pending stepper disable protocol_cancel_disable_steppers(); // Enable stepper drivers. - config->_axes->set_disable(false); + Axes::set_disable(false); // Enable Stepping Driver Interrupt - config->_stepping->startTimer(); + Stepping::startTimer(); } void Stepper::go_idle() { @@ -371,7 +359,7 @@ void Stepper::parking_restore_buffer() { // Increments the step segment buffer block data ring buffer. static uint8_t next_block_index(uint8_t block_index) { block_index++; - return block_index == (config->_stepping->_segments - 1) ? 0 : block_index; + return block_index == (Stepping::_segments - 1) ? 0 : block_index; } /* Prepares step segment buffer. Continuously called from main program. @@ -423,7 +411,7 @@ void Stepper::prep_buffer() { st_prep_block = &st_block_buffer[prep.st_block_index]; st_prep_block->direction_bits = pl_block->direction_bits; uint8_t idx; - auto n_axis = config->_axes->_numberAxis; + auto n_axis = Axes::_numberAxis; // Bit-shift multiply all Bresenham data by the max AMASS level so that // we never divide beyond the original data anywhere in the algorithm. @@ -746,7 +734,7 @@ void Stepper::prep_buffer() { // Segment complete! Increment segment buffer indices, so stepper ISR can immediately execute it. auto lastseg = segment_next_head; - segment_next_head = segment_next_head >= (config->_stepping->_segments - 1) ? 0 : segment_next_head + 1; + segment_next_head = segment_next_head >= (Stepping::_segments - 1) ? 0 : segment_next_head + 1; segment_buffer_head = lastseg; // Update the appropriate planner and segment data. diff --git a/FluidNC/src/Stepping.cpp b/FluidNC/src/Stepping.cpp index edd8a28e4..0df329e0a 100644 --- a/FluidNC/src/Stepping.cpp +++ b/FluidNC/src/Stepping.cpp @@ -9,6 +9,14 @@ namespace Machine { int Stepping::_engine = RMT; + bool Stepping::_switchedStepper = false; + int32_t Stepping::_stepPulseEndTime; + size_t Stepping::_segments = 12; + + uint32_t Stepping::_idleMsecs = 255; + uint32_t Stepping::_pulseUsecs = 4; + uint32_t Stepping::_directionDelayUsecs = 0; + uint32_t Stepping::_disableDelayUsecs = 0; const EnumItem stepTypes[] = { { Stepping::TIMED, "Timed" }, { Stepping::RMT, "RMT" }, diff --git a/FluidNC/src/Stepping.h b/FluidNC/src/Stepping.h index f4af242ec..0dfb25f6f 100644 --- a/FluidNC/src/Stepping.h +++ b/FluidNC/src/Stepping.h @@ -14,12 +14,11 @@ namespace Machine { static const uint32_t fStepperTimer = 20000000; // frequency of step pulse timer private: - static bool onStepperDriverTimer(); - static const int ticksPerMicrosecond = fStepperTimer / 1000000; - bool _switchedStepper = false; - int32_t _stepPulseEndTime; + static bool _switchedStepper; + static int32_t _stepPulseEndTime; + public: enum stepper_id_t { @@ -38,32 +37,38 @@ namespace Machine { // execution lead time there is for other processes to run. The latency for a feedhold or other // override is roughly 10 ms times _segments. - size_t _segments = 12; + static size_t _segments; - uint32_t _idleMsecs = 255; - uint32_t _pulseUsecs = 4; - uint32_t _directionDelayUsecs = 0; - uint32_t _disableDelayUsecs = 0; + static uint32_t _idleMsecs; + static uint32_t _pulseUsecs; + static uint32_t _directionDelayUsecs; + static uint32_t _disableDelayUsecs; static int _engine; // Interfaces to stepping engine - void init(); + static void init(); + + static void assignMotor(int axis, int motor, int step_pin, bool step_invert, int dir_pin, bool dir_invert); + + static void setStepPin(int axis, int motor, int pin, bool invert); + static void setDirPin(int axis, int motor, int pin, bool invert); + + static void reset(); // Clean up old state and start fresh + static void beginLowLatency(); + static void endLowLatency(); + static void startPulseTimer(); + static void waitPulse(); // Wait for pulse length + static void waitDirection(); // Wait for direction delay + static void waitMotion(); // Wait for motion to complete + static void finishPulse(); // Cleanup after unstep - void reset(); // Clean up old state and start fresh - void beginLowLatency(); - void endLowLatency(); - void startPulseTimer(); - void waitPulse(); // Wait for pulse length - void waitDirection(); // Wait for direction delay - void waitMotion(); // Wait for motion to complete - void finishPulse(); // Cleanup after unstep - uint32_t maxPulsesPerSec(); + static uint32_t maxPulsesPerSec(); // Timers - void setTimerPeriod(uint16_t timerTicks); - void startTimer(); + static void setTimerPeriod(uint16_t timerTicks); + static void startTimer(); static void stopTimer(); // Configuration system helpers: diff --git a/FluidNC/src/System.cpp b/FluidNC/src/System.cpp index 1a33c6da5..907f48e93 100644 --- a/FluidNC/src/System.cpp +++ b/FluidNC/src/System.cpp @@ -34,10 +34,10 @@ void system_reset() { } float steps_to_mpos(int32_t steps, size_t axis) { - return float(steps / config->_axes->_axis[axis]->_stepsPerMm); + return float(steps / Axes::_axis[axis]->_stepsPerMm); } int32_t mpos_to_steps(float mpos, size_t axis) { - return lroundf(mpos * config->_axes->_axis[axis]->_stepsPerMm); + return lroundf(mpos * Axes::_axis[axis]->_stepsPerMm); } void motor_steps_to_mpos(float* position, int32_t* steps) { @@ -98,7 +98,7 @@ float* get_mpos() { float* get_wco() { static float wco[MAX_N_AXIS]; - auto n_axis = config->_axes->_numberAxis; + auto n_axis = Axes::_numberAxis; for (int idx = 0; idx < n_axis; idx++) { // Apply work coordinate offsets and tool length offset to current position. wco[idx] = gc_state.coord_system[idx] + gc_state.coord_offset[idx]; diff --git a/FluidNC/src/WebUI/WifiConfig.cpp b/FluidNC/src/WebUI/WifiConfig.cpp index b32d2edee..7bf85c294 100644 --- a/FluidNC/src/WebUI/WifiConfig.cpp +++ b/FluidNC/src/WebUI/WifiConfig.cpp @@ -278,8 +278,8 @@ namespace WebUI { j.id_value_object("SSID: ", (const char*)conf.ap.ssid); j.id_value_object("Visible: ", (conf.ap.ssid_hidden == 0 ? "Yes" : "No")); j.id_value_object("Radio country set: ", - std::string("") + country.cc[0] + country.cc[1] + " (channels " + std::to_string(country.schan) + "-" + - std::to_string((country.schan + country.nchan - 1)) + ", max power " + + std::string("") + country.cc[0] + country.cc[1] + " (channels " + std::to_string(country.schan) + + "-" + std::to_string((country.schan + country.nchan - 1)) + ", max power " + std::to_string(country.max_tx_power) + "dBm)"); const char* mode; @@ -520,7 +520,7 @@ namespace WebUI { j.member("FlashFileSystem", "LittleFS"); j.member("HostPath", "/"); j.member("Time", "none"); - j.member("Axisletters", config->_axes->_names); + j.member("Axisletters", Axes::_names); j.end_object(); j.end(); return Error::Ok; @@ -582,7 +582,7 @@ namespace WebUI { } //to save time in decoding `?` - s << " # axis:" << config->_axes->_numberAxis; + s << " # axis:" << Axes::_numberAxis; return Error::Ok; }