andrey
/
Marlin_FB4S
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Followup for I2C_POSITION_ENCODERS

pull/1/head
Scott Lahteine 8 years ago
parent
f4246dc8ff
commit
fc140c4962
7 changed files with 352 additions and 321 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      .travis.yml
  2. 1
      Marlin/Configuration_adv.h
  3. 420
      Marlin/I2CPositionEncoder.cpp
  4. 235
      Marlin/I2CPositionEncoder.h
  5. 4
      Marlin/Marlin_main.cpp
  6. 4
      Marlin/SanityCheck.h
  7. 7
      Marlin/ubl_G29.cpp

2
.travis.yml
View File

@ -93,7 +93,7 @@ script:
# #
- restore_configs - 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 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 - build_marlin
# #
# Test a Sled Z Probe # Test a Sled Z Probe

1
Marlin/Configuration_adv.h
View File

@ -1271,6 +1271,7 @@
//=========================================================================== //===========================================================================
//====================== I2C Position Encoder Settings ====================== //====================== I2C Position Encoder Settings ======================
//=========================================================================== //===========================================================================
/** /**
* I2C position encoders for closed loop control. * I2C position encoders for closed loop control.
* Developed by Chris Barr at Aus3D. * Developed by Chris Barr at Aus3D.

420
Marlin/I2CPositionEncoder.cpp
View File

@ -40,13 +40,14 @@
#include <Wire.h> #include <Wire.h>
void I2CPositionEncoder::init(uint8_t address, AxisEnum axis) {
void I2CPositionEncoder::init(const uint8_t address, const AxisEnum axis) {
encoderAxis = axis; encoderAxis = axis;
i2cAddress = address; i2cAddress = address;
initialised++; initialised++;
SERIAL_ECHOPAIR("Seetting up encoder on ", axis_codes[encoderAxis]); SERIAL_ECHOPAIR("Setting up encoder on ", axis_codes[encoderAxis]);
SERIAL_ECHOLNPAIR(" axis, addr = ", address); SERIAL_ECHOLNPAIR(" axis, addr = ", address);
position = get_position(); 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 //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 //idea of where it the axis is to re-initialise
double position = stepper.get_axis_position_mm(encoderAxis); float position = stepper.get_axis_position_mm(encoderAxis);
long positionInTicks = position * get_ticks_unit(); int32_t positionInTicks = position * get_ticks_unit();
//shift position from previous to current position //shift position from previous to current position
zeroOffset -= (positionInTicks - get_position()); zeroOffset -= (positionInTicks - get_position());
#if defined(I2CPE_DEBUG) #ifdef I2CPE_DEBUG
SERIAL_ECHOPGM("Current position is "); SERIAL_ECHOPGM("Current position is ");
SERIAL_ECHOLN(position); SERIAL_ECHOLN(position);
@ -126,23 +127,23 @@
} }
lastPosition = position; lastPosition = position;
millis_t positionTime = millis(); const millis_t positionTime = millis();
//only do error correction if setup and enabled //only do error correction if setup and enabled
if (ec && ecMethod != I2CPE_ECM_NONE) { if (ec && ecMethod != I2CPE_ECM_NONE) {
#if defined(I2CPE_EC_THRESH_PROPORTIONAL) #ifdef I2CPE_EC_THRESH_PROPORTIONAL
millis_t deltaTime = positionTime - lastPositionTime; const millis_t deltaTime = positionTime - lastPositionTime;
unsigned long distance = abs(position - lastPosition); const uint32_t distance = abs(position - lastPosition),
unsigned long speed = distance / deltaTime; speed = distance / deltaTime;
float threshold = constrain(speed / 50, 1, 50) * ecThreshold; const float threshold = constrain((speed / 50), 1, 50) * ecThreshold;
#else #else
float threshold = get_error_correct_threshold(); const float threshold = get_error_correct_threshold();
#endif #endif
//check error //check error
#if ENABLED(I2CPE_ERR_ROLLING_AVERAGE) #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; errIdx = (errIdx >= I2CPE_ERR_ARRAY_SIZE - 1) ? 0 : errIdx + 1;
err[errIdx] = get_axis_error_steps(false); err[errIdx] = get_axis_error_steps(false);
@ -152,16 +153,16 @@
if (i) diffSum += abs(err[i-1] - err[i]); 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 #else
long error = get_axis_error_steps(false); const int32_t error = get_axis_error_steps(false);
#endif #endif
//SERIAL_ECHOPGM("Axis err*r steps: "); //SERIAL_ECHOPGM("Axis error steps: ");
//SERIAL_ECHOLN(error); //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]) { if (labs(error) > I2CPE_ERR_THRESH_ABORT * planner.axis_steps_per_mm[encoderAxis]) {
//kill("Significant Error"); //kill("Significant Error");
SERIAL_ECHOPGM("Axis error greater than set threshold, aborting!"); SERIAL_ECHOPGM("Axis error greater than set threshold, aborting!");
@ -215,7 +216,7 @@
homed++; homed++;
trusted++; trusted++;
#if defined(I2CPE_DEBUG) #ifdef I2CPE_DEBUG
SERIAL_ECHO(axis_codes[encoderAxis]); SERIAL_ECHO(axis_codes[encoderAxis]);
SERIAL_ECHOPAIR(" axis encoder homed, offset of ", zeroOffset); SERIAL_ECHOPAIR(" axis encoder homed, offset of ", zeroOffset);
SERIAL_ECHOLNPGM(" ticks."); SERIAL_ECHOLNPGM(" ticks.");
@ -223,36 +224,27 @@
} }
} }
bool I2CPositionEncoder::passes_test(bool report) { bool I2CPositionEncoder::passes_test(const 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;
}
if (report) { if (report) {
SERIAL_ECHOPAIR("Warning, ", axis_codes[encoderAxis]); if (H != I2CPE_MAG_SIG_GOOD) SERIAL_ECHOPGM("Warning. ");
SERIAL_ECHOLNPGM(" axis encoder not detected!"); 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) { float I2CPositionEncoder::get_axis_error_mm(const bool report) {
double target, actual, error; float target, actual, error;
target = stepper.get_axis_position_mm(encoderAxis); target = stepper.get_axis_position_mm(encoderAxis);
actual = mm_from_count(position); actual = mm_from_count(position);
@ -270,7 +262,7 @@
return error; return error;
} }
long I2CPositionEncoder::get_axis_error_steps(bool report) { int32_t I2CPositionEncoder::get_axis_error_steps(const bool report) {
if (!active) { if (!active) {
if (report) { if (report) {
SERIAL_ECHO(axis_codes[encoderAxis]); SERIAL_ECHO(axis_codes[encoderAxis]);
@ -280,8 +272,8 @@
} }
float stepperTicksPerUnit; float stepperTicksPerUnit;
long encoderTicks = position, encoderCountInStepperTicksScaled; int32_t encoderTicks = position, encoderCountInStepperTicksScaled;
//long stepperTicks = stepper.position(encoderAxis); //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 // 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]; stepperTicksPerUnit = (type == I2CPE_ENC_TYPE_ROTARY) ? stepperTicks : planner.axis_steps_per_mm[encoderAxis];
@ -289,8 +281,8 @@
//convert both 'ticks' into same units / base //convert both 'ticks' into same units / base
encoderCountInStepperTicksScaled = LROUND((stepperTicksPerUnit * encoderTicks) / encoderTicksPerUnit); encoderCountInStepperTicksScaled = LROUND((stepperTicksPerUnit * encoderTicks) / encoderTicksPerUnit);
long target = stepper.position(encoderAxis), int32_t target = stepper.position(encoderAxis),
error = (encoderCountInStepperTicksScaled - target); error = (encoderCountInStepperTicksScaled - target);
//suppress discontinuities (might be caused by bad I2C readings...?) //suppress discontinuities (might be caused by bad I2C readings...?)
bool suppressOutput = (labs(error - errorPrev) > 100); bool suppressOutput = (labs(error - errorPrev) > 100);
@ -309,7 +301,7 @@
return (suppressOutput ? 0 : error); return (suppressOutput ? 0 : error);
} }
long I2CPositionEncoder::get_raw_count() { int32_t I2CPositionEncoder::get_raw_count() {
uint8_t index = 0; uint8_t index = 0;
i2cLong encoderCount; i2cLong encoderCount;
@ -340,14 +332,11 @@
//only works on XYZ cartesian machines for the time being //only works on XYZ cartesian machines for the time being
if (!(encoderAxis == X_AXIS || encoderAxis == Y_AXIS || encoderAxis == Z_AXIS)) return false; if (!(encoderAxis == X_AXIS || encoderAxis == Y_AXIS || encoderAxis == Z_AXIS)) return false;
int feedrate; float startCoord[NUM_AXIS] = { 0 }, endCoord[NUM_AXIS] = { 0 };
float startPosition, endPosition;
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; 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 the module isn't currently trusted, wait until it is (or until it should be if things are working)
if (!trusted) { if (!trusted) {
long startWaitingTime = millis(); int32_t startWaitingTime = millis();
while (!trusted && millis() - startWaitingTime < I2CPE_TIME_TRUSTED) while (!trusted && millis() - startWaitingTime < I2CPE_TIME_TRUSTED)
safe_delay(500); safe_delay(500);
} }
@ -381,7 +370,7 @@
return trusted; return trusted;
} }
void I2CPositionEncoder::calibrate_steps_mm(int iter) { void I2CPositionEncoder::calibrate_steps_mm(const uint8_t iter) {
if (type != I2CPE_ENC_TYPE_LINEAR) { if (type != I2CPE_ENC_TYPE_LINEAR) {
SERIAL_ECHOLNPGM("Steps per mm calibration is only available using linear encoders."); SERIAL_ECHOLNPGM("Steps per mm calibration is only available using linear encoders.");
return; return;
@ -392,14 +381,14 @@
return; return;
} }
float oldStepsMm, newStepsMm, float old_steps_mm, new_steps_mm,
startDistance, endDistance, startDistance, endDistance,
travelDistance, travelledDistance, total = 0, 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); feedrate = MMM_TO_MMS((encoderAxis == Z_AXIS) ? HOMING_FEEDRATE_Z : HOMING_FEEDRATE_XY);
@ -447,17 +436,17 @@
SERIAL_ECHOLNPGM("mm."); SERIAL_ECHOLNPGM("mm.");
//Calculate new axis steps per unit //Calculate new axis steps per unit
oldStepsMm = planner.axis_steps_per_mm[encoderAxis]; old_steps_mm = planner.axis_steps_per_mm[encoderAxis];
newStepsMm = (oldStepsMm * travelDistance) / travelledDistance; new_steps_mm = (old_steps_mm * travelDistance) / travelledDistance;
SERIAL_ECHOLNPAIR("Old steps per mm: ", oldStepsMm); SERIAL_ECHOLNPAIR("Old steps per mm: ", old_steps_mm);
SERIAL_ECHOLNPAIR("New steps per mm: ", newStepsMm); SERIAL_ECHOLNPAIR("New steps per mm: ", new_steps_mm);
//Save new value //Save new value
planner.axis_steps_per_mm[encoderAxis] = newStepsMm; planner.axis_steps_per_mm[encoderAxis] = new_steps_mm;
if (iter > 1) { if (iter > 1) {
total += newStepsMm; total += new_steps_mm;
// swap start and end points so next loop runs from current position // swap start and end points so next loop runs from current position
float tempCoord = startCoord[encoderAxis]; float tempCoord = startCoord[encoderAxis];
@ -486,6 +475,12 @@
#endif #endif
} }
bool I2CPositionEncodersMgr::I2CPE_anyaxis;
uint8_t I2CPositionEncodersMgr::I2CPE_addr,
I2CPositionEncodersMgr::I2CPE_idx;
I2CPositionEncoder I2CPositionEncodersMgr::encoders[I2CPE_ENCODER_CNT];
void I2CPositionEncodersMgr::init() { void I2CPositionEncodersMgr::init() {
Wire.begin(); Wire.begin();
@ -494,28 +489,28 @@
encoders[i].init(I2CPE_ENC_1_ADDR, I2CPE_ENC_1_AXIS); 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); encoders[i].set_type(I2CPE_ENC_1_TYPE);
#endif #endif
#if defined(I2CPE_ENC_1_TICKS_UNIT) #ifdef I2CPE_ENC_1_TICKS_UNIT
encoders[i].set_ticks_unit(I2CPE_ENC_1_TICKS_UNIT); encoders[i].set_ticks_unit(I2CPE_ENC_1_TICKS_UNIT);
#endif #endif
#if defined(I2CPE_ENC_1_TICKS_REV) #ifdef I2CPE_ENC_1_TICKS_REV
encoders[i].set_stepper_ticks(I2CPE_ENC_1_TICKS_REV); encoders[i].set_stepper_ticks(I2CPE_ENC_1_TICKS_REV);
#endif #endif
#if defined(I2CPE_ENC_1_INVERT) #ifdef I2CPE_ENC_1_INVERT
encoders[i].set_inverted(I2CPE_ENC_1_INVERT); encoders[i].set_inverted(I2CPE_ENC_1_INVERT);
#endif #endif
#if defined(I2CPE_ENC_1_EC_METHOD) #ifdef I2CPE_ENC_1_EC_METHOD
encoders[i].set_ec_method(I2CPE_ENC_1_EC_METHOD); encoders[i].set_ec_method(I2CPE_ENC_1_EC_METHOD);
#endif #endif
#if defined(I2CPE_ENC_1_EC_THRESH) #ifdef I2CPE_ENC_1_EC_THRESH
encoders[i].set_ec_threshold(I2CPE_ENC_1_EC_THRESH); encoders[i].set_ec_threshold(I2CPE_ENC_1_EC_THRESH);
#endif #endif
encoders[i].set_active(encoders[i].passes_test(true)); 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(); encoders[i].set_homed();
#endif #endif
#endif #endif
@ -525,28 +520,28 @@
encoders[i].init(I2CPE_ENC_2_ADDR, I2CPE_ENC_2_AXIS); 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); encoders[i].set_type(I2CPE_ENC_2_TYPE);
#endif #endif
#if defined(I2CPE_ENC_2_TICKS_UNIT) #ifdef I2CPE_ENC_2_TICKS_UNIT
encoders[i].set_ticks_unit(I2CPE_ENC_2_TICKS_UNIT); encoders[i].set_ticks_unit(I2CPE_ENC_2_TICKS_UNIT);
#endif #endif
#if defined(I2CPE_ENC_2_TICKS_REV) #ifdef I2CPE_ENC_2_TICKS_REV
encoders[i].set_stepper_ticks(I2CPE_ENC_2_TICKS_REV); encoders[i].set_stepper_ticks(I2CPE_ENC_2_TICKS_REV);
#endif #endif
#if defined(I2CPE_ENC_2_INVERT) #ifdef I2CPE_ENC_2_INVERT
encoders[i].set_inverted(I2CPE_ENC_2_INVERT); encoders[i].set_inverted(I2CPE_ENC_2_INVERT);
#endif #endif
#if defined(I2CPE_ENC_2_EC_METHOD) #ifdef I2CPE_ENC_2_EC_METHOD
encoders[i].set_ec_method(I2CPE_ENC_2_EC_METHOD); encoders[i].set_ec_method(I2CPE_ENC_2_EC_METHOD);
#endif #endif
#if defined(I2CPE_ENC_2_EC_THRESH) #ifdef I2CPE_ENC_2_EC_THRESH
encoders[i].set_ec_threshold(I2CPE_ENC_2_EC_THRESH); encoders[i].set_ec_threshold(I2CPE_ENC_2_EC_THRESH);
#endif #endif
encoders[i].set_active(encoders[i].passes_test(true)); 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(); encoders[i].set_homed();
#endif #endif
#endif #endif
@ -556,28 +551,28 @@
encoders[i].init(I2CPE_ENC_3_ADDR, I2CPE_ENC_3_AXIS); 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); encoders[i].set_type(I2CPE_ENC_3_TYPE);
#endif #endif
#if defined(I2CPE_ENC_3_TICKS_UNIT) #ifdef I2CPE_ENC_3_TICKS_UNIT
encoders[i].set_ticks_unit(I2CPE_ENC_3_TICKS_UNIT); encoders[i].set_ticks_unit(I2CPE_ENC_3_TICKS_UNIT);
#endif #endif
#if defined(I2CPE_ENC_3_TICKS_REV) #ifdef I2CPE_ENC_3_TICKS_REV
encoders[i].set_stepper_ticks(I2CPE_ENC_3_TICKS_REV); encoders[i].set_stepper_ticks(I2CPE_ENC_3_TICKS_REV);
#endif #endif
#if defined(I2CPE_ENC_3_INVERT) #ifdef I2CPE_ENC_3_INVERT
encoders[i].set_inverted(I2CPE_ENC_3_INVERT); encoders[i].set_inverted(I2CPE_ENC_3_INVERT);
#endif #endif
#if defined(I2CPE_ENC_3_EC_METHOD) #ifdef I2CPE_ENC_3_EC_METHOD
encoders[i].set_ec_method(I2CPE_ENC_3_EC_METHOD); encoders[i].set_ec_method(I2CPE_ENC_3_EC_METHOD);
#endif #endif
#if defined(I2CPE_ENC_3_EC_THRESH) #ifdef I2CPE_ENC_3_EC_THRESH
encoders[i].set_ec_threshold(I2CPE_ENC_3_EC_THRESH); encoders[i].set_ec_threshold(I2CPE_ENC_3_EC_THRESH);
#endif #endif
encoders[i].set_active(encoders[i].passes_test(true)); 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(); encoders[i].set_homed();
#endif #endif
#endif #endif
@ -587,28 +582,28 @@
encoders[i].init(I2CPE_ENC_4_ADDR, I2CPE_ENC_4_AXIS); 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); encoders[i].set_type(I2CPE_ENC_4_TYPE);
#endif #endif
#if defined(I2CPE_ENC_4_TICKS_UNIT) #ifdef I2CPE_ENC_4_TICKS_UNIT
encoders[i].set_ticks_unit(I2CPE_ENC_4_TICKS_UNIT); encoders[i].set_ticks_unit(I2CPE_ENC_4_TICKS_UNIT);
#endif #endif
#if defined(I2CPE_ENC_4_TICKS_REV) #ifdef I2CPE_ENC_4_TICKS_REV
encoders[i].set_stepper_ticks(I2CPE_ENC_4_TICKS_REV); encoders[i].set_stepper_ticks(I2CPE_ENC_4_TICKS_REV);
#endif #endif
#if defined(I2CPE_ENC_4_INVERT) #ifdef I2CPE_ENC_4_INVERT
encoders[i].set_inverted(I2CPE_ENC_4_INVERT); encoders[i].set_inverted(I2CPE_ENC_4_INVERT);
#endif #endif
#if defined(I2CPE_ENC_4_EC_METHOD) #ifdef I2CPE_ENC_4_EC_METHOD
encoders[i].set_ec_method(I2CPE_ENC_4_EC_METHOD); encoders[i].set_ec_method(I2CPE_ENC_4_EC_METHOD);
#endif #endif
#if defined(I2CPE_ENC_4_EC_THRESH) #ifdef I2CPE_ENC_4_EC_THRESH
encoders[i].set_ec_threshold(I2CPE_ENC_4_EC_THRESH); encoders[i].set_ec_threshold(I2CPE_ENC_4_EC_THRESH);
#endif #endif
encoders[i].set_active(encoders[i].passes_test(true)); 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(); encoders[i].set_homed();
#endif #endif
#endif #endif
@ -618,56 +613,57 @@
encoders[i].init(I2CPE_ENC_5_ADDR, I2CPE_ENC_5_AXIS); 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); encoders[i].set_type(I2CPE_ENC_5_TYPE);
#endif #endif
#if defined(I2CPE_ENC_5_TICKS_UNIT) #ifdef I2CPE_ENC_5_TICKS_UNIT
encoders[i].set_ticks_unit(I2CPE_ENC_5_TICKS_UNIT); encoders[i].set_ticks_unit(I2CPE_ENC_5_TICKS_UNIT);
#endif #endif
#if defined(I2CPE_ENC_5_TICKS_REV) #ifdef I2CPE_ENC_5_TICKS_REV
encoders[i].set_stepper_ticks(I2CPE_ENC_5_TICKS_REV); encoders[i].set_stepper_ticks(I2CPE_ENC_5_TICKS_REV);
#endif #endif
#if defined(I2CPE_ENC_5_INVERT) #ifdef I2CPE_ENC_5_INVERT
encoders[i].set_inverted(I2CPE_ENC_5_INVERT); encoders[i].set_inverted(I2CPE_ENC_5_INVERT);
#endif #endif
#if defined(I2CPE_ENC_5_EC_METHOD) #ifdef I2CPE_ENC_5_EC_METHOD
encoders[i].set_ec_method(I2CPE_ENC_5_EC_METHOD); encoders[i].set_ec_method(I2CPE_ENC_5_EC_METHOD);
#endif #endif
#if defined(I2CPE_ENC_5_EC_THRESH) #ifdef I2CPE_ENC_5_EC_THRESH
encoders[i].set_ec_threshold(I2CPE_ENC_5_EC_THRESH); encoders[i].set_ec_threshold(I2CPE_ENC_5_EC_THRESH);
#endif #endif
encoders[i].set_active(encoders[i].passes_test(true)); 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(); encoders[i].set_homed();
#endif #endif
#endif #endif
} }
void I2CPositionEncodersMgr::report_position(uint8_t idx, bool units, bool noOffset) { void I2CPositionEncodersMgr::report_position(const int8_t idx, const bool units, const bool noOffset) {
CHECK_IDX CHECK_IDX();
if (units) { if (units)
SERIAL_ECHOLN(noOffset ? encoders[idx].mm_from_count(encoders[idx].get_raw_count()) : encoders[idx].get_position_mm()); SERIAL_ECHOLN(noOffset ? encoders[idx].mm_from_count(encoders[idx].get_raw_count()) : encoders[idx].get_position_mm());
} else { else {
if (noOffset) { 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_ECHO(axis_codes[encoders[idx].get_axis()]);
SERIAL_ECHOPGM(" "); SERIAL_CHAR(' ');
for (uint8_t j = 31; j > 0; j--) for (uint8_t j = 31; j > 0; j--)
SERIAL_ECHO((bool)(0x00000001 & (raw_count >> j))); SERIAL_ECHO((bool)(0x00000001 & (raw_count >> j)));
SERIAL_ECHO((bool)(0x00000001 & (raw_count))); SERIAL_ECHO((bool)(0x00000001 & raw_count));
SERIAL_ECHOLNPAIR(" ", raw_count); SERIAL_CHAR(' ');
} else SERIAL_ECHOLN(raw_count);
}
else
SERIAL_ECHOLN(encoders[idx].get_position()); 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 // First check 'new' address is not in use
Wire.beginTransmission(newaddr); Wire.beginTransmission(newaddr);
if (!Wire.endTransmission()) { if (!Wire.endTransmission()) {
@ -709,7 +705,7 @@
// Now, if this module is configured, find which encoder instance it's supposed to correspond to // 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). // 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()) { if (idx >= 0 && !encoders[idx].get_active()) {
SERIAL_ECHO(axis_codes[encoders[idx].get_axis()]); SERIAL_ECHO(axis_codes[encoders[idx].get_axis()]);
SERIAL_ECHOLNPGM(" axis encoder was not detected on printer startup. Trying again."); 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 // First check there is a module
Wire.beginTransmission(address); Wire.beginTransmission(address);
if (Wire.endTransmission()) { if (Wire.endTransmission()) {
@ -727,7 +723,7 @@
} }
SERIAL_ECHOPAIR("Requesting version info from module at address ", address); SERIAL_ECHOPAIR("Requesting version info from module at address ", address);
SERIAL_ECHOPGM(":\n"); SERIAL_ECHOLNPGM(":");
Wire.beginTransmission(address); Wire.beginTransmission(address);
Wire.write(I2CPE_SET_REPORT_MODE); Wire.write(I2CPE_SET_REPORT_MODE);
@ -743,7 +739,7 @@
} }
// Set module back to normal (distance) mode // Set module back to normal (distance) mode
Wire.beginTransmission((int)address); Wire.beginTransmission(address);
Wire.write(I2CPE_SET_REPORT_MODE); Wire.write(I2CPE_SET_REPORT_MODE);
Wire.write(I2CPE_REPORT_DISTANCE); Wire.write(I2CPE_REPORT_DISTANCE);
Wire.endTransmission(); Wire.endTransmission();
@ -753,43 +749,43 @@
I2CPE_addr = 0; I2CPE_addr = 0;
if (parser.seen('A')) { if (parser.seen('A')) {
if (!parser.has_value()) { if (!parser.has_value()) {
SERIAL_PROTOCOLLNPGM("?A seen, but no address specified! [30-200]"); SERIAL_PROTOCOLLNPGM("?A seen, but no address specified! [30-200]");
return I2CPE_PARSE_ERR; return I2CPE_PARSE_ERR;
}; };
I2CPE_addr = parser.value_byte(); I2CPE_addr = parser.value_byte();
if (!WITHIN(I2CPE_addr, 30, 200)) { // reserve the first 30 and last 55 if (!WITHIN(I2CPE_addr, 30, 200)) { // reserve the first 30 and last 55
SERIAL_PROTOCOLLNPGM("?Address out of range. [30-200]"); SERIAL_PROTOCOLLNPGM("?Address out of range. [30-200]");
return I2CPE_PARSE_ERR; return I2CPE_PARSE_ERR;
} }
I2CPE_idx = idx_from_addr(I2CPE_addr); I2CPE_idx = idx_from_addr(I2CPE_addr);
if (I2CPE_idx >= I2CPE_ENCODER_CNT) {
if (!WITHIN(I2CPE_idx, 0, I2CPE_ENCODER_CNT - 1)) {
SERIAL_PROTOCOLLNPGM("?No device with this address!"); SERIAL_PROTOCOLLNPGM("?No device with this address!");
return I2CPE_PARSE_ERR; return I2CPE_PARSE_ERR;
} }
} else if (parser.seenval('I')) { }
else if (parser.seenval('I')) {
if (!parser.has_value()) { if (!parser.has_value()) {
SERIAL_PROTOCOLLNPAIR("?I seen, but no index specified! [0-", I2CPE_ENCODER_CNT - 1); SERIAL_PROTOCOLLNPAIR("?I seen, but no index specified! [0-", I2CPE_ENCODER_CNT - 1);
SERIAL_ECHOLNPGM("]"); SERIAL_PROTOCOLLNPGM("]");
return I2CPE_PARSE_ERR; return I2CPE_PARSE_ERR;
}; };
I2CPE_idx = parser.value_byte(); I2CPE_idx = parser.value_byte();
if (I2CPE_idx >= I2CPE_ENCODER_CNT) {
if (!WITHIN(I2CPE_idx, 0, I2CPE_ENCODER_CNT - 1)) {
SERIAL_PROTOCOLLNPAIR("?Index out of range. [0-", I2CPE_ENCODER_CNT - 1); SERIAL_PROTOCOLLNPAIR("?Index out of range. [0-", I2CPE_ENCODER_CNT - 1);
SERIAL_ECHOLNPGM("]"); SERIAL_ECHOLNPGM("]");
return I2CPE_PARSE_ERR; return I2CPE_PARSE_ERR;
} }
I2CPE_addr = encoders[I2CPE_idx].get_address(); I2CPE_addr = encoders[I2CPE_idx].get_address();
} else {
I2CPE_idx = -1;
} }
else
I2CPE_idx = 0xFF;
I2CPE_anyaxis = parser.seen_axis(); I2CPE_anyaxis = parser.seen_axis();
@ -814,15 +810,18 @@
void I2CPositionEncodersMgr::M860() { void I2CPositionEncodersMgr::M860() {
if (parse()) return; 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) { if (I2CPE_idx == 0xFF) {
int8_t idx;
LOOP_XYZE(i) { LOOP_XYZE(i) {
if ((!I2CPE_anyaxis || parser.seen(axis_codes[i])) && ((idx = idx_from_axis(AxisEnum(i))) >= 0)) if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) {
report_position((uint8_t)idx, hasU, hasO); 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() { void I2CPositionEncodersMgr::M861() {
if (parse()) return; if (parse()) return;
if (I2CPE_idx < 0) { if (I2CPE_idx == 0xFF) {
int8_t idx;
LOOP_XYZE(i) { LOOP_XYZE(i) {
if ((!I2CPE_anyaxis || parser.seen(axis_codes[i])) && ((idx = idx_from_axis(AxisEnum(i))) >= 0)) if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) {
report_status((uint8_t)idx); 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() { void I2CPositionEncodersMgr::M862() {
if (parse()) return; if (parse()) return;
if (I2CPE_idx < 0) { if (I2CPE_idx == 0xFF) {
int8_t idx;
LOOP_XYZE(i) { LOOP_XYZE(i) {
if ((!I2CPE_anyaxis || parser.seen(axis_codes[i])) && ((idx = idx_from_axis(AxisEnum(i))) >= 0)) if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) {
test_axis((uint8_t)idx); 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() { void I2CPositionEncodersMgr::M863() {
if (parse()) return; 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) { if (I2CPE_idx == 0xFF) {
int8_t idx;
LOOP_XYZE(i) { LOOP_XYZE(i) {
if ((!I2CPE_anyaxis || parser.seen(axis_codes[i])) && ((idx = idx_from_axis(AxisEnum(i))) >= 0)) if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) {
calibrate_steps_mm((uint8_t)idx, iterations); 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<addr> Module current/old I2C address. If not present, * A<addr> Module current/old I2C address. If not present,
* assumes default address (030). [30, 200]. * assumes default address (030). [30, 200].
* N<addr> Module new I2C address. [30, 200]. * S<addr> Module new I2C address. [30, 200].
* *
* If N not specified: * If S is not specified:
* X Use I2CPE_PRESET_ADDR_X (030). * X Use I2CPE_PRESET_ADDR_X (030).
* Y Use I2CPE_PRESET_ADDR_Y (031). * Y Use I2CPE_PRESET_ADDR_Y (031).
* Z Use I2CPE_PRESET_ADDR_Z (032). * Z Use I2CPE_PRESET_ADDR_Z (032).
@ -925,23 +933,24 @@
if (!I2CPE_addr) I2CPE_addr = I2CPE_PRESET_ADDR_X; if (!I2CPE_addr) I2CPE_addr = I2CPE_PRESET_ADDR_X;
if (parser.seen('N')) { if (parser.seen('S')) {
if (!parser.has_value()) { 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; return;
}; };
newAddress = parser.value_byte(); newAddress = parser.value_byte();
if (!WITHIN(newAddress, 30, 200)) { if (!WITHIN(newAddress, 30, 200)) {
SERIAL_PROTOCOLLNPGM("?New address out of range. [30-200]"); SERIAL_PROTOCOLLNPGM("?New address out of range. [30-200]");
return; 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; 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('Y')) newAddress = I2CPE_PRESET_ADDR_Y;
else if (parser.seen('Z')) newAddress = I2CPE_PRESET_ADDR_Z; else if (parser.seen('Z')) newAddress = I2CPE_PRESET_ADDR_Z;
else if (parser.seen('E')) newAddress = I2CPE_PRESET_ADDR_E; else if (parser.seen('E')) newAddress = I2CPE_PRESET_ADDR_E;
@ -970,12 +979,15 @@
if (parse()) return; if (parse()) return;
if (!I2CPE_addr) { if (!I2CPE_addr) {
int8_t idx;
LOOP_XYZE(i) { LOOP_XYZE(i) {
if ((!I2CPE_anyaxis || parser.seen(axis_codes[i])) && ((idx = idx_from_axis(AxisEnum(i))) >= 0)) if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) {
report_module_firmware(encoders[idx].get_address()); 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() { void I2CPositionEncodersMgr::M866() {
if (parse()) return; if (parse()) return;
bool hasR = parser.seen('R'); const bool hasR = parser.seen('R');
if (I2CPE_idx < 0) { if (I2CPE_idx == 0xFF) {
int8_t idx;
LOOP_XYZE(i) { LOOP_XYZE(i) {
if ((!I2CPE_anyaxis || parser.seen(axis_codes[i])) && ((idx = idx_from_axis(AxisEnum(i))) >= 0)) { if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) {
if (hasR) reset_error_count((uint8_t)idx, AxisEnum(i)); const uint8_t idx = idx_from_axis(AxisEnum(i));
else report_error_count((uint8_t)idx, 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() { void I2CPositionEncodersMgr::M867() {
if (parse()) return; 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) { if (I2CPE_idx == 0xFF) {
int8_t idx;
LOOP_XYZE(i) { LOOP_XYZE(i) {
if ((!I2CPE_anyaxis || parser.seen(axis_codes[i])) && ((idx = idx_from_axis(AxisEnum(i))) >= 0)) { if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) {
if (onoff == -1) enable_ec((uint8_t)idx, !encoders[idx].get_ec_enabled(), AxisEnum(i)); const uint8_t idx = idx_from_axis(AxisEnum(i));
else enable_ec((uint8_t)idx, (bool)onoff, 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 {
else enable_ec((uint8_t)I2CPE_idx, (bool)onoff, encoders[I2CPE_idx].get_axis()); 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() { void I2CPositionEncodersMgr::M868() {
if (parse()) return; 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) { if (I2CPE_idx == 0xFF) {
int8_t idx;
LOOP_XYZE(i) { LOOP_XYZE(i) {
if ((!I2CPE_anyaxis || parser.seen(axis_codes[i])) && ((idx = idx_from_axis(AxisEnum(i))) >= 0)) { if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) {
if (newThreshold != -9999) set_ec_threshold((uint8_t)idx, newThreshold, encoders[idx].get_axis()); const uint8_t idx = idx_from_axis(AxisEnum(i));
else get_ec_threshold((uint8_t)idx, encoders[idx].get_axis()); 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() { void I2CPositionEncodersMgr::M869() {
if (parse()) return; if (parse()) return;
if (I2CPE_idx < 0) { if (I2CPE_idx == 0xFF) {
int8_t idx;
LOOP_XYZE(i) { LOOP_XYZE(i) {
if ((!I2CPE_anyaxis || parser.seen(axis_codes[i])) && ((idx = idx_from_axis(AxisEnum(i))) >= 0)) if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) {
report_error((uint8_t)idx); 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

235
Marlin/I2CPositionEncoder.h
View File

@ -95,195 +95,202 @@
#define I2CPE_PARSE_OK 0 #define I2CPE_PARSE_OK 0
#define LOOP_PE(VAR) LOOP_L_N(VAR, I2CPE_ENCODER_CNT) #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]; extern const char axis_codes[XYZE];
typedef union { typedef union {
volatile long val = 0; volatile int32_t val = 0;
uint8_t bval[4]; uint8_t bval[4];
} i2cLong; } i2cLong;
class I2CPositionEncoder { class I2CPositionEncoder {
private: private:
AxisEnum encoderAxis = I2CPE_DEF_AXIS; AxisEnum encoderAxis = I2CPE_DEF_AXIS;
uint8_t i2cAddress = I2CPE_DEF_ADDR, uint8_t i2cAddress = I2CPE_DEF_ADDR,
ecMethod = I2CPE_DEF_EC_METHOD, ecMethod = I2CPE_DEF_EC_METHOD,
type = I2CPE_DEF_TYPE, type = I2CPE_DEF_TYPE,
H = I2CPE_MAG_SIG_NF; // Magnetic field strength H = I2CPE_MAG_SIG_NF; // Magnetic field strength
int encoderTicksPerUnit = I2CPE_DEF_ENC_TICKS_UNIT, int encoderTicksPerUnit = I2CPE_DEF_ENC_TICKS_UNIT,
stepperTicks = I2CPE_DEF_TICKS_REV; stepperTicks = I2CPE_DEF_TICKS_REV,
errorCount = 0,
errorPrev = 0;
float ecThreshold = I2CPE_DEF_EC_THRESH; float ecThreshold = I2CPE_DEF_EC_THRESH;
bool homed = false, bool homed = false,
trusted = false, trusted = false,
initialised = false, initialised = false,
active = false, active = false,
invert = false, invert = false,
ec = true; ec = true;
int errorCount = 0, float axisOffset = 0;
errorPrev = 0;
float axisOffset = 0; int32_t axisOffsetTicks = 0,
zeroOffset = 0,
lastPosition = 0,
position;
long axisOffsetTicks = 0, millis_t lastPositionTime = 0,
zeroOffset = 0, nextErrorCountTime = 0,
lastPosition = 0, lastErrorTime;
position;
unsigned long lastPositionTime = 0,
nextErrorCountTime = 0,
lastErrorTime;
//double positionMm; //calculate //double positionMm; //calculate
#if ENABLED(I2CPE_ERR_ROLLING_AVERAGE) #if ENABLED(I2CPE_ERR_ROLLING_AVERAGE)
uint8_t errIdx = 0; uint8_t errIdx = 0;
int err[I2CPE_ERR_ARRAY_SIZE] = {0}; int err[I2CPE_ERR_ARRAY_SIZE] = { 0 };
#endif #endif
//float positionMm; //calculate
public: public:
void init(uint8_t address, AxisEnum axis); void init(const uint8_t address, const AxisEnum axis);
void reset(); void reset();
void update(); void update();
void set_homed(); void set_homed();
long get_raw_count(); int32_t get_raw_count();
FORCE_INLINE double mm_from_count(long count) { FORCE_INLINE float mm_from_count(const int32_t count) {
if (type == I2CPE_ENC_TYPE_LINEAR) return count / encoderTicksPerUnit; switch (type) {
else if (type == I2CPE_ENC_TYPE_ROTARY) default: return -1;
return (count * stepperTicks) / (encoderTicksPerUnit * planner.axis_steps_per_mm[encoderAxis]); case I2CPE_ENC_TYPE_LINEAR:
return -1; 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 float get_position_mm() { return mm_from_count(get_position()); }
FORCE_INLINE long get_position() { return get_raw_count() - zeroOffset - axisOffsetTicks; } FORCE_INLINE int32_t get_position() { return get_raw_count() - zeroOffset - axisOffsetTicks; }
long get_axis_error_steps(bool report); int32_t get_axis_error_steps(const bool report);
double get_axis_error_mm(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); bool test_axis(void);
FORCE_INLINE int get_error_count(void) { return errorCount; } 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 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 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 AxisEnum get_axis() { return encoderAxis; }
FORCE_INLINE bool get_ec_enabled() { return ec; } 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 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 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() { FORCE_INLINE int get_encoder_ticks_mm() {
if (type == I2CPE_ENC_TYPE_LINEAR) return encoderTicksPerUnit; switch (type) {
else if (type == I2CPE_ENC_TYPE_ROTARY) default: return 0;
return (int)((encoderTicksPerUnit / stepperTicks) * planner.axis_steps_per_mm[encoderAxis]); case I2CPE_ENC_TYPE_LINEAR:
return 0; 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 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 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 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 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; 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); set_axis_offset(get_position_mm() - newPositionMm + axisOffset);
} }
}; };
class I2CPositionEncodersMgr { class I2CPositionEncodersMgr {
private: private:
bool I2CPE_anyaxis; static bool I2CPE_anyaxis;
uint8_t I2CPE_addr; static uint8_t I2CPE_addr, I2CPE_idx;
int8_t I2CPE_idx;
public: public:
void init(void);
static void init(void);
// consider only updating one endoder per call / tick if encoders become too time intensive // 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) LOOP_PE(i)
if (encoders[i].get_axis() == axis) encoders[i].set_homed(); 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) { static void report_status(const int8_t idx) {
CHECK_IDX CHECK_IDX();
SERIAL_ECHOPAIR("Encoder ",idx); SERIAL_ECHOPAIR("Encoder ",idx);
SERIAL_ECHOPGM(": "); SERIAL_ECHOPGM(": ");
encoders[idx].get_raw_count(); encoders[idx].get_raw_count();
encoders[idx].passes_test(true); encoders[idx].passes_test(true);
} }
void report_error(uint8_t idx) { static void report_error(const int8_t idx) {
CHECK_IDX CHECK_IDX();
encoders[idx].get_axis_error_steps(true); encoders[idx].get_axis_error_steps(true);
} }
void test_axis(uint8_t idx) { static void test_axis(const int8_t idx) {
CHECK_IDX CHECK_IDX();
encoders[idx].test_axis(); encoders[idx].test_axis();
} }
void calibrate_steps_mm(uint8_t idx, int iterations) { static void calibrate_steps_mm(const int8_t idx, const int iterations) {
CHECK_IDX CHECK_IDX();
encoders[idx].calibrate_steps_mm(iterations); encoders[idx].calibrate_steps_mm(iterations);
} }
void change_module_address(uint8_t oldaddr, uint8_t newaddr); static void change_module_address(const uint8_t oldaddr, const uint8_t newaddr);
void report_module_firmware(uint8_t address); static void report_module_firmware(const uint8_t address);
void report_error_count(uint8_t idx, AxisEnum axis) { static void report_error_count(const int8_t idx, const AxisEnum axis) {
CHECK_IDX CHECK_IDX();
SERIAL_ECHOPAIR("Error count on ", axis_codes[axis]); SERIAL_ECHOPAIR("Error count on ", axis_codes[axis]);
SERIAL_ECHOLNPAIR(" axis is ", encoders[idx].get_error_count()); SERIAL_ECHOLNPAIR(" axis is ", encoders[idx].get_error_count());
} }
void reset_error_count(uint8_t idx, AxisEnum axis) { static void reset_error_count(const int8_t idx, const AxisEnum axis) {
CHECK_IDX CHECK_IDX();
encoders[idx].set_error_count(0); encoders[idx].set_error_count(0);
SERIAL_ECHOPAIR("Error count on ", axis_codes[axis]); SERIAL_ECHOPAIR("Error count on ", axis_codes[axis]);
SERIAL_ECHOLNPGM(" axis has been reset."); SERIAL_ECHOLNPGM(" axis has been reset.");
} }
void enable_ec(uint8_t idx, bool enabled, AxisEnum axis) { static void enable_ec(const int8_t idx, const bool enabled, const AxisEnum axis) {
CHECK_IDX CHECK_IDX();
encoders[idx].set_ec_enabled(enabled); encoders[idx].set_ec_enabled(enabled);
SERIAL_ECHOPAIR("Error correction on ", axis_codes[axis]); SERIAL_ECHOPAIR("Error correction on ", axis_codes[axis]);
SERIAL_ECHOPGM(" axis is "); SERIAL_ECHOPGM(" axis is ");
@ -291,66 +298,62 @@
SERIAL_ECHOLNPGM("abled."); SERIAL_ECHOLNPGM("abled.");
} }
void set_ec_threshold(uint8_t idx, float newThreshold, AxisEnum axis) { static void set_ec_threshold(const int8_t idx, const float newThreshold, const AxisEnum axis) {
CHECK_IDX CHECK_IDX();
encoders[idx].set_ec_threshold(newThreshold); encoders[idx].set_ec_threshold(newThreshold);
SERIAL_ECHOPAIR("Error correct threshold for ", axis_codes[axis]); SERIAL_ECHOPAIR("Error correct threshold for ", axis_codes[axis]);
SERIAL_ECHOPAIR_F(" axis set to ", newThreshold); SERIAL_ECHOPAIR_F(" axis set to ", newThreshold);
SERIAL_ECHOLNPGM("mm."); SERIAL_ECHOLNPGM("mm.");
} }
void get_ec_threshold(uint8_t idx, AxisEnum axis) { static void get_ec_threshold(const int8_t idx, const AxisEnum axis) {
CHECK_IDX CHECK_IDX();
float threshold = encoders[idx].get_ec_threshold(); const float threshold = encoders[idx].get_ec_threshold();
SERIAL_ECHOPAIR("Error correct threshold for ", axis_codes[axis]); SERIAL_ECHOPAIR("Error correct threshold for ", axis_codes[axis]);
SERIAL_ECHOPAIR_F(" axis is ", threshold); SERIAL_ECHOPAIR_F(" axis is ", threshold);
SERIAL_ECHOLNPGM("mm."); SERIAL_ECHOLNPGM("mm.");
} }
int8_t idx_from_axis(AxisEnum axis) { static int8_t idx_from_axis(const AxisEnum axis) {
LOOP_PE(i) LOOP_PE(i)
if (encoders[i].get_axis() == axis) return i; if (encoders[i].get_axis() == axis) return i;
return -1; return -1;
} }
int8_t idx_from_addr(uint8_t addr) { static int8_t idx_from_addr(const uint8_t addr) {
LOOP_PE(i) LOOP_PE(i)
if (encoders[i].get_address() == addr) return i; if (encoders[i].get_address() == addr) return i;
return -1; return -1;
} }
int8_t parse(); static int8_t parse();
void M860(); static void M860();
void M861(); static void M861();
void M862(); static void M862();
void M863(); static void M863();
void M864(); static void M864();
void M865(); static void M865();
void M866(); static void M866();
void M867(); static void M867();
void M868(); static void M868();
void M869(); static void M869();
I2CPositionEncoder encoders[I2CPE_ENCODER_CNT]; static I2CPositionEncoder encoders[I2CPE_ENCODER_CNT];
}; };
extern I2CPositionEncodersMgr I2CPEM; extern I2CPositionEncodersMgr I2CPEM;
FORCE_INLINE void gcode_M860() { I2CPEM.M860(); } FORCE_INLINE static void gcode_M860() { I2CPEM.M860(); }
FORCE_INLINE void gcode_M861() { I2CPEM.M861(); } FORCE_INLINE static void gcode_M861() { I2CPEM.M861(); }
FORCE_INLINE void gcode_M862() { I2CPEM.M862(); } FORCE_INLINE static void gcode_M862() { I2CPEM.M862(); }
FORCE_INLINE void gcode_M863() { I2CPEM.M863(); } FORCE_INLINE static void gcode_M863() { I2CPEM.M863(); }
FORCE_INLINE void gcode_M864() { I2CPEM.M864(); } FORCE_INLINE static void gcode_M864() { I2CPEM.M864(); }
FORCE_INLINE void gcode_M865() { I2CPEM.M865(); } FORCE_INLINE static void gcode_M865() { I2CPEM.M865(); }
FORCE_INLINE void gcode_M866() { I2CPEM.M866(); } FORCE_INLINE static void gcode_M866() { I2CPEM.M866(); }
FORCE_INLINE void gcode_M867() { I2CPEM.M867(); } FORCE_INLINE static void gcode_M867() { I2CPEM.M867(); }
FORCE_INLINE void gcode_M868() { I2CPEM.M868(); } FORCE_INLINE static void gcode_M868() { I2CPEM.M868(); }
FORCE_INLINE void gcode_M869() { I2CPEM.M869(); } FORCE_INLINE static void gcode_M869() { I2CPEM.M869(); }
#endif //I2C_POSITION_ENCODERS #endif //I2C_POSITION_ENCODERS
#endif //I2CPOSENC_H #endif //I2CPOSENC_H

4
Marlin/Marlin_main.cpp
View File

@ -5141,7 +5141,7 @@ void home_all_axes() { gcode_G28(true); }
* T Don't calibrate tower angle corrections * T Don't calibrate tower angle corrections
* *
* Cn.nn Calibration precision; when omitted calibrates to maximum precision * Cn.nn Calibration precision; when omitted calibrates to maximum precision
* *
* Fn Force to run at least n iterations and takes the best result * Fn Force to run at least n iterations and takes the best result
* *
* Vn Verbose level: * Vn Verbose level:
@ -5687,7 +5687,7 @@ inline void gcode_G92() {
update_software_endstops((AxisEnum)i); update_software_endstops((AxisEnum)i);
#if ENABLED(I2C_POSITION_ENCODERS) #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
#endif #endif

4
Marlin/SanityCheck.h
View File

@ -276,9 +276,7 @@
#if ENABLED(I2C_POSITION_ENCODERS) #if ENABLED(I2C_POSITION_ENCODERS)
#if DISABLED(BABYSTEPPING) #if DISABLED(BABYSTEPPING)
#error "I2C_POSITION_ENCODERS requires BABYSTEPPING." #error "I2C_POSITION_ENCODERS requires BABYSTEPPING."
#endif #elif !WITHIN(I2CPE_ENCODER_CNT, 1, 5)
#if I2CPE_ENCODER_CNT > 5 || I2CPE_ENCODER_CNT < 1
#error "I2CPE_ENCODER_CNT must be between 1 and 5." #error "I2CPE_ENCODER_CNT must be between 1 and 5."
#endif #endif
#endif #endif

7
Marlin/ubl_G29.cpp
View File

@ -328,7 +328,8 @@
g29_repetition_cnt = parser.has_value() ? parser.value_int() : 1; g29_repetition_cnt = parser.has_value() ? parser.value_int() : 1;
if (g29_repetition_cnt >= GRID_MAX_POINTS) { if (g29_repetition_cnt >= GRID_MAX_POINTS) {
set_all_mesh_points_to_value(NAN); set_all_mesh_points_to_value(NAN);
} else { }
else {
while (g29_repetition_cnt--) { while (g29_repetition_cnt--) {
if (cnt > 20) { cnt = 0; idle(); } 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); 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) { 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 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. g29_repetition_cnt = 1; // do exactly one mesh location. Otherwise use what the parser decided.
#if ENABLED(UBL_MESH_EDIT_MOVES_Z) #if ENABLED(UBL_MESH_EDIT_MOVES_Z)
const bool is_offset = parser.seen('H'); const bool is_offset = parser.seen('H');
const float h_offset = is_offset ? parser.value_linear_units() : Z_CLEARANCE_BETWEEN_PROBES; const float h_offset = is_offset ? parser.value_linear_units() : Z_CLEARANCE_BETWEEN_PROBES;
@ -1463,7 +1464,7 @@
return; return;
} }
#endif #endif
mesh_index_pair location; mesh_index_pair location;
if (!position_is_reachable_xy(lx, ly)) { if (!position_is_reachable_xy(lx, ly)) {

Write Preview
Loading…
Cancel
Save