Followup for I2C_POSITION_ENCODERS
This commit is contained in:
Scott Lahteine
parent
f4246dc8ff
commit
fc140c4962
@ -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
|
||||
|
||||
@ -1271,6 +1271,7 @@
|
||||
//===========================================================================
|
||||
//====================== I2C Position Encoder Settings ======================
|
||||
//===========================================================================
|
||||
|
||||
/**
|
||||
* I2C position encoders for closed loop control.
|
||||
* Developed by Chris Barr at Aus3D.
|
||||
|
||||
@ -40,13 +40,14 @@
|
||||
|
||||
#include <Wire.h>
|
||||
|
||||
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<addr> Module current/old I2C address. If not present,
|
||||
* 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).
|
||||
* 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
|
||||
|
||||
@ -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
|
||||
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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)) {
|
||||
|
||||
Loading…
Reference in New Issue
Block a user