2017-06-09 07:06:23 -05:00
/**
* Marlin 3 D Printer Firmware
2020-02-03 08:00:57 -06:00
* Copyright ( c ) 2020 MarlinFirmware [ https : //github.com/MarlinFirmware/Marlin]
2017-06-09 07:06:23 -05:00
*
* Based on Sprinter and grbl .
2019-06-27 23:57:50 -05:00
* Copyright ( c ) 2011 Camiel Gubbels / Erik van der Zalm
2017-06-09 07:06:23 -05:00
*
* This program is free software : you can redistribute it and / or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation , either version 3 of the License , or
* ( at your option ) any later version .
*
* This program is distributed in the hope that it will be useful ,
* but WITHOUT ANY WARRANTY ; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE . See the
* GNU General Public License for more details .
*
* You should have received a copy of the GNU General Public License
2020-07-22 22:20:14 -05:00
* along with this program . If not , see < https : //www.gnu.org/licenses/>.
2017-06-09 07:06:23 -05:00
*
*/
//todo: add support for multiple encoders on a single axis
//todo: add z axis auto-leveling
//todo: consolidate some of the related M codes?
//todo: add endstop-replacement mode?
//todo: try faster I2C speed; tweak TWI_FREQ (400000L, or faster?); or just TWBR = ((CPU_FREQ / 400000L) - 16) / 2;
//todo: consider Marlin-optimized Wire library; i.e. MarlinWire, like MarlinSerial
2017-09-06 06:28:32 -05:00
# include "../inc/MarlinConfig.h"
2017-06-09 07:06:23 -05:00
# if ENABLED(I2C_POSITION_ENCODERS)
2020-03-13 16:29:29 -05:00
# include "encoder_i2c.h"
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
# include "../module/stepper.h"
# include "../gcode/parser.h"
2017-06-09 07:06:23 -05:00
2019-04-06 18:04:34 -05:00
# include "../feature/babystep.h"
2017-09-06 06:28:32 -05:00
# include <Wire.h>
2017-06-09 10:22:49 -05:00
2021-01-21 03:40:07 -06:00
I2CPositionEncodersMgr I2CPEM ;
2017-09-06 06:28:32 -05:00
void I2CPositionEncoder : : init ( const uint8_t address , const AxisEnum axis ) {
encoderAxis = axis ;
i2cAddress = address ;
2017-06-09 07:06:23 -05:00
2021-03-18 15:59:48 -05:00
initialized = true ;
2017-06-09 07:06:23 -05:00
2021-02-08 00:37:24 -06:00
SERIAL_ECHOLNPAIR ( " Setting up encoder on " , AS_CHAR ( axis_codes [ encoderAxis ] ) , " axis, addr = " , address ) ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
position = get_position ( ) ;
}
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
void I2CPositionEncoder : : update ( ) {
2018-08-22 17:14:38 -05:00
if ( ! initialized | | ! homed | | ! active ) return ; //check encoder is set up and active
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
position = get_position ( ) ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
//we don't want to stop things just because the encoder missed a message,
//so we only care about responses that indicate bad magnetic strength
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
if ( ! passes_test ( false ) ) { //check encoder data is good
lastErrorTime = millis ( ) ;
/*
if ( trusted ) { //commented out as part of the note below
trusted = false ;
2021-02-08 00:37:24 -06:00
SERIAL_ECHOLNPAIR ( " Fault detected on " , AS_CHAR ( axis_codes [ encoderAxis ] ) , " axis encoder. Disengaging error correction until module is trusted again. " ) ;
2017-06-09 07:06:23 -05:00
}
2017-09-06 06:28:32 -05:00
*/
return ;
}
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
if ( ! trusted ) {
/**
* This is commented out because it introduces error and can cause bad print quality .
*
* This code is intended to manage situations where the encoder has reported bad magnetic strength .
* This indicates that the magnetic strip was too far away from the sensor to reliably track position .
* When this happens , this code resets the offset based on where the printer thinks it is . This has been
* shown to introduce errors in actual position which result in drifting prints and poor print quality .
* Perhaps a better method would be to disable correction on the axis with a problem , report it to the
* user via the status leds on the encoder module and prompt the user to re - home the axis at which point
* the encoder would be re - enabled .
*/
2021-01-08 15:07:35 -06:00
#if 0
2017-09-06 06:28:32 -05:00
// If the magnetic strength has been good for a certain time, start trusting the module again
if ( millis ( ) - lastErrorTime > I2CPE_TIME_TRUSTED ) {
trusted = true ;
2021-02-08 00:37:24 -06:00
SERIAL_ECHOLNPAIR ( " Untrusted encoder module on " , AS_CHAR ( axis_codes [ encoderAxis ] ) , " axis has been fault-free for set duration, reinstating error correction. " ) ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
//the encoder likely lost its place when the error occured, so we'll reset and use the printer's
2018-08-22 17:14:38 -05:00
//idea of where it the axis is to re-initialize
2018-05-12 09:59:11 -05:00
const float pos = planner . get_axis_position_mm ( encoderAxis ) ;
int32_t positionInTicks = pos * get_ticks_unit ( ) ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
//shift position from previous to current position
zeroOffset - = ( positionInTicks - get_position ( ) ) ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
# ifdef I2CPE_DEBUG
2018-11-29 16:58:58 -06:00
SERIAL_ECHOLNPAIR ( " Current position is " , pos ) ;
SERIAL_ECHOLNPAIR ( " Position in encoder ticks is " , positionInTicks ) ;
SERIAL_ECHOLNPAIR ( " New zero-offset of " , zeroOffset ) ;
SERIAL_ECHOPAIR ( " New position reads as " , get_position ( ) ) ;
2018-01-23 19:09:40 -06:00
SERIAL_CHAR ( ' ( ' ) ;
2020-06-22 21:12:45 -05:00
SERIAL_DECIMAL ( mm_from_count ( get_position ( ) ) ) ;
2017-09-06 06:28:32 -05:00
SERIAL_ECHOLNPGM ( " ) " ) ;
# endif
}
2021-01-08 15:07:35 -06:00
# endif
2017-09-06 06:28:32 -05:00
return ;
}
lastPosition = position ;
const millis_t positionTime = millis ( ) ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
//only do error correction if setup and enabled
if ( ec & & ecMethod ! = I2CPE_ECM_NONE ) {
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
# ifdef I2CPE_EC_THRESH_PROPORTIONAL
const millis_t deltaTime = positionTime - lastPositionTime ;
2018-05-13 01:10:34 -05:00
const uint32_t distance = ABS ( position - lastPosition ) ,
2017-09-06 06:28:32 -05:00
speed = distance / deltaTime ;
const float threshold = constrain ( ( speed / 50 ) , 1 , 50 ) * ecThreshold ;
# else
const float threshold = get_error_correct_threshold ( ) ;
# endif
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
//check error
# if ENABLED(I2CPE_ERR_ROLLING_AVERAGE)
float sum = 0 , diffSum = 0 ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
errIdx = ( errIdx > = I2CPE_ERR_ARRAY_SIZE - 1 ) ? 0 : errIdx + 1 ;
err [ errIdx ] = get_axis_error_steps ( false ) ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
LOOP_L_N ( i , I2CPE_ERR_ARRAY_SIZE ) {
sum + = err [ i ] ;
2018-05-13 01:10:34 -05:00
if ( i ) diffSum + = ABS ( err [ i - 1 ] - err [ i ] ) ;
2017-09-06 06:28:32 -05:00
}
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
const int32_t error = int32_t ( sum / ( I2CPE_ERR_ARRAY_SIZE + 1 ) ) ; //calculate average for error
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
# else
const int32_t error = get_axis_error_steps ( false ) ;
# endif
2017-06-09 07:06:23 -05:00
2018-11-29 16:58:58 -06:00
//SERIAL_ECHOLNPAIR("Axis error steps: ", error);
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
# ifdef I2CPE_ERR_THRESH_ABORT
2018-10-10 09:45:20 -05:00
if ( ABS ( error ) > I2CPE_ERR_THRESH_ABORT * planner . settings . axis_steps_per_mm [ encoderAxis ] ) {
2018-10-18 21:20:56 -05:00
//kill(PSTR("Significant Error"));
2019-02-12 16:21:44 -06:00
SERIAL_ECHOLNPAIR ( " Axis error over threshold, aborting! " , error ) ;
2017-09-06 06:28:32 -05:00
safe_delay ( 5000 ) ;
}
# endif
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
# if ENABLED(I2CPE_ERR_ROLLING_AVERAGE)
if ( errIdx = = 0 ) {
2018-02-08 03:57:11 -06:00
// In order to correct for "error" but avoid correcting for noise and non-skips
2017-09-06 06:28:32 -05:00
// it must be > threshold and have a difference average of < 10 and be < 2000 steps
2020-02-01 00:57:14 -06:00
if ( ABS ( error ) > threshold * planner . settings . axis_steps_per_mm [ encoderAxis ]
& & diffSum < 10 * ( I2CPE_ERR_ARRAY_SIZE - 1 )
& & ABS ( error ) < 2000
) { // Check for persistent error (skip)
2018-02-08 03:57:11 -06:00
errPrst [ errPrstIdx + + ] = error ; // Error must persist for I2CPE_ERR_PRST_ARRAY_SIZE error cycles. This also serves to improve the average accuracy
if ( errPrstIdx > = I2CPE_ERR_PRST_ARRAY_SIZE ) {
float sumP = 0 ;
2018-02-10 19:25:34 -06:00
LOOP_L_N ( i , I2CPE_ERR_PRST_ARRAY_SIZE ) sumP + = errPrst [ i ] ;
2019-09-14 03:05:10 -05:00
const int32_t errorP = int32_t ( sumP * RECIPROCAL ( I2CPE_ERR_PRST_ARRAY_SIZE ) ) ;
2021-02-04 19:18:31 -06:00
SERIAL_CHAR ( axis_codes [ encoderAxis ] ) ;
2020-02-01 00:57:14 -06:00
SERIAL_ECHOLNPAIR ( " : CORRECT ERR " , errorP * planner . steps_to_mm [ encoderAxis ] , " mm " ) ;
2019-04-06 18:04:34 -05:00
babystep . add_steps ( encoderAxis , - LROUND ( errorP ) ) ;
2018-02-08 03:57:11 -06:00
errPrstIdx = 0 ;
}
2017-06-19 22:39:23 -05:00
}
2018-02-08 03:57:11 -06:00
else
errPrstIdx = 0 ;
2017-06-09 07:06:23 -05:00
}
2017-09-06 06:28:32 -05:00
# else
2018-10-10 09:45:20 -05:00
if ( ABS ( error ) > threshold * planner . settings . axis_steps_per_mm [ encoderAxis ] ) {
2017-09-06 06:28:32 -05:00
//SERIAL_ECHOLN(error);
//SERIAL_ECHOLN(position);
2019-04-06 18:04:34 -05:00
babystep . add_steps ( encoderAxis , - LROUND ( error / 2 ) ) ;
2017-09-06 06:28:32 -05:00
}
# endif
2017-06-09 07:06:23 -05:00
2018-10-10 09:45:20 -05:00
if ( ABS ( error ) > I2CPE_ERR_CNT_THRESH * planner . settings . axis_steps_per_mm [ encoderAxis ] ) {
2017-09-06 06:28:32 -05:00
const millis_t ms = millis ( ) ;
if ( ELAPSED ( ms , nextErrorCountTime ) ) {
2021-02-04 19:18:31 -06:00
SERIAL_CHAR ( axis_codes [ encoderAxis ] ) ;
2021-02-08 00:37:24 -06:00
SERIAL_ECHOLNPAIR ( " : LARGE ERR " , error , " ; diffSum= " , diffSum ) ;
2017-09-06 06:28:32 -05:00
errorCount + + ;
nextErrorCountTime = ms + I2CPE_ERR_CNT_DEBOUNCE_MS ;
}
}
2017-06-09 07:06:23 -05:00
}
2017-09-06 06:28:32 -05:00
lastPositionTime = positionTime ;
}
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
void I2CPositionEncoder : : set_homed ( ) {
if ( active ) {
reset ( ) ; // Reset module's offset to zero (so current position is homed / zero)
delay ( 10 ) ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
zeroOffset = get_raw_count ( ) ;
2021-03-18 15:59:48 -05:00
homed = trusted = true ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
# ifdef I2CPE_DEBUG
2021-02-04 19:18:31 -06:00
SERIAL_CHAR ( axis_codes [ encoderAxis ] ) ;
2019-03-05 06:46:19 -06:00
SERIAL_ECHOLNPAIR ( " axis encoder homed, offset of " , zeroOffset , " ticks. " ) ;
2017-09-06 06:28:32 -05:00
# endif
}
}
2018-10-29 14:01:36 -05:00
void I2CPositionEncoder : : set_unhomed ( ) {
zeroOffset = 0 ;
homed = trusted = false ;
# ifdef I2CPE_DEBUG
2021-02-04 19:18:31 -06:00
SERIAL_CHAR ( axis_codes [ encoderAxis ] ) ;
2018-10-29 14:01:36 -05:00
SERIAL_ECHOLNPGM ( " axis encoder unhomed. " ) ;
# endif
}
2017-09-06 06:28:32 -05:00
bool I2CPositionEncoder : : passes_test ( const bool report ) {
if ( report ) {
if ( H ! = I2CPE_MAG_SIG_GOOD ) SERIAL_ECHOPGM ( " Warning. " ) ;
2021-02-04 19:18:31 -06:00
SERIAL_CHAR ( axis_codes [ encoderAxis ] ) ;
2019-04-05 20:02:46 -05:00
serial_ternary ( H = = I2CPE_MAG_SIG_BAD , PSTR ( " axis " ) , PSTR ( " magnetic strip " ) , PSTR ( " encoder " ) ) ;
2017-09-06 06:28:32 -05:00
switch ( H ) {
case I2CPE_MAG_SIG_GOOD :
case I2CPE_MAG_SIG_MID :
2019-09-08 00:55:34 -05:00
SERIAL_ECHO_TERNARY ( H = = I2CPE_MAG_SIG_GOOD , " passes test; field strength " , " good " , " fair " , " . \n " ) ;
2017-09-06 06:28:32 -05:00
break ;
default :
SERIAL_ECHOLNPGM ( " not detected! " ) ;
2017-06-09 07:06:23 -05:00
}
}
2017-09-06 06:28:32 -05:00
return ( H = = I2CPE_MAG_SIG_GOOD | | H = = I2CPE_MAG_SIG_MID ) ;
}
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
float I2CPositionEncoder : : get_axis_error_mm ( const bool report ) {
2020-02-01 00:57:14 -06:00
const float target = planner . get_axis_position_mm ( encoderAxis ) ,
actual = mm_from_count ( position ) ,
diff = actual - target ,
error = ABS ( diff ) > 10000 ? 0 : diff ; // Huge error is a bad reading
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
if ( report ) {
2021-02-04 19:18:31 -06:00
SERIAL_CHAR ( axis_codes [ encoderAxis ] ) ;
2020-02-01 00:57:14 -06:00
SERIAL_ECHOLNPAIR ( " axis target= " , target , " mm; actual= " , actual , " mm; err= " , error , " mm " ) ;
2017-09-06 06:28:32 -05:00
}
return error ;
}
int32_t I2CPositionEncoder : : get_axis_error_steps ( const bool report ) {
if ( ! active ) {
2017-06-09 07:06:23 -05:00
if ( report ) {
2021-02-04 19:18:31 -06:00
SERIAL_CHAR ( axis_codes [ encoderAxis ] ) ;
2017-09-06 06:28:32 -05:00
SERIAL_ECHOLNPGM ( " axis encoder not active! " ) ;
2017-06-09 07:06:23 -05:00
}
2017-09-06 06:28:32 -05:00
return 0 ;
}
float stepperTicksPerUnit ;
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
2018-10-10 09:45:20 -05:00
stepperTicksPerUnit = ( type = = I2CPE_ENC_TYPE_ROTARY ) ? stepperTicks : planner . settings . axis_steps_per_mm [ encoderAxis ] ;
2017-09-06 06:28:32 -05:00
//convert both 'ticks' into same units / base
encoderCountInStepperTicksScaled = LROUND ( ( stepperTicksPerUnit * encoderTicks ) / encoderTicksPerUnit ) ;
2017-06-09 07:06:23 -05:00
2020-02-01 00:57:14 -06:00
const int32_t target = stepper . position ( encoderAxis ) ;
int32_t error = encoderCountInStepperTicksScaled - target ;
2017-09-06 06:28:32 -05:00
//suppress discontinuities (might be caused by bad I2C readings...?)
2018-05-13 03:44:24 -05:00
const bool suppressOutput = ( ABS ( error - errorPrev ) > 100 ) ;
2017-09-06 06:28:32 -05:00
2020-02-01 00:57:14 -06:00
errorPrev = error ;
2017-09-06 06:28:32 -05:00
if ( report ) {
2021-02-04 19:18:31 -06:00
SERIAL_CHAR ( axis_codes [ encoderAxis ] ) ;
2020-02-01 00:57:14 -06:00
SERIAL_ECHOLNPAIR ( " axis target= " , target , " ; actual= " , encoderCountInStepperTicksScaled , " ; err= " , error ) ;
2017-06-09 07:06:23 -05:00
}
2020-02-01 00:57:14 -06:00
if ( suppressOutput ) {
if ( report ) SERIAL_ECHOLNPGM ( " !Discontinuity. Suppressing error. " ) ;
error = 0 ;
}
2017-09-06 06:28:32 -05:00
2020-02-01 00:57:14 -06:00
return error ;
2017-09-06 06:28:32 -05:00
}
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
int32_t I2CPositionEncoder : : get_raw_count ( ) {
uint8_t index = 0 ;
i2cLong encoderCount ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
encoderCount . val = 0x00 ;
2017-06-09 07:06:23 -05:00
2020-04-29 03:13:29 -05:00
if ( Wire . requestFrom ( I2C_ADDRESS ( i2cAddress ) , uint8_t ( 3 ) ) ! = 3 ) {
2017-09-06 06:28:32 -05:00
//houston, we have a problem...
H = I2CPE_MAG_SIG_NF ;
return 0 ;
}
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
while ( Wire . available ( ) )
encoderCount . bval [ index + + ] = ( uint8_t ) Wire . read ( ) ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
//extract the magnetic strength
H = ( B00000011 & ( encoderCount . bval [ 2 ] > > 6 ) ) ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
//extract sign bit; sign = (encoderCount.bval[2] & B00100000);
//set all upper bits to the sign value to overwrite H
encoderCount . val = ( encoderCount . bval [ 2 ] & B00100000 ) ? ( encoderCount . val | 0xFFC00000 ) : ( encoderCount . val & 0x003FFFFF ) ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
if ( invert ) encoderCount . val * = - 1 ;
return encoderCount . val ;
}
bool I2CPositionEncoder : : test_axis ( ) {
//only works on XYZ cartesian machines for the time being
if ( ! ( encoderAxis = = X_AXIS | | encoderAxis = = Y_AXIS | | encoderAxis = = Z_AXIS ) ) return false ;
2019-09-29 04:25:39 -05:00
const float startPosition = soft_endstop . min [ encoderAxis ] + 10 ,
endPosition = soft_endstop . max [ encoderAxis ] - 10 ;
2020-12-16 22:18:40 -06:00
const feedRate_t fr_mm_s = FLOOR ( homing_feedrate ( encoderAxis ) ) ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
ec = false ;
2019-09-29 04:25:39 -05:00
xyze_pos_t startCoord , endCoord ;
LOOP_XYZ ( a ) {
startCoord [ a ] = planner . get_axis_position_mm ( ( AxisEnum ) a ) ;
endCoord [ a ] = planner . get_axis_position_mm ( ( AxisEnum ) a ) ;
2017-06-09 07:06:23 -05:00
}
2017-09-06 06:28:32 -05:00
startCoord [ encoderAxis ] = startPosition ;
endCoord [ encoderAxis ] = endPosition ;
2017-06-09 07:06:23 -05:00
2018-05-12 01:38:02 -05:00
planner . synchronize ( ) ;
2019-09-29 04:25:39 -05:00
startCoord . e = planner . get_axis_position_mm ( E_AXIS ) ;
planner . buffer_line ( startCoord , fr_mm_s , 0 ) ;
2018-05-12 01:38:02 -05:00
planner . synchronize ( ) ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
// if the module isn't currently trusted, wait until it is (or until it should be if things are working)
if ( ! trusted ) {
int32_t startWaitingTime = millis ( ) ;
while ( ! trusted & & millis ( ) - startWaitingTime < I2CPE_TIME_TRUSTED )
safe_delay ( 500 ) ;
}
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
if ( trusted ) { // if trusted, commence test
2019-09-29 04:25:39 -05:00
endCoord . e = planner . get_axis_position_mm ( E_AXIS ) ;
planner . buffer_line ( endCoord , fr_mm_s , 0 ) ;
2018-05-12 01:38:02 -05:00
planner . synchronize ( ) ;
2017-09-06 06:28:32 -05:00
}
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
return trusted ;
}
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
void I2CPositionEncoder : : calibrate_steps_mm ( const uint8_t iter ) {
if ( type ! = I2CPE_ENC_TYPE_LINEAR ) {
2019-02-12 16:21:44 -06:00
SERIAL_ECHOLNPGM ( " Steps/mm calibration requires linear encoder. " ) ;
2017-09-06 06:28:32 -05:00
return ;
}
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
if ( ! ( encoderAxis = = X_AXIS | | encoderAxis = = Y_AXIS | | encoderAxis = = Z_AXIS ) ) {
2019-02-12 16:21:44 -06:00
SERIAL_ECHOLNPGM ( " Steps/mm calibration not supported for this axis. " ) ;
2017-09-06 06:28:32 -05:00
return ;
2017-06-09 07:06:23 -05:00
}
2017-09-06 06:28:32 -05:00
float old_steps_mm , new_steps_mm ,
startDistance , endDistance ,
2019-09-29 04:25:39 -05:00
travelDistance , travelledDistance , total = 0 ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
int32_t startCount , stopCount ;
2017-06-09 07:06:23 -05:00
2020-12-16 22:18:40 -06:00
const feedRate_t fr_mm_s = homing_feedrate ( encoderAxis ) ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
bool oldec = ec ;
ec = false ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
startDistance = 20 ;
2019-09-29 04:25:39 -05:00
endDistance = soft_endstop . max [ encoderAxis ] - 20 ;
2017-09-06 06:28:32 -05:00
travelDistance = endDistance - startDistance ;
2017-06-09 07:06:23 -05:00
2019-09-29 04:25:39 -05:00
xyze_pos_t startCoord , endCoord ;
2019-09-17 18:16:28 -05:00
LOOP_XYZ ( a ) {
startCoord [ a ] = planner . get_axis_position_mm ( ( AxisEnum ) a ) ;
endCoord [ a ] = planner . get_axis_position_mm ( ( AxisEnum ) a ) ;
2017-09-06 06:28:32 -05:00
}
startCoord [ encoderAxis ] = startDistance ;
endCoord [ encoderAxis ] = endDistance ;
2018-05-12 01:38:02 -05:00
planner . synchronize ( ) ;
2017-06-09 07:06:23 -05:00
2018-05-03 20:51:10 -05:00
LOOP_L_N ( i , iter ) {
2019-09-29 04:25:39 -05:00
startCoord . e = planner . get_axis_position_mm ( E_AXIS ) ;
planner . buffer_line ( startCoord , fr_mm_s , 0 ) ;
2018-05-12 01:38:02 -05:00
planner . synchronize ( ) ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
delay ( 250 ) ;
startCount = get_position ( ) ;
2017-06-09 07:06:23 -05:00
2019-09-29 04:25:39 -05:00
//do_blocking_move_to(endCoord);
2017-06-09 07:06:23 -05:00
2019-09-29 04:25:39 -05:00
endCoord . e = planner . get_axis_position_mm ( E_AXIS ) ;
planner . buffer_line ( endCoord , fr_mm_s , 0 ) ;
2018-05-12 01:38:02 -05:00
planner . synchronize ( ) ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
//Read encoder distance
delay ( 250 ) ;
stopCount = get_position ( ) ;
2017-06-09 07:06:23 -05:00
2018-05-13 01:10:34 -05:00
travelledDistance = mm_from_count ( ABS ( stopCount - startCount ) ) ;
2017-06-09 07:06:23 -05:00
2019-03-05 06:46:19 -06:00
SERIAL_ECHOLNPAIR ( " Attempted travel: " , travelDistance , " mm " ) ;
SERIAL_ECHOLNPAIR ( " Actual travel: " , travelledDistance , " mm " ) ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
//Calculate new axis steps per unit
2018-10-10 09:45:20 -05:00
old_steps_mm = planner . settings . axis_steps_per_mm [ encoderAxis ] ;
2017-09-06 06:28:32 -05:00
new_steps_mm = ( old_steps_mm * travelDistance ) / travelledDistance ;
2017-06-09 07:06:23 -05:00
2019-02-12 16:21:44 -06:00
SERIAL_ECHOLNPAIR ( " Old steps/mm: " , old_steps_mm ) ;
SERIAL_ECHOLNPAIR ( " New steps/mm: " , new_steps_mm ) ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
//Save new value
2018-10-10 09:45:20 -05:00
planner . settings . axis_steps_per_mm [ encoderAxis ] = new_steps_mm ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
if ( iter > 1 ) {
total + = new_steps_mm ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
// swap start and end points so next loop runs from current position
2019-09-14 03:05:10 -05:00
const float tempCoord = startCoord [ encoderAxis ] ;
2017-09-06 06:28:32 -05:00
startCoord [ encoderAxis ] = endCoord [ encoderAxis ] ;
endCoord [ encoderAxis ] = tempCoord ;
2017-06-09 07:06:23 -05:00
}
2017-09-06 06:28:32 -05:00
}
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
if ( iter > 1 ) {
total / = ( float ) iter ;
2019-02-12 16:21:44 -06:00
SERIAL_ECHOLNPAIR ( " Average steps/mm: " , total ) ;
2017-09-06 06:28:32 -05:00
}
ec = oldec ;
2017-06-09 07:06:23 -05:00
2019-02-12 16:21:44 -06:00
SERIAL_ECHOLNPGM ( " Calculated steps/mm set. Use M500 to save to EEPROM. " ) ;
2017-09-06 06:28:32 -05:00
}
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
void I2CPositionEncoder : : reset ( ) {
2019-02-20 06:26:36 -06:00
Wire . beginTransmission ( I2C_ADDRESS ( i2cAddress ) ) ;
2017-09-06 06:28:32 -05:00
Wire . write ( I2CPE_RESET_COUNT ) ;
Wire . endTransmission ( ) ;
2017-06-09 07:06:23 -05:00
2020-04-22 16:35:03 -05:00
TERN_ ( I2CPE_ERR_ROLLING_AVERAGE , ZERO ( err ) ) ;
2017-09-06 06:28:32 -05:00
}
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
bool I2CPositionEncodersMgr : : I2CPE_anyaxis ;
uint8_t I2CPositionEncodersMgr : : I2CPE_addr ,
I2CPositionEncodersMgr : : I2CPE_idx ;
I2CPositionEncoder I2CPositionEncodersMgr : : encoders [ I2CPE_ENCODER_CNT ] ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
void I2CPositionEncodersMgr : : init ( ) {
Wire . begin ( ) ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
# if I2CPE_ENCODER_CNT > 0
uint8_t i = 0 ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
encoders [ i ] . init ( I2CPE_ENC_1_ADDR , I2CPE_ENC_1_AXIS ) ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
# ifdef I2CPE_ENC_1_TYPE
encoders [ i ] . set_type ( I2CPE_ENC_1_TYPE ) ;
# endif
# ifdef I2CPE_ENC_1_TICKS_UNIT
encoders [ i ] . set_ticks_unit ( I2CPE_ENC_1_TICKS_UNIT ) ;
# endif
# ifdef I2CPE_ENC_1_TICKS_REV
encoders [ i ] . set_stepper_ticks ( I2CPE_ENC_1_TICKS_REV ) ;
# endif
# ifdef I2CPE_ENC_1_INVERT
encoders [ i ] . set_inverted ( I2CPE_ENC_1_INVERT ) ;
# endif
# ifdef I2CPE_ENC_1_EC_METHOD
encoders [ i ] . set_ec_method ( I2CPE_ENC_1_EC_METHOD ) ;
# endif
# ifdef I2CPE_ENC_1_EC_THRESH
encoders [ i ] . set_ec_threshold ( I2CPE_ENC_1_EC_THRESH ) ;
# endif
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
encoders [ i ] . set_active ( encoders [ i ] . passes_test ( true ) ) ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
# if I2CPE_ENC_1_AXIS == E_AXIS
encoders [ i ] . set_homed ( ) ;
# endif
# endif
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
# if I2CPE_ENCODER_CNT > 1
i + + ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
encoders [ i ] . init ( I2CPE_ENC_2_ADDR , I2CPE_ENC_2_AXIS ) ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
# ifdef I2CPE_ENC_2_TYPE
encoders [ i ] . set_type ( I2CPE_ENC_2_TYPE ) ;
# endif
# ifdef I2CPE_ENC_2_TICKS_UNIT
encoders [ i ] . set_ticks_unit ( I2CPE_ENC_2_TICKS_UNIT ) ;
# endif
# ifdef I2CPE_ENC_2_TICKS_REV
encoders [ i ] . set_stepper_ticks ( I2CPE_ENC_2_TICKS_REV ) ;
# endif
# ifdef I2CPE_ENC_2_INVERT
encoders [ i ] . set_inverted ( I2CPE_ENC_2_INVERT ) ;
# endif
# ifdef I2CPE_ENC_2_EC_METHOD
encoders [ i ] . set_ec_method ( I2CPE_ENC_2_EC_METHOD ) ;
# endif
# ifdef I2CPE_ENC_2_EC_THRESH
encoders [ i ] . set_ec_threshold ( I2CPE_ENC_2_EC_THRESH ) ;
# endif
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
encoders [ i ] . set_active ( encoders [ i ] . passes_test ( true ) ) ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
# if I2CPE_ENC_2_AXIS == E_AXIS
encoders [ i ] . set_homed ( ) ;
# endif
# endif
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
# if I2CPE_ENCODER_CNT > 2
i + + ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
encoders [ i ] . init ( I2CPE_ENC_3_ADDR , I2CPE_ENC_3_AXIS ) ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
# ifdef I2CPE_ENC_3_TYPE
encoders [ i ] . set_type ( I2CPE_ENC_3_TYPE ) ;
# endif
# ifdef I2CPE_ENC_3_TICKS_UNIT
encoders [ i ] . set_ticks_unit ( I2CPE_ENC_3_TICKS_UNIT ) ;
# endif
# ifdef I2CPE_ENC_3_TICKS_REV
encoders [ i ] . set_stepper_ticks ( I2CPE_ENC_3_TICKS_REV ) ;
# endif
# ifdef I2CPE_ENC_3_INVERT
encoders [ i ] . set_inverted ( I2CPE_ENC_3_INVERT ) ;
# endif
# ifdef I2CPE_ENC_3_EC_METHOD
encoders [ i ] . set_ec_method ( I2CPE_ENC_3_EC_METHOD ) ;
# endif
# ifdef I2CPE_ENC_3_EC_THRESH
encoders [ i ] . set_ec_threshold ( I2CPE_ENC_3_EC_THRESH ) ;
2017-06-09 07:06:23 -05:00
# endif
2017-09-06 06:28:32 -05:00
encoders [ i ] . set_active ( encoders [ i ] . passes_test ( true ) ) ;
2017-06-09 10:22:49 -05:00
2017-09-06 06:28:32 -05:00
# if I2CPE_ENC_3_AXIS == E_AXIS
encoders [ i ] . set_homed ( ) ;
2017-06-09 07:06:23 -05:00
# endif
2017-09-06 06:28:32 -05:00
# endif
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
# if I2CPE_ENCODER_CNT > 3
i + + ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
encoders [ i ] . init ( I2CPE_ENC_4_ADDR , I2CPE_ENC_4_AXIS ) ;
# ifdef I2CPE_ENC_4_TYPE
encoders [ i ] . set_type ( I2CPE_ENC_4_TYPE ) ;
# endif
# ifdef I2CPE_ENC_4_TICKS_UNIT
encoders [ i ] . set_ticks_unit ( I2CPE_ENC_4_TICKS_UNIT ) ;
# endif
# ifdef I2CPE_ENC_4_TICKS_REV
encoders [ i ] . set_stepper_ticks ( I2CPE_ENC_4_TICKS_REV ) ;
# endif
# ifdef I2CPE_ENC_4_INVERT
encoders [ i ] . set_inverted ( I2CPE_ENC_4_INVERT ) ;
# endif
# ifdef I2CPE_ENC_4_EC_METHOD
encoders [ i ] . set_ec_method ( I2CPE_ENC_4_EC_METHOD ) ;
# endif
# ifdef I2CPE_ENC_4_EC_THRESH
encoders [ i ] . set_ec_threshold ( I2CPE_ENC_4_EC_THRESH ) ;
# endif
2017-06-09 07:06:23 -05:00
encoders [ i ] . set_active ( encoders [ i ] . passes_test ( true ) ) ;
2017-09-06 06:28:32 -05:00
# if I2CPE_ENC_4_AXIS == E_AXIS
encoders [ i ] . set_homed ( ) ;
2017-06-09 07:06:23 -05:00
# endif
2017-09-06 06:28:32 -05:00
# endif
# if I2CPE_ENCODER_CNT > 4
i + + ;
encoders [ i ] . init ( I2CPE_ENC_5_ADDR , I2CPE_ENC_5_AXIS ) ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
# ifdef I2CPE_ENC_5_TYPE
encoders [ i ] . set_type ( I2CPE_ENC_5_TYPE ) ;
# endif
# ifdef I2CPE_ENC_5_TICKS_UNIT
encoders [ i ] . set_ticks_unit ( I2CPE_ENC_5_TICKS_UNIT ) ;
# endif
# ifdef I2CPE_ENC_5_TICKS_REV
encoders [ i ] . set_stepper_ticks ( I2CPE_ENC_5_TICKS_REV ) ;
# endif
# ifdef I2CPE_ENC_5_INVERT
encoders [ i ] . set_inverted ( I2CPE_ENC_5_INVERT ) ;
# endif
# ifdef I2CPE_ENC_5_EC_METHOD
encoders [ i ] . set_ec_method ( I2CPE_ENC_5_EC_METHOD ) ;
2017-06-09 07:06:23 -05:00
# endif
2017-09-06 06:28:32 -05:00
# 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 ) ) ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
# if I2CPE_ENC_5_AXIS == E_AXIS
encoders [ i ] . set_homed ( ) ;
2017-06-09 07:06:23 -05:00
# endif
2017-09-06 06:28:32 -05:00
# endif
2018-09-13 01:35:55 -05:00
# if I2CPE_ENCODER_CNT > 5
i + + ;
encoders [ i ] . init ( I2CPE_ENC_6_ADDR , I2CPE_ENC_6_AXIS ) ;
# ifdef I2CPE_ENC_6_TYPE
encoders [ i ] . set_type ( I2CPE_ENC_6_TYPE ) ;
# endif
# ifdef I2CPE_ENC_6_TICKS_UNIT
encoders [ i ] . set_ticks_unit ( I2CPE_ENC_6_TICKS_UNIT ) ;
# endif
# ifdef I2CPE_ENC_6_TICKS_REV
encoders [ i ] . set_stepper_ticks ( I2CPE_ENC_6_TICKS_REV ) ;
# endif
# ifdef I2CPE_ENC_6_INVERT
encoders [ i ] . set_inverted ( I2CPE_ENC_6_INVERT ) ;
# endif
# ifdef I2CPE_ENC_6_EC_METHOD
encoders [ i ] . set_ec_method ( I2CPE_ENC_6_EC_METHOD ) ;
# endif
# ifdef I2CPE_ENC_6_EC_THRESH
encoders [ i ] . set_ec_threshold ( I2CPE_ENC_6_EC_THRESH ) ;
# endif
encoders [ i ] . set_active ( encoders [ i ] . passes_test ( true ) ) ;
# if I2CPE_ENC_6_AXIS == E_AXIS
encoders [ i ] . set_homed ( ) ;
# endif
# endif
2017-09-06 06:28:32 -05:00
}
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
void I2CPositionEncodersMgr : : report_position ( const int8_t idx , const bool units , const bool noOffset ) {
CHECK_IDX ( ) ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
if ( units )
SERIAL_ECHOLN ( noOffset ? encoders [ idx ] . mm_from_count ( encoders [ idx ] . get_raw_count ( ) ) : encoders [ idx ] . get_position_mm ( ) ) ;
else {
if ( noOffset ) {
const int32_t raw_count = encoders [ idx ] . get_raw_count ( ) ;
2021-02-04 19:18:31 -06:00
SERIAL_CHAR ( axis_codes [ encoders [ idx ] . get_axis ( ) ] , ' ' ) ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
for ( uint8_t j = 31 ; j > 0 ; j - - )
SERIAL_ECHO ( ( bool ) ( 0x00000001 & ( raw_count > > j ) ) ) ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
SERIAL_ECHO ( ( bool ) ( 0x00000001 & raw_count ) ) ;
SERIAL_CHAR ( ' ' ) ;
SERIAL_ECHOLN ( raw_count ) ;
2017-06-09 07:06:23 -05:00
}
2017-09-06 06:28:32 -05:00
else
SERIAL_ECHOLN ( encoders [ idx ] . get_position ( ) ) ;
}
}
void I2CPositionEncodersMgr : : change_module_address ( const uint8_t oldaddr , const uint8_t newaddr ) {
// First check 'new' address is not in use
2019-02-20 06:26:36 -06:00
Wire . beginTransmission ( I2C_ADDRESS ( newaddr ) ) ;
2017-09-06 06:28:32 -05:00
if ( ! Wire . endTransmission ( ) ) {
2019-03-05 06:46:19 -06:00
SERIAL_ECHOLNPAIR ( " ?There is already a device with that address on the I2C bus! ( " , newaddr , " ) " ) ;
2017-09-06 06:28:32 -05:00
return ;
2017-06-09 07:06:23 -05:00
}
2017-09-06 06:28:32 -05:00
// Now check that we can find the module on the oldaddr address
2019-02-20 06:26:36 -06:00
Wire . beginTransmission ( I2C_ADDRESS ( oldaddr ) ) ;
2017-09-06 06:28:32 -05:00
if ( Wire . endTransmission ( ) ) {
2019-03-05 06:46:19 -06:00
SERIAL_ECHOLNPAIR ( " ?No module detected at this address! ( " , oldaddr , " ) " ) ;
2017-09-06 06:28:32 -05:00
return ;
}
2017-06-09 07:06:23 -05:00
2019-03-05 06:46:19 -06:00
SERIAL_ECHOLNPAIR ( " Module found at " , oldaddr , " , changing address to " , newaddr ) ;
2017-09-06 06:28:32 -05:00
// Change the modules address
2019-02-20 06:26:36 -06:00
Wire . beginTransmission ( I2C_ADDRESS ( oldaddr ) ) ;
2017-09-06 06:28:32 -05:00
Wire . write ( I2CPE_SET_ADDR ) ;
Wire . write ( newaddr ) ;
Wire . endTransmission ( ) ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
SERIAL_ECHOLNPGM ( " Address changed, resetting and waiting for confirmation.. " ) ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
// Wait for the module to reset (can probably be improved by polling address with a timeout).
safe_delay ( I2CPE_REBOOT_TIME ) ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
// Look for the module at the new address.
2019-02-20 06:26:36 -06:00
Wire . beginTransmission ( I2C_ADDRESS ( newaddr ) ) ;
2017-09-06 06:28:32 -05:00
if ( Wire . endTransmission ( ) ) {
SERIAL_ECHOLNPGM ( " Address change failed! Check encoder module. " ) ;
return ;
}
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
SERIAL_ECHOLNPGM ( " Address change successful! " ) ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
// Now, if this module is configured, find which encoder instance it's supposed to correspond to
2018-08-22 17:14:38 -05:00
// and enable it (it will likely have failed initialization on power-up, before the address change).
2017-09-06 06:28:32 -05:00
const int8_t idx = idx_from_addr ( newaddr ) ;
if ( idx > = 0 & & ! encoders [ idx ] . get_active ( ) ) {
2021-02-04 19:18:31 -06:00
SERIAL_CHAR ( axis_codes [ encoders [ idx ] . get_axis ( ) ] ) ;
2017-09-06 06:28:32 -05:00
SERIAL_ECHOLNPGM ( " axis encoder was not detected on printer startup. Trying again. " ) ;
encoders [ idx ] . set_active ( encoders [ idx ] . passes_test ( true ) ) ;
}
}
void I2CPositionEncodersMgr : : report_module_firmware ( const uint8_t address ) {
// First check there is a module
2019-02-20 06:26:36 -06:00
Wire . beginTransmission ( I2C_ADDRESS ( address ) ) ;
2017-09-06 06:28:32 -05:00
if ( Wire . endTransmission ( ) ) {
2019-03-05 06:46:19 -06:00
SERIAL_ECHOLNPAIR ( " ?No module detected at this address! ( " , address , " ) " ) ;
2017-09-06 06:28:32 -05:00
return ;
}
2017-06-09 07:06:23 -05:00
2019-03-05 06:46:19 -06:00
SERIAL_ECHOLNPAIR ( " Requesting version info from module at address " , address , " : " ) ;
2017-06-09 07:06:23 -05:00
2019-02-20 06:26:36 -06:00
Wire . beginTransmission ( I2C_ADDRESS ( address ) ) ;
2017-09-06 06:28:32 -05:00
Wire . write ( I2CPE_SET_REPORT_MODE ) ;
Wire . write ( I2CPE_REPORT_VERSION ) ;
Wire . endTransmission ( ) ;
// Read value
2020-04-29 03:13:29 -05:00
if ( Wire . requestFrom ( I2C_ADDRESS ( address ) , uint8_t ( 32 ) ) ) {
2017-09-06 06:28:32 -05:00
char c ;
while ( Wire . available ( ) > 0 & & ( c = ( char ) Wire . read ( ) ) > 0 )
2021-02-04 19:18:31 -06:00
SERIAL_CHAR ( c ) ;
2017-09-06 06:28:32 -05:00
SERIAL_EOL ( ) ;
2017-06-09 07:06:23 -05:00
}
2017-09-06 06:28:32 -05:00
// Set module back to normal (distance) mode
2019-02-20 06:26:36 -06:00
Wire . beginTransmission ( I2C_ADDRESS ( address ) ) ;
2017-09-06 06:28:32 -05:00
Wire . write ( I2CPE_SET_REPORT_MODE ) ;
Wire . write ( I2CPE_REPORT_DISTANCE ) ;
Wire . endTransmission ( ) ;
}
int8_t I2CPositionEncodersMgr : : parse ( ) {
I2CPE_addr = 0 ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
if ( parser . seen ( ' A ' ) ) {
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
if ( ! parser . has_value ( ) ) {
2018-11-29 16:58:58 -06:00
SERIAL_ECHOLNPGM ( " ?A seen, but no address specified! [30-200] " ) ;
2017-09-06 06:28:32 -05:00
return I2CPE_PARSE_ERR ;
} ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
I2CPE_addr = parser . value_byte ( ) ;
if ( ! WITHIN ( I2CPE_addr , 30 , 200 ) ) { // reserve the first 30 and last 55
2018-11-29 16:58:58 -06:00
SERIAL_ECHOLNPGM ( " ?Address out of range. [30-200] " ) ;
2017-09-06 06:28:32 -05:00
return I2CPE_PARSE_ERR ;
2017-06-09 07:06:23 -05:00
}
2017-09-06 06:28:32 -05:00
I2CPE_idx = idx_from_addr ( I2CPE_addr ) ;
if ( I2CPE_idx > = I2CPE_ENCODER_CNT ) {
2018-11-29 16:58:58 -06:00
SERIAL_ECHOLNPGM ( " ?No device with this address! " ) ;
2017-09-06 06:28:32 -05:00
return I2CPE_PARSE_ERR ;
}
2017-06-09 07:06:23 -05:00
}
2017-09-06 06:28:32 -05:00
else if ( parser . seenval ( ' I ' ) ) {
if ( ! parser . has_value ( ) ) {
2019-03-05 06:46:19 -06:00
SERIAL_ECHOLNPAIR ( " ?I seen, but no index specified! [0- " , I2CPE_ENCODER_CNT - 1 , " ] " ) ;
2017-09-06 06:28:32 -05:00
return I2CPE_PARSE_ERR ;
} ;
I2CPE_idx = parser . value_byte ( ) ;
if ( I2CPE_idx > = I2CPE_ENCODER_CNT ) {
2019-03-05 06:46:19 -06:00
SERIAL_ECHOLNPAIR ( " ?Index out of range. [0- " , I2CPE_ENCODER_CNT - 1 , " ] " ) ;
2017-09-06 06:28:32 -05:00
return I2CPE_PARSE_ERR ;
}
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
I2CPE_addr = encoders [ I2CPE_idx ] . get_address ( ) ;
}
else
I2CPE_idx = 0xFF ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
I2CPE_anyaxis = parser . seen_axis ( ) ;
2017-06-09 10:22:49 -05:00
2017-09-06 06:28:32 -05:00
return I2CPE_PARSE_OK ;
} ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
/**
* M860 : Report the position ( s ) of position encoder module ( s ) .
*
* A < addr > Module I2C address . [ 30 , 200 ] .
* I < index > Module index . [ 0 , I2CPE_ENCODER_CNT - 1 ]
* O Include homed zero - offset in returned position .
* U Units in mm or raw step count .
*
* If A or I not specified :
* X Report on X axis encoder , if present .
* Y Report on Y axis encoder , if present .
* Z Report on Z axis encoder , if present .
* E Report on E axis encoder , if present .
*/
void I2CPositionEncodersMgr : : M860 ( ) {
if ( parse ( ) ) return ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
const bool hasU = parser . seen ( ' U ' ) , hasO = parser . seen ( ' O ' ) ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
if ( I2CPE_idx = = 0xFF ) {
LOOP_XYZE ( i ) {
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 ) ;
2017-06-09 07:06:23 -05:00
}
2017-06-09 10:22:49 -05:00
}
2017-06-09 07:06:23 -05:00
}
2017-09-06 06:28:32 -05:00
else
report_position ( I2CPE_idx , hasU , hasO ) ;
}
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
/**
* M861 : Report the status of position encoder modules .
*
* A < addr > Module I2C address . [ 30 , 200 ] .
* I < index > Module index . [ 0 , I2CPE_ENCODER_CNT - 1 ]
*
* If A or I not specified :
* X Report on X axis encoder , if present .
* Y Report on Y axis encoder , if present .
* Z Report on Z axis encoder , if present .
* E Report on E axis encoder , if present .
*/
void I2CPositionEncodersMgr : : M861 ( ) {
if ( parse ( ) ) return ;
if ( I2CPE_idx = = 0xFF ) {
LOOP_XYZE ( i ) {
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 ) ;
2017-06-09 07:06:23 -05:00
}
2017-06-09 10:22:49 -05:00
}
2017-06-09 07:06:23 -05:00
}
2017-09-06 06:28:32 -05:00
else
report_status ( I2CPE_idx ) ;
}
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
/**
* M862 : Perform an axis continuity test for position encoder
* modules .
*
* A < addr > Module I2C address . [ 30 , 200 ] .
* I < index > Module index . [ 0 , I2CPE_ENCODER_CNT - 1 ]
*
* If A or I not specified :
* X Report on X axis encoder , if present .
* Y Report on Y axis encoder , if present .
* Z Report on Z axis encoder , if present .
* E Report on E axis encoder , if present .
*/
void I2CPositionEncodersMgr : : M862 ( ) {
if ( parse ( ) ) return ;
if ( I2CPE_idx = = 0xFF ) {
LOOP_XYZE ( i ) {
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 ) ;
2017-06-09 07:06:23 -05:00
}
2017-06-09 10:22:49 -05:00
}
2017-06-09 07:06:23 -05:00
}
2017-09-06 06:28:32 -05:00
else
test_axis ( I2CPE_idx ) ;
}
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
/**
* M863 : Perform steps - per - mm calibration for
* position encoder modules .
*
* A < addr > Module I2C address . [ 30 , 200 ] .
* I < index > Module index . [ 0 , I2CPE_ENCODER_CNT - 1 ]
* P Number of rePeats / iterations .
*
* If A or I not specified :
* X Report on X axis encoder , if present .
* Y Report on Y axis encoder , if present .
* Z Report on Z axis encoder , if present .
* E Report on E axis encoder , if present .
*/
void I2CPositionEncodersMgr : : M863 ( ) {
if ( parse ( ) ) return ;
const uint8_t iterations = constrain ( parser . byteval ( ' P ' , 1 ) , 1 , 10 ) ;
if ( I2CPE_idx = = 0xFF ) {
LOOP_XYZE ( i ) {
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 ) ;
2017-06-09 07:06:23 -05:00
}
2017-06-09 10:22:49 -05:00
}
2017-06-09 07:06:23 -05:00
}
2017-09-06 06:28:32 -05:00
else
calibrate_steps_mm ( I2CPE_idx , iterations ) ;
}
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
/**
* M864 : Change position encoder module I2C address .
*
* A < addr > Module current / old I2C address . If not present ,
* assumes default address ( 030 ) . [ 30 , 200 ] .
* S < addr > Module new I2C address . [ 30 , 200 ] .
*
* 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 ) .
* E Use I2CPE_PRESET_ADDR_E ( 033 ) .
*/
void I2CPositionEncodersMgr : : M864 ( ) {
uint8_t newAddress ;
if ( parse ( ) ) return ;
if ( ! I2CPE_addr ) I2CPE_addr = I2CPE_PRESET_ADDR_X ;
if ( parser . seen ( ' S ' ) ) {
if ( ! parser . has_value ( ) ) {
2018-11-29 16:58:58 -06:00
SERIAL_ECHOLNPGM ( " ?S seen, but no address specified! [30-200] " ) ;
2017-09-06 06:28:32 -05:00
return ;
} ;
newAddress = parser . value_byte ( ) ;
if ( ! WITHIN ( newAddress , 30 , 200 ) ) {
2018-11-29 16:58:58 -06:00
SERIAL_ECHOLNPGM ( " ?New address out of range. [30-200] " ) ;
2017-06-09 07:06:23 -05:00
return ;
2017-06-09 10:22:49 -05:00
}
2017-09-06 06:28:32 -05:00
}
else if ( ! I2CPE_anyaxis ) {
2018-11-29 16:58:58 -06:00
SERIAL_ECHOLNPGM ( " ?You must specify S or [XYZE]. " ) ;
2017-09-06 06:28:32 -05:00
return ;
}
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 ;
else return ;
}
2017-06-09 07:06:23 -05:00
2019-03-05 06:46:19 -06:00
SERIAL_ECHOLNPAIR ( " Changing module at address " , I2CPE_addr , " to address " , newAddress ) ;
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
change_module_address ( I2CPE_addr , newAddress ) ;
}
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
/**
* M865 : Check position encoder module firmware version .
*
* A < addr > Module I2C address . [ 30 , 200 ] .
* I < index > Module index . [ 0 , I2CPE_ENCODER_CNT - 1 ] .
*
* If A or I not specified :
* X Check X axis encoder , if present .
* Y Check Y axis encoder , if present .
* Z Check Z axis encoder , if present .
* E Check E axis encoder , if present .
*/
void I2CPositionEncodersMgr : : M865 ( ) {
if ( parse ( ) ) return ;
if ( ! I2CPE_addr ) {
LOOP_XYZE ( i ) {
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 ( ) ) ;
2017-06-09 07:06:23 -05:00
}
2017-06-09 10:22:49 -05:00
}
2017-06-09 07:06:23 -05:00
}
2017-09-06 06:28:32 -05:00
else
report_module_firmware ( I2CPE_addr ) ;
}
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
/**
* M866 : Report or reset position encoder module error
* count .
*
* A < addr > Module I2C address . [ 30 , 200 ] .
* I < index > Module index . [ 0 , I2CPE_ENCODER_CNT - 1 ] .
* R Reset error counter .
*
* If A or I not specified :
* X Act on X axis encoder , if present .
* Y Act on Y axis encoder , if present .
* Z Act on Z axis encoder , if present .
* E Act on E axis encoder , if present .
*/
void I2CPositionEncodersMgr : : M866 ( ) {
if ( parse ( ) ) return ;
const bool hasR = parser . seen ( ' R ' ) ;
if ( I2CPE_idx = = 0xFF ) {
LOOP_XYZE ( 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 ) ) ;
2017-06-09 07:06:23 -05:00
}
}
}
}
2017-09-06 06:28:32 -05:00
else if ( hasR )
reset_error_count ( I2CPE_idx , encoders [ I2CPE_idx ] . get_axis ( ) ) ;
else
report_error_count ( I2CPE_idx , encoders [ I2CPE_idx ] . get_axis ( ) ) ;
}
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
/**
* M867 : Enable / disable or toggle error correction for position encoder modules .
*
* A < addr > Module I2C address . [ 30 , 200 ] .
* I < index > Module index . [ 0 , I2CPE_ENCODER_CNT - 1 ] .
* S < 1 | 0 > Enable / disable error correction . 1 enables , 0 disables . If not
* supplied , toggle .
*
* If A or I not specified :
* X Act on X axis encoder , if present .
* Y Act on Y axis encoder , if present .
* Z Act on Z axis encoder , if present .
* E Act on E axis encoder , if present .
*/
void I2CPositionEncodersMgr : : M867 ( ) {
if ( parse ( ) ) return ;
const int8_t onoff = parser . seenval ( ' S ' ) ? parser . value_int ( ) : - 1 ;
if ( I2CPE_idx = = 0xFF ) {
LOOP_XYZE ( 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 ) ) ;
2017-06-09 07:06:23 -05:00
}
}
2017-06-09 10:22:49 -05:00
}
2017-06-09 07:06:23 -05:00
}
2017-09-06 06:28:32 -05:00
else {
const bool ena = onoff = = - 1 ? ! encoders [ I2CPE_idx ] . get_ec_enabled ( ) : ! ! onoff ;
enable_ec ( I2CPE_idx , ena , encoders [ I2CPE_idx ] . get_axis ( ) ) ;
}
}
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
/**
* M868 : Report or set position encoder module error correction
* threshold .
*
* A < addr > Module I2C address . [ 30 , 200 ] .
* I < index > Module index . [ 0 , I2CPE_ENCODER_CNT - 1 ] .
* T New error correction threshold .
*
* If A not specified :
* X Act on X axis encoder , if present .
* Y Act on Y axis encoder , if present .
* Z Act on Z axis encoder , if present .
* E Act on E axis encoder , if present .
*/
void I2CPositionEncodersMgr : : M868 ( ) {
if ( parse ( ) ) return ;
const float newThreshold = parser . seenval ( ' T ' ) ? parser . value_float ( ) : - 9999 ;
if ( I2CPE_idx = = 0xFF ) {
LOOP_XYZE ( i ) {
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 ( ) ) ;
2017-06-09 07:06:23 -05:00
}
}
}
}
2017-09-06 06:28:32 -05:00
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 ( ) ) ;
}
2017-06-09 07:06:23 -05:00
2017-09-06 06:28:32 -05:00
/**
* M869 : Report position encoder module error .
*
* A < addr > Module I2C address . [ 30 , 200 ] .
* I < index > Module index . [ 0 , I2CPE_ENCODER_CNT - 1 ] .
*
* If A not specified :
* X Act on X axis encoder , if present .
* Y Act on Y axis encoder , if present .
* Z Act on Z axis encoder , if present .
* E Act on E axis encoder , if present .
*/
void I2CPositionEncodersMgr : : M869 ( ) {
if ( parse ( ) ) return ;
if ( I2CPE_idx = = 0xFF ) {
LOOP_XYZE ( i ) {
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 ) ;
2017-06-09 07:06:23 -05:00
}
2017-06-09 10:22:49 -05:00
}
2017-06-09 07:06:23 -05:00
}
2017-09-06 06:28:32 -05:00
else
report_error ( I2CPE_idx ) ;
}
2017-06-09 07:06:23 -05:00
2017-06-09 10:22:49 -05:00
# endif // I2C_POSITION_ENCODERS