From fc140c49624ba845f68d0e2180b0086f585febc3 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Fri, 9 Jun 2017 10:22:49 -0500 Subject: [PATCH] Followup for I2C_POSITION_ENCODERS --- .travis.yml | 2 +- Marlin/Configuration_adv.h | 1 + Marlin/I2CPositionEncoder.cpp | 420 ++++++++++++++++++---------------- Marlin/I2CPositionEncoder.h | 235 +++++++++---------- Marlin/Marlin_main.cpp | 4 +- Marlin/SanityCheck.h | 4 +- Marlin/ubl_G29.cpp | 7 +- 7 files changed, 352 insertions(+), 321 deletions(-) diff --git a/.travis.yml b/.travis.yml index 4d5a00f1b2..f12b1e8918 100644 --- a/.travis.yml +++ b/.travis.yml @@ -93,7 +93,7 @@ script: # - restore_configs - opt_enable AUTO_BED_LEVELING_UBL UBL_G26_MESH_EDITING ENABLE_LEVELING_FADE_HEIGHT FIX_MOUNTED_PROBE EEPROM_SETTINGS G3D_PANEL - - opt_enable_adv CUSTOM_USER_MENUS + - opt_enable_adv CUSTOM_USER_MENUS I2C_POSITION_ENCODERS BABYSTEPPING - build_marlin # # Test a Sled Z Probe diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h index 4c772ec2d7..08d30b58e1 100644 --- a/Marlin/Configuration_adv.h +++ b/Marlin/Configuration_adv.h @@ -1271,6 +1271,7 @@ //=========================================================================== //====================== I2C Position Encoder Settings ====================== //=========================================================================== + /** * I2C position encoders for closed loop control. * Developed by Chris Barr at Aus3D. diff --git a/Marlin/I2CPositionEncoder.cpp b/Marlin/I2CPositionEncoder.cpp index 8f226d9635..84334812f7 100644 --- a/Marlin/I2CPositionEncoder.cpp +++ b/Marlin/I2CPositionEncoder.cpp @@ -40,13 +40,14 @@ #include - void I2CPositionEncoder::init(uint8_t address, AxisEnum axis) { + + void I2CPositionEncoder::init(const uint8_t address, const AxisEnum axis) { encoderAxis = axis; i2cAddress = address; initialised++; - SERIAL_ECHOPAIR("Seetting up encoder on ", axis_codes[encoderAxis]); + SERIAL_ECHOPAIR("Setting up encoder on ", axis_codes[encoderAxis]); SERIAL_ECHOLNPAIR(" axis, addr = ", address); position = get_position(); @@ -98,13 +99,13 @@ //the encoder likely lost its place when the error occured, so we'll reset and use the printer's //idea of where it the axis is to re-initialise - double position = stepper.get_axis_position_mm(encoderAxis); - long positionInTicks = position * get_ticks_unit(); + float position = stepper.get_axis_position_mm(encoderAxis); + int32_t positionInTicks = position * get_ticks_unit(); //shift position from previous to current position zeroOffset -= (positionInTicks - get_position()); - #if defined(I2CPE_DEBUG) + #ifdef I2CPE_DEBUG SERIAL_ECHOPGM("Current position is "); SERIAL_ECHOLN(position); @@ -126,23 +127,23 @@ } lastPosition = position; - millis_t positionTime = millis(); + const millis_t positionTime = millis(); //only do error correction if setup and enabled if (ec && ecMethod != I2CPE_ECM_NONE) { - #if defined(I2CPE_EC_THRESH_PROPORTIONAL) - millis_t deltaTime = positionTime - lastPositionTime; - unsigned long distance = abs(position - lastPosition); - unsigned long speed = distance / deltaTime; - float threshold = constrain(speed / 50, 1, 50) * ecThreshold; + #ifdef I2CPE_EC_THRESH_PROPORTIONAL + const millis_t deltaTime = positionTime - lastPositionTime; + const uint32_t distance = abs(position - lastPosition), + speed = distance / deltaTime; + const float threshold = constrain((speed / 50), 1, 50) * ecThreshold; #else - float threshold = get_error_correct_threshold(); + const float threshold = get_error_correct_threshold(); #endif //check error #if ENABLED(I2CPE_ERR_ROLLING_AVERAGE) - double sum = 0, diffSum = 0; + float sum = 0, diffSum = 0; errIdx = (errIdx >= I2CPE_ERR_ARRAY_SIZE - 1) ? 0 : errIdx + 1; err[errIdx] = get_axis_error_steps(false); @@ -152,16 +153,16 @@ if (i) diffSum += abs(err[i-1] - err[i]); } - long error = (long)(sum/(I2CPE_ERR_ARRAY_SIZE + 1)); //calculate average for error + const int32_t error = int32_t(sum / (I2CPE_ERR_ARRAY_SIZE + 1)); //calculate average for error #else - long error = get_axis_error_steps(false); + const int32_t error = get_axis_error_steps(false); #endif - //SERIAL_ECHOPGM("Axis err*r steps: "); + //SERIAL_ECHOPGM("Axis error steps: "); //SERIAL_ECHOLN(error); - #if defined(I2CPE_ERR_THRESH_ABORT) + #ifdef I2CPE_ERR_THRESH_ABORT if (labs(error) > I2CPE_ERR_THRESH_ABORT * planner.axis_steps_per_mm[encoderAxis]) { //kill("Significant Error"); SERIAL_ECHOPGM("Axis error greater than set threshold, aborting!"); @@ -215,7 +216,7 @@ homed++; trusted++; - #if defined(I2CPE_DEBUG) + #ifdef I2CPE_DEBUG SERIAL_ECHO(axis_codes[encoderAxis]); SERIAL_ECHOPAIR(" axis encoder homed, offset of ", zeroOffset); SERIAL_ECHOLNPGM(" ticks."); @@ -223,36 +224,27 @@ } } - bool I2CPositionEncoder::passes_test(bool report) { - if (H == I2CPE_MAG_SIG_GOOD) { - if (report) { - SERIAL_ECHO(axis_codes[encoderAxis]); - SERIAL_ECHOLNPGM(" axis encoder passes test; field strength good."); - } - return true; - } else if (H == I2CPE_MAG_SIG_MID) { - if (report) { - SERIAL_ECHOPAIR("Warning, ", axis_codes[encoderAxis]); - SERIAL_ECHOLNPGM(" axis encoder passes test; field strength fair."); - } - return true; - } else if (H == I2CPE_MAG_SIG_BAD) { - if (report) { - SERIAL_ECHOPAIR("Warning, ", axis_codes[encoderAxis]); - SERIAL_ECHOLNPGM(" axis magnetic strip not detected!"); - } - return false; - } - + bool I2CPositionEncoder::passes_test(const bool report) { if (report) { - SERIAL_ECHOPAIR("Warning, ", axis_codes[encoderAxis]); - SERIAL_ECHOLNPGM(" axis encoder not detected!"); + if (H != I2CPE_MAG_SIG_GOOD) SERIAL_ECHOPGM("Warning. "); + SERIAL_ECHO(axis_codes[encoderAxis]); + SERIAL_ECHOPGM(" axis "); + serialprintPGM(H == I2CPE_MAG_SIG_BAD ? PSTR("magnetic strip ") : PSTR("encoder ")); + switch (H) { + case I2CPE_MAG_SIG_GOOD: + case I2CPE_MAG_SIG_MID: + SERIAL_ECHOLNPGM("passes test; field strength "); + serialprintPGM(H == I2CPE_MAG_SIG_GOOD ? PSTR("good.\n") : PSTR("fair.\n")); + break; + default: + SERIAL_ECHOLNPGM("not detected!"); + } } - return false; + return (H == I2CPE_MAG_SIG_GOOD || H == I2CPE_MAG_SIG_MID); } - double I2CPositionEncoder::get_axis_error_mm(bool report) { - double target, actual, error; + float I2CPositionEncoder::get_axis_error_mm(const bool report) { + float target, actual, error; target = stepper.get_axis_position_mm(encoderAxis); actual = mm_from_count(position); @@ -270,7 +262,7 @@ return error; } - long I2CPositionEncoder::get_axis_error_steps(bool report) { + int32_t I2CPositionEncoder::get_axis_error_steps(const bool report) { if (!active) { if (report) { SERIAL_ECHO(axis_codes[encoderAxis]); @@ -280,8 +272,8 @@ } float stepperTicksPerUnit; - long encoderTicks = position, encoderCountInStepperTicksScaled; - //long stepperTicks = stepper.position(encoderAxis); + int32_t encoderTicks = position, encoderCountInStepperTicksScaled; + //int32_t stepperTicks = stepper.position(encoderAxis); // With a rotary encoder we're concerned with ticks/rev; whereas with a linear we're concerned with ticks/mm stepperTicksPerUnit = (type == I2CPE_ENC_TYPE_ROTARY) ? stepperTicks : planner.axis_steps_per_mm[encoderAxis]; @@ -289,8 +281,8 @@ //convert both 'ticks' into same units / base encoderCountInStepperTicksScaled = LROUND((stepperTicksPerUnit * encoderTicks) / encoderTicksPerUnit); - long target = stepper.position(encoderAxis), - error = (encoderCountInStepperTicksScaled - target); + int32_t target = stepper.position(encoderAxis), + error = (encoderCountInStepperTicksScaled - target); //suppress discontinuities (might be caused by bad I2C readings...?) bool suppressOutput = (labs(error - errorPrev) > 100); @@ -309,7 +301,7 @@ return (suppressOutput ? 0 : error); } - long I2CPositionEncoder::get_raw_count() { + int32_t I2CPositionEncoder::get_raw_count() { uint8_t index = 0; i2cLong encoderCount; @@ -340,14 +332,11 @@ //only works on XYZ cartesian machines for the time being if (!(encoderAxis == X_AXIS || encoderAxis == Y_AXIS || encoderAxis == Z_AXIS)) return false; - int feedrate; - float startPosition, endPosition; - float startCoord[NUM_AXIS] = {0}, endCoord[NUM_AXIS] = {0}; + float startCoord[NUM_AXIS] = { 0 }, endCoord[NUM_AXIS] = { 0 }; - startPosition = soft_endstop_min[encoderAxis] + 10; - endPosition = soft_endstop_max[encoderAxis] - 10; - - feedrate = (int)MMM_TO_MMS((encoderAxis == Z_AXIS) ? HOMING_FEEDRATE_Z : HOMING_FEEDRATE_XY); + const float startPosition = soft_endstop_min[encoderAxis] + 10, + endPosition = soft_endstop_max[encoderAxis] - 10, + feedrate = FLOOR(MMM_TO_MMS((encoderAxis == Z_AXIS) ? HOMING_FEEDRATE_Z : HOMING_FEEDRATE_XY)); ec = false; @@ -367,7 +356,7 @@ // if the module isn't currently trusted, wait until it is (or until it should be if things are working) if (!trusted) { - long startWaitingTime = millis(); + int32_t startWaitingTime = millis(); while (!trusted && millis() - startWaitingTime < I2CPE_TIME_TRUSTED) safe_delay(500); } @@ -381,7 +370,7 @@ return trusted; } - void I2CPositionEncoder::calibrate_steps_mm(int iter) { + void I2CPositionEncoder::calibrate_steps_mm(const uint8_t iter) { if (type != I2CPE_ENC_TYPE_LINEAR) { SERIAL_ECHOLNPGM("Steps per mm calibration is only available using linear encoders."); return; @@ -392,14 +381,14 @@ return; } - float oldStepsMm, newStepsMm, + float old_steps_mm, new_steps_mm, startDistance, endDistance, travelDistance, travelledDistance, total = 0, - startCoord[NUM_AXIS] = {0}, endCoord[NUM_AXIS] = {0}; + startCoord[NUM_AXIS] = { 0 }, endCoord[NUM_AXIS] = { 0 }; - double feedrate; + float feedrate; - long startCount, stopCount; + int32_t startCount, stopCount; feedrate = MMM_TO_MMS((encoderAxis == Z_AXIS) ? HOMING_FEEDRATE_Z : HOMING_FEEDRATE_XY); @@ -447,17 +436,17 @@ SERIAL_ECHOLNPGM("mm."); //Calculate new axis steps per unit - oldStepsMm = planner.axis_steps_per_mm[encoderAxis]; - newStepsMm = (oldStepsMm * travelDistance) / travelledDistance; + old_steps_mm = planner.axis_steps_per_mm[encoderAxis]; + new_steps_mm = (old_steps_mm * travelDistance) / travelledDistance; - SERIAL_ECHOLNPAIR("Old steps per mm: ", oldStepsMm); - SERIAL_ECHOLNPAIR("New steps per mm: ", newStepsMm); + SERIAL_ECHOLNPAIR("Old steps per mm: ", old_steps_mm); + SERIAL_ECHOLNPAIR("New steps per mm: ", new_steps_mm); //Save new value - planner.axis_steps_per_mm[encoderAxis] = newStepsMm; + planner.axis_steps_per_mm[encoderAxis] = new_steps_mm; if (iter > 1) { - total += newStepsMm; + total += new_steps_mm; // swap start and end points so next loop runs from current position float tempCoord = startCoord[encoderAxis]; @@ -486,6 +475,12 @@ #endif } + + bool I2CPositionEncodersMgr::I2CPE_anyaxis; + uint8_t I2CPositionEncodersMgr::I2CPE_addr, + I2CPositionEncodersMgr::I2CPE_idx; + I2CPositionEncoder I2CPositionEncodersMgr::encoders[I2CPE_ENCODER_CNT]; + void I2CPositionEncodersMgr::init() { Wire.begin(); @@ -494,28 +489,28 @@ encoders[i].init(I2CPE_ENC_1_ADDR, I2CPE_ENC_1_AXIS); - #if defined(I2CPE_ENC_1_TYPE) + #ifdef I2CPE_ENC_1_TYPE encoders[i].set_type(I2CPE_ENC_1_TYPE); #endif - #if defined(I2CPE_ENC_1_TICKS_UNIT) + #ifdef I2CPE_ENC_1_TICKS_UNIT encoders[i].set_ticks_unit(I2CPE_ENC_1_TICKS_UNIT); #endif - #if defined(I2CPE_ENC_1_TICKS_REV) + #ifdef I2CPE_ENC_1_TICKS_REV encoders[i].set_stepper_ticks(I2CPE_ENC_1_TICKS_REV); #endif - #if defined(I2CPE_ENC_1_INVERT) + #ifdef I2CPE_ENC_1_INVERT encoders[i].set_inverted(I2CPE_ENC_1_INVERT); #endif - #if defined(I2CPE_ENC_1_EC_METHOD) + #ifdef I2CPE_ENC_1_EC_METHOD encoders[i].set_ec_method(I2CPE_ENC_1_EC_METHOD); #endif - #if defined(I2CPE_ENC_1_EC_THRESH) + #ifdef I2CPE_ENC_1_EC_THRESH encoders[i].set_ec_threshold(I2CPE_ENC_1_EC_THRESH); #endif encoders[i].set_active(encoders[i].passes_test(true)); - #if (I2CPE_ENC_1_AXIS == E_AXIS) + #if I2CPE_ENC_1_AXIS == E_AXIS encoders[i].set_homed(); #endif #endif @@ -525,28 +520,28 @@ encoders[i].init(I2CPE_ENC_2_ADDR, I2CPE_ENC_2_AXIS); - #if defined(I2CPE_ENC_2_TYPE) + #ifdef I2CPE_ENC_2_TYPE encoders[i].set_type(I2CPE_ENC_2_TYPE); #endif - #if defined(I2CPE_ENC_2_TICKS_UNIT) + #ifdef I2CPE_ENC_2_TICKS_UNIT encoders[i].set_ticks_unit(I2CPE_ENC_2_TICKS_UNIT); #endif - #if defined(I2CPE_ENC_2_TICKS_REV) + #ifdef I2CPE_ENC_2_TICKS_REV encoders[i].set_stepper_ticks(I2CPE_ENC_2_TICKS_REV); #endif - #if defined(I2CPE_ENC_2_INVERT) + #ifdef I2CPE_ENC_2_INVERT encoders[i].set_inverted(I2CPE_ENC_2_INVERT); #endif - #if defined(I2CPE_ENC_2_EC_METHOD) + #ifdef I2CPE_ENC_2_EC_METHOD encoders[i].set_ec_method(I2CPE_ENC_2_EC_METHOD); #endif - #if defined(I2CPE_ENC_2_EC_THRESH) + #ifdef I2CPE_ENC_2_EC_THRESH encoders[i].set_ec_threshold(I2CPE_ENC_2_EC_THRESH); #endif encoders[i].set_active(encoders[i].passes_test(true)); - #if (I2CPE_ENC_2_AXIS == E_AXIS) + #if I2CPE_ENC_2_AXIS == E_AXIS encoders[i].set_homed(); #endif #endif @@ -556,28 +551,28 @@ encoders[i].init(I2CPE_ENC_3_ADDR, I2CPE_ENC_3_AXIS); - #if defined(I2CPE_ENC_3_TYPE) + #ifdef I2CPE_ENC_3_TYPE encoders[i].set_type(I2CPE_ENC_3_TYPE); #endif - #if defined(I2CPE_ENC_3_TICKS_UNIT) + #ifdef I2CPE_ENC_3_TICKS_UNIT encoders[i].set_ticks_unit(I2CPE_ENC_3_TICKS_UNIT); #endif - #if defined(I2CPE_ENC_3_TICKS_REV) + #ifdef I2CPE_ENC_3_TICKS_REV encoders[i].set_stepper_ticks(I2CPE_ENC_3_TICKS_REV); #endif - #if defined(I2CPE_ENC_3_INVERT) + #ifdef I2CPE_ENC_3_INVERT encoders[i].set_inverted(I2CPE_ENC_3_INVERT); #endif - #if defined(I2CPE_ENC_3_EC_METHOD) + #ifdef I2CPE_ENC_3_EC_METHOD encoders[i].set_ec_method(I2CPE_ENC_3_EC_METHOD); #endif - #if defined(I2CPE_ENC_3_EC_THRESH) + #ifdef I2CPE_ENC_3_EC_THRESH encoders[i].set_ec_threshold(I2CPE_ENC_3_EC_THRESH); #endif encoders[i].set_active(encoders[i].passes_test(true)); - #if (I2CPE_ENC_3_AXIS == E_AXIS) + #if I2CPE_ENC_3_AXIS == E_AXIS encoders[i].set_homed(); #endif #endif @@ -587,28 +582,28 @@ encoders[i].init(I2CPE_ENC_4_ADDR, I2CPE_ENC_4_AXIS); - #if defined(I2CPE_ENC_4_TYPE) + #ifdef I2CPE_ENC_4_TYPE encoders[i].set_type(I2CPE_ENC_4_TYPE); #endif - #if defined(I2CPE_ENC_4_TICKS_UNIT) + #ifdef I2CPE_ENC_4_TICKS_UNIT encoders[i].set_ticks_unit(I2CPE_ENC_4_TICKS_UNIT); #endif - #if defined(I2CPE_ENC_4_TICKS_REV) + #ifdef I2CPE_ENC_4_TICKS_REV encoders[i].set_stepper_ticks(I2CPE_ENC_4_TICKS_REV); #endif - #if defined(I2CPE_ENC_4_INVERT) + #ifdef I2CPE_ENC_4_INVERT encoders[i].set_inverted(I2CPE_ENC_4_INVERT); #endif - #if defined(I2CPE_ENC_4_EC_METHOD) + #ifdef I2CPE_ENC_4_EC_METHOD encoders[i].set_ec_method(I2CPE_ENC_4_EC_METHOD); #endif - #if defined(I2CPE_ENC_4_EC_THRESH) + #ifdef I2CPE_ENC_4_EC_THRESH encoders[i].set_ec_threshold(I2CPE_ENC_4_EC_THRESH); #endif encoders[i].set_active(encoders[i].passes_test(true)); - #if (I2CPE_ENC_4_AXIS == E_AXIS) + #if I2CPE_ENC_4_AXIS == E_AXIS encoders[i].set_homed(); #endif #endif @@ -618,56 +613,57 @@ encoders[i].init(I2CPE_ENC_5_ADDR, I2CPE_ENC_5_AXIS); - #if defined(I2CPE_ENC_5_TYPE) + #ifdef I2CPE_ENC_5_TYPE encoders[i].set_type(I2CPE_ENC_5_TYPE); #endif - #if defined(I2CPE_ENC_5_TICKS_UNIT) + #ifdef I2CPE_ENC_5_TICKS_UNIT encoders[i].set_ticks_unit(I2CPE_ENC_5_TICKS_UNIT); #endif - #if defined(I2CPE_ENC_5_TICKS_REV) + #ifdef I2CPE_ENC_5_TICKS_REV encoders[i].set_stepper_ticks(I2CPE_ENC_5_TICKS_REV); #endif - #if defined(I2CPE_ENC_5_INVERT) + #ifdef I2CPE_ENC_5_INVERT encoders[i].set_inverted(I2CPE_ENC_5_INVERT); #endif - #if defined(I2CPE_ENC_5_EC_METHOD) + #ifdef I2CPE_ENC_5_EC_METHOD encoders[i].set_ec_method(I2CPE_ENC_5_EC_METHOD); #endif - #if defined(I2CPE_ENC_5_EC_THRESH) + #ifdef I2CPE_ENC_5_EC_THRESH encoders[i].set_ec_threshold(I2CPE_ENC_5_EC_THRESH); #endif encoders[i].set_active(encoders[i].passes_test(true)); - #if (I2CPE_ENC_5_AXIS == E_AXIS) + #if I2CPE_ENC_5_AXIS == E_AXIS encoders[i].set_homed(); #endif #endif - } - void I2CPositionEncodersMgr::report_position(uint8_t idx, bool units, bool noOffset) { - CHECK_IDX + void I2CPositionEncodersMgr::report_position(const int8_t idx, const bool units, const bool noOffset) { + CHECK_IDX(); - if (units) { + if (units) SERIAL_ECHOLN(noOffset ? encoders[idx].mm_from_count(encoders[idx].get_raw_count()) : encoders[idx].get_position_mm()); - } else { + else { if (noOffset) { - long raw_count = encoders[idx].get_raw_count(); + const int32_t raw_count = encoders[idx].get_raw_count(); SERIAL_ECHO(axis_codes[encoders[idx].get_axis()]); - SERIAL_ECHOPGM(" "); + SERIAL_CHAR(' '); for (uint8_t j = 31; j > 0; j--) SERIAL_ECHO((bool)(0x00000001 & (raw_count >> j))); - SERIAL_ECHO((bool)(0x00000001 & (raw_count))); - SERIAL_ECHOLNPAIR(" ", raw_count); - } else + SERIAL_ECHO((bool)(0x00000001 & raw_count)); + SERIAL_CHAR(' '); + SERIAL_ECHOLN(raw_count); + } + else SERIAL_ECHOLN(encoders[idx].get_position()); } } - void I2CPositionEncodersMgr::change_module_address(uint8_t oldaddr, uint8_t newaddr) { + void I2CPositionEncodersMgr::change_module_address(const uint8_t oldaddr, const uint8_t newaddr) { // First check 'new' address is not in use Wire.beginTransmission(newaddr); if (!Wire.endTransmission()) { @@ -709,7 +705,7 @@ // Now, if this module is configured, find which encoder instance it's supposed to correspond to // and enable it (it will likely have failed initialisation on power-up, before the address change). - int8_t idx = idx_from_addr(newaddr); + const int8_t idx = idx_from_addr(newaddr); if (idx >= 0 && !encoders[idx].get_active()) { SERIAL_ECHO(axis_codes[encoders[idx].get_axis()]); SERIAL_ECHOLNPGM(" axis encoder was not detected on printer startup. Trying again."); @@ -717,7 +713,7 @@ } } - void I2CPositionEncodersMgr::report_module_firmware(uint8_t address) { + void I2CPositionEncodersMgr::report_module_firmware(const uint8_t address) { // First check there is a module Wire.beginTransmission(address); if (Wire.endTransmission()) { @@ -727,7 +723,7 @@ } SERIAL_ECHOPAIR("Requesting version info from module at address ", address); - SERIAL_ECHOPGM(":\n"); + SERIAL_ECHOLNPGM(":"); Wire.beginTransmission(address); Wire.write(I2CPE_SET_REPORT_MODE); @@ -743,7 +739,7 @@ } // Set module back to normal (distance) mode - Wire.beginTransmission((int)address); + Wire.beginTransmission(address); Wire.write(I2CPE_SET_REPORT_MODE); Wire.write(I2CPE_REPORT_DISTANCE); Wire.endTransmission(); @@ -753,43 +749,43 @@ I2CPE_addr = 0; if (parser.seen('A')) { + if (!parser.has_value()) { SERIAL_PROTOCOLLNPGM("?A seen, but no address specified! [30-200]"); return I2CPE_PARSE_ERR; }; I2CPE_addr = parser.value_byte(); - if (!WITHIN(I2CPE_addr, 30, 200)) { // reserve the first 30 and last 55 SERIAL_PROTOCOLLNPGM("?Address out of range. [30-200]"); return I2CPE_PARSE_ERR; } I2CPE_idx = idx_from_addr(I2CPE_addr); - - if (!WITHIN(I2CPE_idx, 0, I2CPE_ENCODER_CNT - 1)) { + if (I2CPE_idx >= I2CPE_ENCODER_CNT) { SERIAL_PROTOCOLLNPGM("?No device with this address!"); return I2CPE_PARSE_ERR; } - } else if (parser.seenval('I')) { + } + else if (parser.seenval('I')) { + if (!parser.has_value()) { SERIAL_PROTOCOLLNPAIR("?I seen, but no index specified! [0-", I2CPE_ENCODER_CNT - 1); - SERIAL_ECHOLNPGM("]"); + SERIAL_PROTOCOLLNPGM("]"); return I2CPE_PARSE_ERR; }; I2CPE_idx = parser.value_byte(); - - if (!WITHIN(I2CPE_idx, 0, I2CPE_ENCODER_CNT - 1)) { + if (I2CPE_idx >= I2CPE_ENCODER_CNT) { SERIAL_PROTOCOLLNPAIR("?Index out of range. [0-", I2CPE_ENCODER_CNT - 1); SERIAL_ECHOLNPGM("]"); return I2CPE_PARSE_ERR; } I2CPE_addr = encoders[I2CPE_idx].get_address(); - } else { - I2CPE_idx = -1; } + else + I2CPE_idx = 0xFF; I2CPE_anyaxis = parser.seen_axis(); @@ -814,15 +810,18 @@ void I2CPositionEncodersMgr::M860() { if (parse()) return; - bool hasU = parser.seen('U'), hasO = parser.seen('O'); + const bool hasU = parser.seen('U'), hasO = parser.seen('O'); - if (I2CPE_idx < 0) { - int8_t idx; + if (I2CPE_idx == 0xFF) { LOOP_XYZE(i) { - if ((!I2CPE_anyaxis || parser.seen(axis_codes[i])) && ((idx = idx_from_axis(AxisEnum(i))) >= 0)) - report_position((uint8_t)idx, hasU, hasO); + if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) { + const uint8_t idx = idx_from_axis(AxisEnum(i)); + if ((int8_t)idx >= 0) report_position(idx, hasU, hasO); + } } - } else report_position((uint8_t)I2CPE_idx, hasU, hasO); + } + else + report_position(I2CPE_idx, hasU, hasO); } /** @@ -841,13 +840,16 @@ void I2CPositionEncodersMgr::M861() { if (parse()) return; - if (I2CPE_idx < 0) { - int8_t idx; + if (I2CPE_idx == 0xFF) { LOOP_XYZE(i) { - if ((!I2CPE_anyaxis || parser.seen(axis_codes[i])) && ((idx = idx_from_axis(AxisEnum(i))) >= 0)) - report_status((uint8_t)idx); + if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) { + const uint8_t idx = idx_from_axis(AxisEnum(i)); + if ((int8_t)idx >= 0) report_status(idx); + } } - } else report_status((uint8_t)I2CPE_idx); + } + else + report_status(I2CPE_idx); } /** @@ -867,13 +869,16 @@ void I2CPositionEncodersMgr::M862() { if (parse()) return; - if (I2CPE_idx < 0) { - int8_t idx; + if (I2CPE_idx == 0xFF) { LOOP_XYZE(i) { - if ((!I2CPE_anyaxis || parser.seen(axis_codes[i])) && ((idx = idx_from_axis(AxisEnum(i))) >= 0)) - test_axis((uint8_t)idx); + if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) { + const uint8_t idx = idx_from_axis(AxisEnum(i)); + if ((int8_t)idx >= 0) test_axis(idx); + } } - } else test_axis((uint8_t)I2CPE_idx); + } + else + test_axis(I2CPE_idx); } /** @@ -894,15 +899,18 @@ void I2CPositionEncodersMgr::M863() { if (parse()) return; - int iterations = parser.seenval('P') ? constrain(parser.value_byte(), 1, 10) : 1; + const uint8_t iterations = constrain(parser.byteval('P', 1), 1, 10); - if (I2CPE_idx < 0) { - int8_t idx; + if (I2CPE_idx == 0xFF) { LOOP_XYZE(i) { - if ((!I2CPE_anyaxis || parser.seen(axis_codes[i])) && ((idx = idx_from_axis(AxisEnum(i))) >= 0)) - calibrate_steps_mm((uint8_t)idx, iterations); + if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) { + const uint8_t idx = idx_from_axis(AxisEnum(i)); + if ((int8_t)idx >= 0) calibrate_steps_mm(idx, iterations); + } } - } else calibrate_steps_mm((uint8_t)I2CPE_idx, iterations); + } + else + calibrate_steps_mm(I2CPE_idx, iterations); } /** @@ -910,9 +918,9 @@ * * A Module current/old I2C address. If not present, * assumes default address (030). [30, 200]. - * N Module new I2C address. [30, 200]. + * S Module new I2C address. [30, 200]. * - * If N not specified: + * If S is not specified: * X Use I2CPE_PRESET_ADDR_X (030). * Y Use I2CPE_PRESET_ADDR_Y (031). * Z Use I2CPE_PRESET_ADDR_Z (032). @@ -925,23 +933,24 @@ if (!I2CPE_addr) I2CPE_addr = I2CPE_PRESET_ADDR_X; - if (parser.seen('N')) { + if (parser.seen('S')) { if (!parser.has_value()) { - SERIAL_PROTOCOLLNPGM("?N seen, but no address specified! [30-200]"); + SERIAL_PROTOCOLLNPGM("?S seen, but no address specified! [30-200]"); return; }; newAddress = parser.value_byte(); - if (!WITHIN(newAddress, 30, 200)) { SERIAL_PROTOCOLLNPGM("?New address out of range. [30-200]"); return; } - } else if (!I2CPE_anyaxis) { - SERIAL_PROTOCOLLNPGM("?You must specify N or [XYZE]."); + } + else if (!I2CPE_anyaxis) { + SERIAL_PROTOCOLLNPGM("?You must specify S or [XYZE]."); return; - } else { - if (parser.seen('X')) newAddress = I2CPE_PRESET_ADDR_X; + } + else { + if (parser.seen('X')) newAddress = I2CPE_PRESET_ADDR_X; else if (parser.seen('Y')) newAddress = I2CPE_PRESET_ADDR_Y; else if (parser.seen('Z')) newAddress = I2CPE_PRESET_ADDR_Z; else if (parser.seen('E')) newAddress = I2CPE_PRESET_ADDR_E; @@ -970,12 +979,15 @@ if (parse()) return; if (!I2CPE_addr) { - int8_t idx; LOOP_XYZE(i) { - if ((!I2CPE_anyaxis || parser.seen(axis_codes[i])) && ((idx = idx_from_axis(AxisEnum(i))) >= 0)) - report_module_firmware(encoders[idx].get_address()); + if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) { + const uint8_t idx = idx_from_axis(AxisEnum(i)); + if ((int8_t)idx >= 0) report_module_firmware(encoders[idx].get_address()); + } } - } else report_module_firmware(I2CPE_addr); + } + else + report_module_firmware(I2CPE_addr); } /** @@ -995,20 +1007,25 @@ void I2CPositionEncodersMgr::M866() { if (parse()) return; - bool hasR = parser.seen('R'); + const bool hasR = parser.seen('R'); - if (I2CPE_idx < 0) { - int8_t idx; + if (I2CPE_idx == 0xFF) { LOOP_XYZE(i) { - if ((!I2CPE_anyaxis || parser.seen(axis_codes[i])) && ((idx = idx_from_axis(AxisEnum(i))) >= 0)) { - if (hasR) reset_error_count((uint8_t)idx, AxisEnum(i)); - else report_error_count((uint8_t)idx, AxisEnum(i)); + if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) { + const uint8_t idx = idx_from_axis(AxisEnum(i)); + if ((int8_t)idx >= 0) { + if (hasR) + reset_error_count(idx, AxisEnum(i)); + else + report_error_count(idx, AxisEnum(i)); + } } } - } else { - if (hasR) reset_error_count((uint8_t)I2CPE_idx, encoders[I2CPE_idx].get_axis()); - else report_error_count((uint8_t)I2CPE_idx, encoders[I2CPE_idx].get_axis()); } + else if (hasR) + reset_error_count(I2CPE_idx, encoders[I2CPE_idx].get_axis()); + else + report_error_count(I2CPE_idx, encoders[I2CPE_idx].get_axis()); } /** @@ -1028,19 +1045,22 @@ void I2CPositionEncodersMgr::M867() { if (parse()) return; - int8_t onoff = parser.seenval('S') ? parser.value_int() : -1; + const int8_t onoff = parser.seenval('S') ? parser.value_int() : -1; - if (I2CPE_idx < 0) { - int8_t idx; + if (I2CPE_idx == 0xFF) { LOOP_XYZE(i) { - if ((!I2CPE_anyaxis || parser.seen(axis_codes[i])) && ((idx = idx_from_axis(AxisEnum(i))) >= 0)) { - if (onoff == -1) enable_ec((uint8_t)idx, !encoders[idx].get_ec_enabled(), AxisEnum(i)); - else enable_ec((uint8_t)idx, (bool)onoff, AxisEnum(i)); + if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) { + const uint8_t idx = idx_from_axis(AxisEnum(i)); + if ((int8_t)idx >= 0) { + const bool ena = onoff == -1 ? !encoders[I2CPE_idx].get_ec_enabled() : !!onoff; + enable_ec(idx, ena, AxisEnum(i)); + } } } - } else { - if (onoff == -1) enable_ec((uint8_t)I2CPE_idx, !encoders[I2CPE_idx].get_ec_enabled(), encoders[I2CPE_idx].get_axis()); - else enable_ec((uint8_t)I2CPE_idx, (bool)onoff, encoders[I2CPE_idx].get_axis()); + } + else { + const bool ena = onoff == -1 ? !encoders[I2CPE_idx].get_ec_enabled() : !!onoff; + enable_ec(I2CPE_idx, ena, encoders[I2CPE_idx].get_axis()); } } @@ -1061,20 +1081,25 @@ void I2CPositionEncodersMgr::M868() { if (parse()) return; - float newThreshold = parser.seenval('T') ? parser.value_float() : -9999; + const float newThreshold = parser.seenval('T') ? parser.value_float() : -9999; - if (I2CPE_idx < 0) { - int8_t idx; + if (I2CPE_idx == 0xFF) { LOOP_XYZE(i) { - if ((!I2CPE_anyaxis || parser.seen(axis_codes[i])) && ((idx = idx_from_axis(AxisEnum(i))) >= 0)) { - if (newThreshold != -9999) set_ec_threshold((uint8_t)idx, newThreshold, encoders[idx].get_axis()); - else get_ec_threshold((uint8_t)idx, encoders[idx].get_axis()); + if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) { + const uint8_t idx = idx_from_axis(AxisEnum(i)); + if ((int8_t)idx >= 0) { + if (newThreshold != -9999) + set_ec_threshold(idx, newThreshold, encoders[idx].get_axis()); + else + get_ec_threshold(idx, encoders[idx].get_axis()); + } } } - } else { - if (newThreshold != -9999) set_ec_threshold((uint8_t)I2CPE_idx, newThreshold, encoders[I2CPE_idx].get_axis()); - else get_ec_threshold((uint8_t)I2CPE_idx, encoders[I2CPE_idx].get_axis()); } + else if (newThreshold != -9999) + set_ec_threshold(I2CPE_idx, newThreshold, encoders[I2CPE_idx].get_axis()); + else + get_ec_threshold(I2CPE_idx, encoders[I2CPE_idx].get_axis()); } /** @@ -1092,13 +1117,16 @@ void I2CPositionEncodersMgr::M869() { if (parse()) return; - if (I2CPE_idx < 0) { - int8_t idx; + if (I2CPE_idx == 0xFF) { LOOP_XYZE(i) { - if ((!I2CPE_anyaxis || parser.seen(axis_codes[i])) && ((idx = idx_from_axis(AxisEnum(i))) >= 0)) - report_error((uint8_t)idx); + if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) { + const uint8_t idx = idx_from_axis(AxisEnum(i)); + if ((int8_t)idx >= 0) report_error(idx); + } } - } else report_error((uint8_t)I2CPE_idx); + } + else + report_error(I2CPE_idx); } -#endif +#endif // I2C_POSITION_ENCODERS diff --git a/Marlin/I2CPositionEncoder.h b/Marlin/I2CPositionEncoder.h index fe0be390a5..a582a87b6d 100644 --- a/Marlin/I2CPositionEncoder.h +++ b/Marlin/I2CPositionEncoder.h @@ -95,195 +95,202 @@ #define I2CPE_PARSE_OK 0 #define LOOP_PE(VAR) LOOP_L_N(VAR, I2CPE_ENCODER_CNT) - #define CHECK_IDX if (!WITHIN(idx, 0, I2CPE_ENCODER_CNT - 1)) return; + #define CHECK_IDX() do{ if (!WITHIN(idx, 0, I2CPE_ENCODER_CNT - 1)) return; }while(0) extern const char axis_codes[XYZE]; typedef union { - volatile long val = 0; - uint8_t bval[4]; + volatile int32_t val = 0; + uint8_t bval[4]; } i2cLong; class I2CPositionEncoder { private: - AxisEnum encoderAxis = I2CPE_DEF_AXIS; + AxisEnum encoderAxis = I2CPE_DEF_AXIS; - uint8_t i2cAddress = I2CPE_DEF_ADDR, - ecMethod = I2CPE_DEF_EC_METHOD, - type = I2CPE_DEF_TYPE, - H = I2CPE_MAG_SIG_NF; // Magnetic field strength + uint8_t i2cAddress = I2CPE_DEF_ADDR, + ecMethod = I2CPE_DEF_EC_METHOD, + type = I2CPE_DEF_TYPE, + H = I2CPE_MAG_SIG_NF; // Magnetic field strength - int encoderTicksPerUnit = I2CPE_DEF_ENC_TICKS_UNIT, - stepperTicks = I2CPE_DEF_TICKS_REV; + int encoderTicksPerUnit = I2CPE_DEF_ENC_TICKS_UNIT, + stepperTicks = I2CPE_DEF_TICKS_REV, + errorCount = 0, + errorPrev = 0; - float ecThreshold = I2CPE_DEF_EC_THRESH; + float ecThreshold = I2CPE_DEF_EC_THRESH; - bool homed = false, - trusted = false, - initialised = false, - active = false, - invert = false, - ec = true; + bool homed = false, + trusted = false, + initialised = false, + active = false, + invert = false, + ec = true; - int errorCount = 0, - errorPrev = 0; + float axisOffset = 0; - float axisOffset = 0; + int32_t axisOffsetTicks = 0, + zeroOffset = 0, + lastPosition = 0, + position; - long axisOffsetTicks = 0, - zeroOffset = 0, - lastPosition = 0, - position; - - unsigned long lastPositionTime = 0, - nextErrorCountTime = 0, - lastErrorTime; + millis_t lastPositionTime = 0, + nextErrorCountTime = 0, + lastErrorTime; //double positionMm; //calculate #if ENABLED(I2CPE_ERR_ROLLING_AVERAGE) - uint8_t errIdx = 0; - int err[I2CPE_ERR_ARRAY_SIZE] = {0}; + uint8_t errIdx = 0; + int err[I2CPE_ERR_ARRAY_SIZE] = { 0 }; #endif + //float positionMm; //calculate + public: - void init(uint8_t address, AxisEnum axis); + void init(const uint8_t address, const AxisEnum axis); void reset(); void update(); void set_homed(); - long get_raw_count(); + int32_t get_raw_count(); - FORCE_INLINE double mm_from_count(long count) { - if (type == I2CPE_ENC_TYPE_LINEAR) return count / encoderTicksPerUnit; - else if (type == I2CPE_ENC_TYPE_ROTARY) - return (count * stepperTicks) / (encoderTicksPerUnit * planner.axis_steps_per_mm[encoderAxis]); - return -1; + FORCE_INLINE float mm_from_count(const int32_t count) { + switch (type) { + default: return -1; + case I2CPE_ENC_TYPE_LINEAR: + return count / encoderTicksPerUnit; + case I2CPE_ENC_TYPE_ROTARY: + return (count * stepperTicks) / (encoderTicksPerUnit * planner.axis_steps_per_mm[encoderAxis]); + } } - FORCE_INLINE double get_position_mm() { return mm_from_count(get_position()); } - FORCE_INLINE long get_position() { return get_raw_count() - zeroOffset - axisOffsetTicks; } + FORCE_INLINE float get_position_mm() { return mm_from_count(get_position()); } + FORCE_INLINE int32_t get_position() { return get_raw_count() - zeroOffset - axisOffsetTicks; } - long get_axis_error_steps(bool report); - double get_axis_error_mm(bool report); + int32_t get_axis_error_steps(const bool report); + float get_axis_error_mm(const bool report); - void calibrate_steps_mm(int iter); + void calibrate_steps_mm(const uint8_t iter); - bool passes_test(bool report); + bool passes_test(const bool report); bool test_axis(void); FORCE_INLINE int get_error_count(void) { return errorCount; } - FORCE_INLINE void set_error_count(int newCount) { errorCount = newCount; } + FORCE_INLINE void set_error_count(const int newCount) { errorCount = newCount; } FORCE_INLINE uint8_t get_address() { return i2cAddress; } - FORCE_INLINE void set_address(uint8_t addr) { i2cAddress = addr; } + FORCE_INLINE void set_address(const uint8_t addr) { i2cAddress = addr; } FORCE_INLINE bool get_active(void) { return active; } - FORCE_INLINE void set_active(bool a) { active = a; } + FORCE_INLINE void set_active(const bool a) { active = a; } - FORCE_INLINE void set_inverted(bool i) { invert = i; } + FORCE_INLINE void set_inverted(const bool i) { invert = i; } FORCE_INLINE AxisEnum get_axis() { return encoderAxis; } FORCE_INLINE bool get_ec_enabled() { return ec; } - FORCE_INLINE void set_ec_enabled(bool enabled) { ec = enabled; } + FORCE_INLINE void set_ec_enabled(const bool enabled) { ec = enabled; } FORCE_INLINE uint8_t get_ec_method() { return ecMethod; } - FORCE_INLINE void set_ec_method(byte method) { ecMethod = method; } + FORCE_INLINE void set_ec_method(const byte method) { ecMethod = method; } FORCE_INLINE float get_ec_threshold() { return ecThreshold; } - FORCE_INLINE void set_ec_threshold(float newThreshold) { ecThreshold = newThreshold; } + FORCE_INLINE void set_ec_threshold(const float newThreshold) { ecThreshold = newThreshold; } FORCE_INLINE int get_encoder_ticks_mm() { - if (type == I2CPE_ENC_TYPE_LINEAR) return encoderTicksPerUnit; - else if (type == I2CPE_ENC_TYPE_ROTARY) - return (int)((encoderTicksPerUnit / stepperTicks) * planner.axis_steps_per_mm[encoderAxis]); - return 0; + switch (type) { + default: return 0; + case I2CPE_ENC_TYPE_LINEAR: + return encoderTicksPerUnit; + case I2CPE_ENC_TYPE_ROTARY: + return (int)((encoderTicksPerUnit / stepperTicks) * planner.axis_steps_per_mm[encoderAxis]); + } } FORCE_INLINE int get_ticks_unit() { return encoderTicksPerUnit; } - FORCE_INLINE void set_ticks_unit(int ticks) { encoderTicksPerUnit = ticks; } + FORCE_INLINE void set_ticks_unit(const int ticks) { encoderTicksPerUnit = ticks; } FORCE_INLINE uint8_t get_type() { return type; } - FORCE_INLINE void set_type(byte newType) { type = newType; } + FORCE_INLINE void set_type(const byte newType) { type = newType; } FORCE_INLINE int get_stepper_ticks() { return stepperTicks; } - FORCE_INLINE void set_stepper_ticks(int ticks) { stepperTicks = ticks; } + FORCE_INLINE void set_stepper_ticks(const int ticks) { stepperTicks = ticks; } FORCE_INLINE float get_axis_offset() { return axisOffset; } - FORCE_INLINE void set_axis_offset(float newOffset) { + FORCE_INLINE void set_axis_offset(const float newOffset) { axisOffset = newOffset; - axisOffsetTicks = (long)(axisOffset * get_encoder_ticks_mm()); + axisOffsetTicks = int32_t(axisOffset * get_encoder_ticks_mm()); } - FORCE_INLINE void set_current_position(float newPositionMm) { + FORCE_INLINE void set_current_position(const float newPositionMm) { set_axis_offset(get_position_mm() - newPositionMm + axisOffset); } }; class I2CPositionEncodersMgr { private: - bool I2CPE_anyaxis; - uint8_t I2CPE_addr; - int8_t I2CPE_idx; + static bool I2CPE_anyaxis; + static uint8_t I2CPE_addr, I2CPE_idx; public: - void init(void); + + static void init(void); // consider only updating one endoder per call / tick if encoders become too time intensive - void update(void) { LOOP_PE(i) encoders[i].update(); } + static void update(void) { LOOP_PE(i) encoders[i].update(); } - void homed(AxisEnum axis) { + static void homed(const AxisEnum axis) { LOOP_PE(i) if (encoders[i].get_axis() == axis) encoders[i].set_homed(); } - void report_position(uint8_t idx, bool units, bool noOffset); + static void report_position(const int8_t idx, const bool units, const bool noOffset); - void report_status(uint8_t idx) { - CHECK_IDX + static void report_status(const int8_t idx) { + CHECK_IDX(); SERIAL_ECHOPAIR("Encoder ",idx); SERIAL_ECHOPGM(": "); encoders[idx].get_raw_count(); encoders[idx].passes_test(true); } - void report_error(uint8_t idx) { - CHECK_IDX + static void report_error(const int8_t idx) { + CHECK_IDX(); encoders[idx].get_axis_error_steps(true); } - void test_axis(uint8_t idx) { - CHECK_IDX + static void test_axis(const int8_t idx) { + CHECK_IDX(); encoders[idx].test_axis(); } - void calibrate_steps_mm(uint8_t idx, int iterations) { - CHECK_IDX + static void calibrate_steps_mm(const int8_t idx, const int iterations) { + CHECK_IDX(); encoders[idx].calibrate_steps_mm(iterations); } - void change_module_address(uint8_t oldaddr, uint8_t newaddr); - void report_module_firmware(uint8_t address); + static void change_module_address(const uint8_t oldaddr, const uint8_t newaddr); + static void report_module_firmware(const uint8_t address); - void report_error_count(uint8_t idx, AxisEnum axis) { - CHECK_IDX + static void report_error_count(const int8_t idx, const AxisEnum axis) { + CHECK_IDX(); SERIAL_ECHOPAIR("Error count on ", axis_codes[axis]); SERIAL_ECHOLNPAIR(" axis is ", encoders[idx].get_error_count()); } - void reset_error_count(uint8_t idx, AxisEnum axis) { - CHECK_IDX + static void reset_error_count(const int8_t idx, const AxisEnum axis) { + CHECK_IDX(); encoders[idx].set_error_count(0); SERIAL_ECHOPAIR("Error count on ", axis_codes[axis]); SERIAL_ECHOLNPGM(" axis has been reset."); } - void enable_ec(uint8_t idx, bool enabled, AxisEnum axis) { - CHECK_IDX + static void enable_ec(const int8_t idx, const bool enabled, const AxisEnum axis) { + CHECK_IDX(); encoders[idx].set_ec_enabled(enabled); SERIAL_ECHOPAIR("Error correction on ", axis_codes[axis]); SERIAL_ECHOPGM(" axis is "); @@ -291,66 +298,62 @@ SERIAL_ECHOLNPGM("abled."); } - void set_ec_threshold(uint8_t idx, float newThreshold, AxisEnum axis) { - CHECK_IDX + static void set_ec_threshold(const int8_t idx, const float newThreshold, const AxisEnum axis) { + CHECK_IDX(); encoders[idx].set_ec_threshold(newThreshold); SERIAL_ECHOPAIR("Error correct threshold for ", axis_codes[axis]); SERIAL_ECHOPAIR_F(" axis set to ", newThreshold); SERIAL_ECHOLNPGM("mm."); } - void get_ec_threshold(uint8_t idx, AxisEnum axis) { - CHECK_IDX - float threshold = encoders[idx].get_ec_threshold(); + static void get_ec_threshold(const int8_t idx, const AxisEnum axis) { + CHECK_IDX(); + const float threshold = encoders[idx].get_ec_threshold(); SERIAL_ECHOPAIR("Error correct threshold for ", axis_codes[axis]); SERIAL_ECHOPAIR_F(" axis is ", threshold); SERIAL_ECHOLNPGM("mm."); } - int8_t idx_from_axis(AxisEnum axis) { + static int8_t idx_from_axis(const AxisEnum axis) { LOOP_PE(i) if (encoders[i].get_axis() == axis) return i; - return -1; } - int8_t idx_from_addr(uint8_t addr) { + static int8_t idx_from_addr(const uint8_t addr) { LOOP_PE(i) if (encoders[i].get_address() == addr) return i; - return -1; } - int8_t parse(); + static int8_t parse(); - void M860(); - void M861(); - void M862(); - void M863(); - void M864(); - void M865(); - void M866(); - void M867(); - void M868(); - void M869(); + static void M860(); + static void M861(); + static void M862(); + static void M863(); + static void M864(); + static void M865(); + static void M866(); + static void M867(); + static void M868(); + static void M869(); - I2CPositionEncoder encoders[I2CPE_ENCODER_CNT]; + static I2CPositionEncoder encoders[I2CPE_ENCODER_CNT]; }; extern I2CPositionEncodersMgr I2CPEM; - FORCE_INLINE void gcode_M860() { I2CPEM.M860(); } - FORCE_INLINE void gcode_M861() { I2CPEM.M861(); } - FORCE_INLINE void gcode_M862() { I2CPEM.M862(); } - FORCE_INLINE void gcode_M863() { I2CPEM.M863(); } - FORCE_INLINE void gcode_M864() { I2CPEM.M864(); } - FORCE_INLINE void gcode_M865() { I2CPEM.M865(); } - FORCE_INLINE void gcode_M866() { I2CPEM.M866(); } - FORCE_INLINE void gcode_M867() { I2CPEM.M867(); } - FORCE_INLINE void gcode_M868() { I2CPEM.M868(); } - FORCE_INLINE void gcode_M869() { I2CPEM.M869(); } + FORCE_INLINE static void gcode_M860() { I2CPEM.M860(); } + FORCE_INLINE static void gcode_M861() { I2CPEM.M861(); } + FORCE_INLINE static void gcode_M862() { I2CPEM.M862(); } + FORCE_INLINE static void gcode_M863() { I2CPEM.M863(); } + FORCE_INLINE static void gcode_M864() { I2CPEM.M864(); } + FORCE_INLINE static void gcode_M865() { I2CPEM.M865(); } + FORCE_INLINE static void gcode_M866() { I2CPEM.M866(); } + FORCE_INLINE static void gcode_M867() { I2CPEM.M867(); } + FORCE_INLINE static void gcode_M868() { I2CPEM.M868(); } + FORCE_INLINE static void gcode_M869() { I2CPEM.M869(); } #endif //I2C_POSITION_ENCODERS #endif //I2CPOSENC_H - - diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index 208abc5eb3..b6dd4c2c8c 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -5141,7 +5141,7 @@ void home_all_axes() { gcode_G28(true); } * T Don't calibrate tower angle corrections * * Cn.nn Calibration precision; when omitted calibrates to maximum precision - * + * * Fn Force to run at least n iterations and takes the best result * * Vn Verbose level: @@ -5687,7 +5687,7 @@ inline void gcode_G92() { update_software_endstops((AxisEnum)i); #if ENABLED(I2C_POSITION_ENCODERS) - I2CPEM.encoders[I2CPEM.idx_from_axis((AxisEnum) i)].set_axis_offset(position_shift[i]); + I2CPEM.encoders[I2CPEM.idx_from_axis((AxisEnum)i)].set_axis_offset(position_shift[i]); #endif #endif diff --git a/Marlin/SanityCheck.h b/Marlin/SanityCheck.h index d2bcda6580..73d0fc57b2 100644 --- a/Marlin/SanityCheck.h +++ b/Marlin/SanityCheck.h @@ -276,9 +276,7 @@ #if ENABLED(I2C_POSITION_ENCODERS) #if DISABLED(BABYSTEPPING) #error "I2C_POSITION_ENCODERS requires BABYSTEPPING." - #endif - - #if I2CPE_ENCODER_CNT > 5 || I2CPE_ENCODER_CNT < 1 + #elif !WITHIN(I2CPE_ENCODER_CNT, 1, 5) #error "I2CPE_ENCODER_CNT must be between 1 and 5." #endif #endif diff --git a/Marlin/ubl_G29.cpp b/Marlin/ubl_G29.cpp index 294e3616a3..f77f3746a0 100644 --- a/Marlin/ubl_G29.cpp +++ b/Marlin/ubl_G29.cpp @@ -328,7 +328,8 @@ g29_repetition_cnt = parser.has_value() ? parser.value_int() : 1; if (g29_repetition_cnt >= GRID_MAX_POINTS) { set_all_mesh_points_to_value(NAN); - } else { + } + else { while (g29_repetition_cnt--) { if (cnt > 20) { cnt = 0; idle(); } const mesh_index_pair location = find_closest_mesh_point_of_type(REAL, g29_x_pos, g29_y_pos, USE_NOZZLE_AS_REFERENCE, NULL, false); @@ -1454,7 +1455,7 @@ void unified_bed_leveling::fine_tune_mesh(const float &lx, const float &ly, const bool do_ubl_mesh_map) { if (!parser.seen('R')) // fine_tune_mesh() is special. If no repetition count flag is specified g29_repetition_cnt = 1; // do exactly one mesh location. Otherwise use what the parser decided. - + #if ENABLED(UBL_MESH_EDIT_MOVES_Z) const bool is_offset = parser.seen('H'); const float h_offset = is_offset ? parser.value_linear_units() : Z_CLEARANCE_BETWEEN_PROBES; @@ -1463,7 +1464,7 @@ return; } #endif - + mesh_index_pair location; if (!position_is_reachable_xy(lx, ly)) {