bissen21/Marlin_Firmware
1
0
Fork 0
Code Issues Pull Requests Projects Releases 4 Wiki Activity

Followup for I2C_POSITION_ENCODERS

Browse Source
This commit is contained in:
Scott Lahteine
2017-06-09 10:22:49 -05:00
parent f4246dc8ff
commit fc140c4962
7 changed files with 352 additions and 321 deletions
Download Patch File Download Diff File Expand all files Collapse all files

420
Marlin/I2CPositionEncoder.cpp
View File

@ -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