@ -346,6 +346,9 @@ int fanSpeed = 0;
float delta_diagonal_rod = DELTA_DIAGONAL_ROD ;
float delta_diagonal_rod = DELTA_DIAGONAL_ROD ;
float delta_diagonal_rod_2 = sq ( delta_diagonal_rod ) ;
float delta_diagonal_rod_2 = sq ( delta_diagonal_rod ) ;
float delta_segments_per_second = DELTA_SEGMENTS_PER_SECOND ;
float delta_segments_per_second = DELTA_SEGMENTS_PER_SECOND ;
# ifdef ENABLE_AUTO_BED_LEVELING
float bed_level [ AUTO_BED_LEVELING_GRID_POINTS ] [ AUTO_BED_LEVELING_GRID_POINTS ] ;
# endif
# endif
# endif
# ifdef SCARA
# ifdef SCARA
@ -1058,6 +1061,8 @@ static void axis_is_at_home(int axis) {
# ifdef ENABLE_AUTO_BED_LEVELING
# ifdef ENABLE_AUTO_BED_LEVELING
# ifdef AUTO_BED_LEVELING_GRID
# ifdef AUTO_BED_LEVELING_GRID
# ifndef DELTA
static void set_bed_level_equation_lsq ( double * plane_equation_coefficients )
static void set_bed_level_equation_lsq ( double * plane_equation_coefficients )
{
{
vector_3 planeNormal = vector_3 ( - plane_equation_coefficients [ 0 ] , - plane_equation_coefficients [ 1 ] , 1 ) ;
vector_3 planeNormal = vector_3 ( - plane_equation_coefficients [ 0 ] , - plane_equation_coefficients [ 1 ] , 1 ) ;
@ -1080,6 +1085,7 @@ static void set_bed_level_equation_lsq(double *plane_equation_coefficients)
plan_set_position ( current_position [ X_AXIS ] , current_position [ Y_AXIS ] , current_position [ Z_AXIS ] , current_position [ E_AXIS ] ) ;
plan_set_position ( current_position [ X_AXIS ] , current_position [ Y_AXIS ] , current_position [ Z_AXIS ] , current_position [ E_AXIS ] ) ;
}
}
# endif
# else // not AUTO_BED_LEVELING_GRID
# else // not AUTO_BED_LEVELING_GRID
@ -1113,6 +1119,27 @@ static void set_bed_level_equation_3pts(float z_at_pt_1, float z_at_pt_2, float
# endif // AUTO_BED_LEVELING_GRID
# endif // AUTO_BED_LEVELING_GRID
static void run_z_probe ( ) {
static void run_z_probe ( ) {
# ifdef DELTA
float start_z = current_position [ Z_AXIS ] ;
long start_steps = st_get_position ( Z_AXIS ) ;
// move down slowly until you find the bed
feedrate = homing_feedrate [ Z_AXIS ] / 4 ;
destination [ Z_AXIS ] = - 10 ;
prepare_move_raw ( ) ;
st_synchronize ( ) ;
endstops_hit_on_purpose ( ) ;
// we have to let the planner know where we are right now as it is not where we said to go.
long stop_steps = st_get_position ( Z_AXIS ) ;
float mm = start_z - float ( start_steps - stop_steps ) / axis_steps_per_unit [ Z_AXIS ] ;
current_position [ Z_AXIS ] = mm ;
calculate_delta ( current_position ) ;
plan_set_position ( delta [ X_AXIS ] , delta [ Y_AXIS ] , delta [ Z_AXIS ] , current_position [ E_AXIS ] ) ;
# else
plan_bed_level_matrix . set_to_identity ( ) ;
plan_bed_level_matrix . set_to_identity ( ) ;
feedrate = homing_feedrate [ Z_AXIS ] ;
feedrate = homing_feedrate [ Z_AXIS ] ;
@ -1139,11 +1166,25 @@ static void run_z_probe() {
current_position [ Z_AXIS ] = st_get_position_mm ( Z_AXIS ) ;
current_position [ Z_AXIS ] = st_get_position_mm ( Z_AXIS ) ;
// make sure the planner knows where we are as it may be a bit different than we last said to move to
// make sure the planner knows where we are as it may be a bit different than we last said to move to
plan_set_position ( current_position [ X_AXIS ] , current_position [ Y_AXIS ] , current_position [ Z_AXIS ] , current_position [ E_AXIS ] ) ;
plan_set_position ( current_position [ X_AXIS ] , current_position [ Y_AXIS ] , current_position [ Z_AXIS ] , current_position [ E_AXIS ] ) ;
# endif
}
}
static void do_blocking_move_to ( float x , float y , float z ) {
static void do_blocking_move_to ( float x , float y , float z ) {
float oldFeedRate = feedrate ;
float oldFeedRate = feedrate ;
# ifdef DELTA
feedrate = XY_TRAVEL_SPEED ;
destination [ X_AXIS ] = x ;
destination [ Y_AXIS ] = y ;
destination [ Z_AXIS ] = z ;
prepare_move_raw ( ) ;
st_synchronize ( ) ;
# else
feedrate = homing_feedrate [ Z_AXIS ] ;
feedrate = homing_feedrate [ Z_AXIS ] ;
current_position [ Z_AXIS ] = z ;
current_position [ Z_AXIS ] = z ;
@ -1157,6 +1198,8 @@ static void do_blocking_move_to(float x, float y, float z) {
plan_buffer_line ( current_position [ X_AXIS ] , current_position [ Y_AXIS ] , current_position [ Z_AXIS ] , current_position [ E_AXIS ] , feedrate / 60 , active_extruder ) ;
plan_buffer_line ( current_position [ X_AXIS ] , current_position [ Y_AXIS ] , current_position [ Z_AXIS ] , current_position [ E_AXIS ] , feedrate / 60 , active_extruder ) ;
st_synchronize ( ) ;
st_synchronize ( ) ;
# endif
feedrate = oldFeedRate ;
feedrate = oldFeedRate ;
}
}
@ -1196,7 +1239,40 @@ static void engage_z_probe() {
servos [ servo_endstops [ Z_AXIS ] ] . detach ( ) ;
servos [ servo_endstops [ Z_AXIS ] ] . detach ( ) ;
# endif
# endif
}
}
# elif defined(Z_PROBE_ALLEN_KEY)
feedrate = homing_feedrate [ X_AXIS ] ;
// Move to the start position to initiate deployment
destination [ X_AXIS ] = Z_PROBE_ALLEN_KEY_DEPLOY_X ;
destination [ Y_AXIS ] = Z_PROBE_ALLEN_KEY_DEPLOY_Y ;
destination [ Z_AXIS ] = Z_PROBE_ALLEN_KEY_DEPLOY_Z ;
prepare_move_raw ( ) ;
// Home X to touch the belt
feedrate = homing_feedrate [ X_AXIS ] / 10 ;
destination [ X_AXIS ] = 0 ;
prepare_move_raw ( ) ;
// Home Y for safety
feedrate = homing_feedrate [ X_AXIS ] / 2 ;
destination [ Y_AXIS ] = 0 ;
prepare_move_raw ( ) ;
st_synchronize ( ) ;
bool z_min_endstop = ( READ ( Z_MIN_PIN ) ! = Z_MIN_ENDSTOP_INVERTING ) ;
if ( z_min_endstop )
{
if ( ! Stopped )
{
SERIAL_ERROR_START ;
SERIAL_ERRORLNPGM ( " Z-Probe failed to engage! " ) ;
LCD_ALERTMESSAGEPGM ( " Err: ZPROBE " ) ;
}
Stop ( ) ;
}
# endif
# endif
}
}
static void retract_z_probe ( ) {
static void retract_z_probe ( ) {
@ -1212,7 +1288,49 @@ static void retract_z_probe() {
servos [ servo_endstops [ Z_AXIS ] ] . detach ( ) ;
servos [ servo_endstops [ Z_AXIS ] ] . detach ( ) ;
# endif
# endif
}
}
# elif defined(Z_PROBE_ALLEN_KEY)
// Move up for safety
feedrate = homing_feedrate [ X_AXIS ] ;
destination [ Z_AXIS ] = current_position [ Z_AXIS ] + 20 ;
prepare_move_raw ( ) ;
// Move to the start position to initiate retraction
destination [ X_AXIS ] = Z_PROBE_ALLEN_KEY_RETRACT_X ;
destination [ Y_AXIS ] = Z_PROBE_ALLEN_KEY_RETRACT_Y ;
destination [ Z_AXIS ] = Z_PROBE_ALLEN_KEY_RETRACT_Z ;
prepare_move_raw ( ) ;
// Move the nozzle down to push the probe into retracted position
feedrate = homing_feedrate [ Z_AXIS ] / 10 ;
destination [ Z_AXIS ] = current_position [ Z_AXIS ] - Z_PROBE_ALLEN_KEY_RETRACT_DEPTH ;
prepare_move_raw ( ) ;
// Move up for safety
feedrate = homing_feedrate [ Z_AXIS ] / 2 ;
destination [ Z_AXIS ] = current_position [ Z_AXIS ] + Z_PROBE_ALLEN_KEY_RETRACT_DEPTH * 2 ;
prepare_move_raw ( ) ;
// Home XY for safety
feedrate = homing_feedrate [ X_AXIS ] / 2 ;
destination [ X_AXIS ] = 0 ;
destination [ Y_AXIS ] = 0 ;
prepare_move_raw ( ) ;
st_synchronize ( ) ;
bool z_min_endstop = ( READ ( Z_MIN_PIN ) ! = Z_MIN_ENDSTOP_INVERTING ) ;
if ( ! z_min_endstop )
{
if ( ! Stopped )
{
SERIAL_ERROR_START ;
SERIAL_ERRORLNPGM ( " Z-Probe failed to retract! " ) ;
LCD_ALERTMESSAGEPGM ( " Err: ZPROBE " ) ;
}
Stop ( ) ;
}
# endif
# endif
}
}
enum ProbeAction { ProbeStay , ProbeEngage , ProbeRetract , ProbeEngageRetract } ;
enum ProbeAction { ProbeStay , ProbeEngage , ProbeRetract , ProbeEngageRetract } ;
@ -1223,14 +1341,14 @@ static float probe_pt(float x, float y, float z_before, ProbeAction retract_acti
do_blocking_move_to ( current_position [ X_AXIS ] , current_position [ Y_AXIS ] , z_before ) ;
do_blocking_move_to ( current_position [ X_AXIS ] , current_position [ Y_AXIS ] , z_before ) ;
do_blocking_move_to ( x - X_PROBE_OFFSET_FROM_EXTRUDER , y - Y_PROBE_OFFSET_FROM_EXTRUDER , current_position [ Z_AXIS ] ) ;
do_blocking_move_to ( x - X_PROBE_OFFSET_FROM_EXTRUDER , y - Y_PROBE_OFFSET_FROM_EXTRUDER , current_position [ Z_AXIS ] ) ;
# if ndef Z_PROBE_SLED
# if !defined(Z_PROBE_SLED) && !defined(Z_PROBE_ALLEN_KEY)
if ( retract_action & ProbeEngage ) engage_z_probe ( ) ;
if ( retract_action & ProbeEngage ) engage_z_probe ( ) ;
# endif
# endif
run_z_probe ( ) ;
run_z_probe ( ) ;
float measured_z = current_position [ Z_AXIS ] ;
float measured_z = current_position [ Z_AXIS ] ;
# if ndef Z_PROBE_SLED
# if !defined(Z_PROBE_SLED) && !defined(Z_PROBE_ALLEN_KEY)
if ( retract_action & ProbeRetract ) retract_z_probe ( ) ;
if ( retract_action & ProbeRetract ) retract_z_probe ( ) ;
# endif
# endif
@ -1247,6 +1365,62 @@ static float probe_pt(float x, float y, float z_before, ProbeAction retract_acti
return measured_z ;
return measured_z ;
}
}
# ifdef DELTA
static void extrapolate_one_point ( int x , int y , int xdir , int ydir ) {
if ( bed_level [ x ] [ y ] ! = 0.0 ) {
return ; // Don't overwrite good values.
}
float a = 2 * bed_level [ x + xdir ] [ y ] - bed_level [ x + xdir * 2 ] [ y ] ; // Left to right.
float b = 2 * bed_level [ x ] [ y + ydir ] - bed_level [ x ] [ y + ydir * 2 ] ; // Front to back.
float c = 2 * bed_level [ x + xdir ] [ y + ydir ] - bed_level [ x + xdir * 2 ] [ y + ydir * 2 ] ; // Diagonal.
float median = c ; // Median is robust (ignores outliers).
if ( a < b ) {
if ( b < c ) median = b ;
if ( c < a ) median = a ;
} else { // b <= a
if ( c < b ) median = b ;
if ( a < c ) median = a ;
}
bed_level [ x ] [ y ] = median ;
}
// Fill in the unprobed points (corners of circular print surface)
// using linear extrapolation, away from the center.
static void extrapolate_unprobed_bed_level ( ) {
int half = ( AUTO_BED_LEVELING_GRID_POINTS - 1 ) / 2 ;
for ( int y = 0 ; y < = half ; y + + ) {
for ( int x = 0 ; x < = half ; x + + ) {
if ( x + y < 3 ) continue ;
extrapolate_one_point ( half - x , half - y , x > 1 ? + 1 : 0 , y > 1 ? + 1 : 0 ) ;
extrapolate_one_point ( half + x , half - y , x > 1 ? - 1 : 0 , y > 1 ? + 1 : 0 ) ;
extrapolate_one_point ( half - x , half + y , x > 1 ? + 1 : 0 , y > 1 ? - 1 : 0 ) ;
extrapolate_one_point ( half + x , half + y , x > 1 ? - 1 : 0 , y > 1 ? - 1 : 0 ) ;
}
}
}
// Print calibration results for plotting or manual frame adjustment.
static void print_bed_level ( ) {
for ( int y = 0 ; y < AUTO_BED_LEVELING_GRID_POINTS ; y + + ) {
for ( int x = 0 ; x < AUTO_BED_LEVELING_GRID_POINTS ; x + + ) {
SERIAL_PROTOCOL_F ( bed_level [ x ] [ y ] , 2 ) ;
SERIAL_PROTOCOLPGM ( " " ) ;
}
SERIAL_ECHOLN ( " " ) ;
}
}
// Reset calibration results to zero.
void reset_bed_level ( ) {
for ( int y = 0 ; y < AUTO_BED_LEVELING_GRID_POINTS ; y + + ) {
for ( int x = 0 ; x < AUTO_BED_LEVELING_GRID_POINTS ; x + + ) {
bed_level [ x ] [ y ] = 0.0 ;
}
}
}
# endif // DELTA
# endif // ENABLE_AUTO_BED_LEVELING
# endif // ENABLE_AUTO_BED_LEVELING
static void homeaxis ( int axis ) {
static void homeaxis ( int axis ) {
@ -1523,7 +1697,11 @@ inline void gcode_G4() {
*/
*/
inline void gcode_G28 ( ) {
inline void gcode_G28 ( ) {
# ifdef ENABLE_AUTO_BED_LEVELING
# ifdef ENABLE_AUTO_BED_LEVELING
plan_bed_level_matrix . set_to_identity ( ) ; //Reset the plane ("erase" all leveling data)
# ifdef DELTA
reset_bed_level ( ) ;
# else
plan_bed_level_matrix . set_to_identity ( ) ; //Reset the plane ("erase" all leveling data)
# endif
# endif
# endif
saved_feedrate = feedrate ;
saved_feedrate = feedrate ;
@ -1804,6 +1982,7 @@ inline void gcode_G28() {
* Parameters With AUTO_BED_LEVELING_GRID :
* Parameters With AUTO_BED_LEVELING_GRID :
*
*
* P Set the size of the grid that will be probed ( P x P points ) .
* P Set the size of the grid that will be probed ( P x P points ) .
* Not supported by non - linear delta printer bed leveling .
* Example : " G29 P4 "
* Example : " G29 P4 "
*
*
* V Set the verbose level ( 0 - 4 ) . Example : " G29 V3 "
* V Set the verbose level ( 0 - 4 ) . Example : " G29 V3 "
@ -1811,6 +1990,7 @@ inline void gcode_G28() {
* T Generate a Bed Topology Report . Example : " G29 P5 T " for a detailed report .
* T Generate a Bed Topology Report . Example : " G29 P5 T " for a detailed report .
* This is useful for manual bed leveling and finding flaws in the bed ( to
* This is useful for manual bed leveling and finding flaws in the bed ( to
* assist with part placement ) .
* assist with part placement ) .
* Not supported by non - linear delta printer bed leveling .
*
*
* F Set the Front limit of the probing grid
* F Set the Front limit of the probing grid
* B Set the Back limit of the probing grid
* B Set the Back limit of the probing grid
@ -1856,16 +2036,21 @@ inline void gcode_G28() {
# ifdef AUTO_BED_LEVELING_GRID
# ifdef AUTO_BED_LEVELING_GRID
# ifndef DELTA
bool topo_flag = verbose_level > 2 | | code_seen ( ' T ' ) | | code_seen ( ' t ' ) ;
bool topo_flag = verbose_level > 2 | | code_seen ( ' T ' ) | | code_seen ( ' t ' ) ;
# endif
if ( verbose_level > 0 )
if ( verbose_level > 0 )
SERIAL_PROTOCOLPGM ( " G29 Auto Bed Leveling \n " ) ;
SERIAL_PROTOCOLPGM ( " G29 Auto Bed Leveling \n " ) ;
int auto_bed_leveling_grid_points = code_seen ( ' P ' ) ? code_value_long ( ) : AUTO_BED_LEVELING_GRID_POINTS ;
int auto_bed_leveling_grid_points = AUTO_BED_LEVELING_GRID_POINTS ;
if ( auto_bed_leveling_grid_points < 2 | | auto_bed_leveling_grid_points > AUTO_BED_LEVELING_GRID_POINTS ) {
# ifndef DELTA
SERIAL_PROTOCOLPGM ( " ?Number of probed (P)oints is implausible (2 minimum). \n " ) ;
if ( code_seen ( ' P ' ) ) auto_bed_leveling_grid_points = code_value_long ( ) ;
return ;
if ( auto_bed_leveling_grid_points < 2 | | auto_bed_leveling_grid_points > AUTO_BED_LEVELING_GRID_POINTS ) {
}
SERIAL_PROTOCOLPGM ( " ?Number of probed (P)oints is implausible (2 minimum). \n " ) ;
return ;
}
# endif
int left_probe_bed_position = code_seen ( ' L ' ) ? code_value_long ( ) : LEFT_PROBE_BED_POSITION ,
int left_probe_bed_position = code_seen ( ' L ' ) ? code_value_long ( ) : LEFT_PROBE_BED_POSITION ,
right_probe_bed_position = code_seen ( ' R ' ) ? code_value_long ( ) : RIGHT_PROBE_BED_POSITION ,
right_probe_bed_position = code_seen ( ' R ' ) ? code_value_long ( ) : RIGHT_PROBE_BED_POSITION ,
@ -1905,20 +2090,27 @@ inline void gcode_G28() {
# ifdef Z_PROBE_SLED
# ifdef Z_PROBE_SLED
dock_sled ( false ) ; // engage (un-dock) the probe
dock_sled ( false ) ; // engage (un-dock) the probe
# elif not defined(SERVO_ENDSTOPS)
engage_z_probe ( ) ;
# endif
# endif
st_synchronize ( ) ;
st_synchronize ( ) ;
# ifdef DELTA
reset_bed_level ( ) ;
# else
// make sure the bed_level_rotation_matrix is identity or the planner will get it incorectly
// make sure the bed_level_rotation_matrix is identity or the planner will get it incorectly
//vector_3 corrected_position = plan_get_position_mm();
//vector_3 corrected_position = plan_get_position_mm();
//corrected_position.debug("position before G29");
//corrected_position.debug("position before G29");
plan_bed_level_matrix . set_to_identity ( ) ;
plan_bed_level_matrix . set_to_identity ( ) ;
vector_3 uncorrected_position = plan_get_position ( ) ;
vector_3 uncorrected_position = plan_get_position ( ) ;
//uncorrected_position.debug("position dur r ing G29");
//uncorrected_position.debug("position dur ing G29");
current_position [ X_AXIS ] = uncorrected_position . x ;
current_position [ X_AXIS ] = uncorrected_position . x ;
current_position [ Y_AXIS ] = uncorrected_position . y ;
current_position [ Y_AXIS ] = uncorrected_position . y ;
current_position [ Z_AXIS ] = uncorrected_position . z ;
current_position [ Z_AXIS ] = uncorrected_position . z ;
plan_set_position ( current_position [ X_AXIS ] , current_position [ Y_AXIS ] , current_position [ Z_AXIS ] , current_position [ E_AXIS ] ) ;
plan_set_position ( current_position [ X_AXIS ] , current_position [ Y_AXIS ] , current_position [ Z_AXIS ] , current_position [ E_AXIS ] ) ;
# endif
setup_for_endstop_move ( ) ;
setup_for_endstop_move ( ) ;
feedrate = homing_feedrate [ Z_AXIS ] ;
feedrate = homing_feedrate [ Z_AXIS ] ;
@ -1926,9 +2118,10 @@ inline void gcode_G28() {
# ifdef AUTO_BED_LEVELING_GRID
# ifdef AUTO_BED_LEVELING_GRID
// probe at the points of a lattice grid
// probe at the points of a lattice grid
int xGridSpacing = ( right_probe_bed_position - left_probe_bed_position ) / ( auto_bed_leveling_grid_points - 1 ) ;
const int xGridSpacing = ( right_probe_bed_position - left_probe_bed_position ) / ( auto_bed_leveling_grid_points - 1 ) ;
int yGridSpacing = ( back_probe_bed_position - front_probe_bed_position ) / ( auto_bed_leveling_grid_points - 1 ) ;
const int yGridSpacing = ( back_probe_bed_position - front_probe_bed_position ) / ( auto_bed_leveling_grid_points - 1 ) ;
# ifndef DELTA
// solve the plane equation ax + by + d = z
// solve the plane equation ax + by + d = z
// A is the matrix with rows [x y 1] for all the probed points
// A is the matrix with rows [x y 1] for all the probed points
// B is the vector of the Z positions
// B is the vector of the Z positions
@ -1941,26 +2134,60 @@ inline void gcode_G28() {
eqnBVector [ abl2 ] , // "B" vector of Z points
eqnBVector [ abl2 ] , // "B" vector of Z points
mean = 0.0 ;
mean = 0.0 ;
# else
delta_grid_spacing [ 0 ] = xGridSpacing ;
delta_grid_spacing [ 1 ] = yGridSpacing ;
float z_offset = Z_PROBE_OFFSET_FROM_EXTRUDER ;
if ( code_seen ( axis_codes [ Z_AXIS ] ) ) {
z_offset + = code_value ( ) ;
}
# endif
int probePointCounter = 0 ;
int probePointCounter = 0 ;
bool zig = true ;
bool zig = true ;
for ( int yProbe = front_probe_bed_position ; yProbe < = back_probe_bed_position ; yProbe + = yGridSpacing ) {
for ( int yCount = 0 ; yCount < auto_bed_leveling_grid_points ; yCount + + )
int xProbe , xInc ;
{
double yProbe = front_probe_bed_position + yGridSpacing * yCount ;
int xStart , xStop , xInc ;
if ( zig )
if ( zig )
xProbe = left_probe_bed_position , xInc = xGridSpacing ;
{
xStart = 0 ;
xStop = auto_bed_leveling_grid_points ;
xInc = 1 ;
zig = false ;
}
else
else
xProbe = right_probe_bed_position , xInc = - xGridSpacing ;
{
xStart = auto_bed_leveling_grid_points - 1 ;
xStop = - 1 ;
xInc = - 1 ;
zig = true ;
}
# ifndef DELTA
// If topo_flag is set then don't zig-zag. Just scan in one direction.
// If topo_flag is set then don't zig-zag. Just scan in one direction.
// This gets the probe points in more readable order.
// This gets the probe points in more readable order.
if ( ! topo_flag ) zig = ! zig ;
if ( ! topo_flag ) zig = ! zig ;
# endif
for ( int xCount = xStart ; xCount ! = xStop ; xCount + = xInc )
{
double xProbe = left_probe_bed_position + xGridSpacing * xCount ;
for ( int xCount = 0 ; xCount < auto_bed_leveling_grid_points ; xCount + + ) {
// raise extruder
// raise extruder
float measured_z ,
float measured_z ,
z_before = probePointCounter = = 0 ? Z_RAISE_BEFORE_PROBING : current_position [ Z_AXIS ] + Z_RAISE_BETWEEN_PROBINGS ;
z_before = probePointCounter = = 0 ? Z_RAISE_BEFORE_PROBING : current_position [ Z_AXIS ] + Z_RAISE_BETWEEN_PROBINGS ;
# ifdef DELTA
// Avoid probing the corners (outside the round or hexagon print surface) on a delta printer.
float distance_from_center = sqrt ( xProbe * xProbe + yProbe * yProbe ) ;
if ( distance_from_center > DELTA_PROBABLE_RADIUS )
continue ;
# endif //DELTA
// Enhanced G29 - Do not retract servo between probes
// Enhanced G29 - Do not retract servo between probes
ProbeAction act ;
ProbeAction act ;
if ( enhanced_g29 ) {
if ( enhanced_g29 ) {
@ -1976,22 +2203,24 @@ inline void gcode_G28() {
measured_z = probe_pt ( xProbe , yProbe , z_before , act , verbose_level ) ;
measured_z = probe_pt ( xProbe , yProbe , z_before , act , verbose_level ) ;
# ifndef DELTA
mean + = measured_z ;
mean + = measured_z ;
eqnBVector [ probePointCounter ] = measured_z ;
eqnBVector [ probePointCounter ] = measured_z ;
eqnAMatrix [ probePointCounter + 0 * abl2 ] = xProbe ;
eqnAMatrix [ probePointCounter + 0 * abl2 ] = xProbe ;
eqnAMatrix [ probePointCounter + 1 * abl2 ] = yProbe ;
eqnAMatrix [ probePointCounter + 1 * abl2 ] = yProbe ;
eqnAMatrix [ probePointCounter + 2 * abl2 ] = 1 ;
eqnAMatrix [ probePointCounter + 2 * abl2 ] = 1 ;
# else
bed_level [ xCount ] [ yCount ] = measured_z + z_offset ;
# endif
probePointCounter + + ;
probePointCounter + + ;
xProbe + = xInc ;
} //xProbe
} //xProbe
} //yProbe
} //yProbe
clean_up_after_endstop_move ( ) ;
clean_up_after_endstop_move ( ) ;
# ifndef DELTA
// solve lsq problem
// solve lsq problem
double * plane_equation_coefficients = qr_solve ( abl2 , 3 , eqnAMatrix , eqnBVector ) ;
double * plane_equation_coefficients = qr_solve ( abl2 , 3 , eqnAMatrix , eqnBVector ) ;
@ -2053,6 +2282,10 @@ inline void gcode_G28() {
set_bed_level_equation_lsq ( plane_equation_coefficients ) ;
set_bed_level_equation_lsq ( plane_equation_coefficients ) ;
free ( plane_equation_coefficients ) ;
free ( plane_equation_coefficients ) ;
# else
extrapolate_unprobed_bed_level ( ) ;
print_bed_level ( ) ;
# endif
# else // !AUTO_BED_LEVELING_GRID
# else // !AUTO_BED_LEVELING_GRID
@ -2075,11 +2308,13 @@ inline void gcode_G28() {
# endif // !AUTO_BED_LEVELING_GRID
# endif // !AUTO_BED_LEVELING_GRID
do_blocking_move_to ( MANUAL_X_HOME_POS , MANUAL_Y_HOME_POS , Z_RAISE_AFTER_PROBING ) ;
st_synchronize ( ) ;
st_synchronize ( ) ;
if ( verbose_level > 0 )
if ( verbose_level > 0 )
plan_bed_level_matrix . debug ( " \n \n Bed Level Correction Matrix: " ) ;
plan_bed_level_matrix . debug ( " \n \n Bed Level Correction Matrix: " ) ;
# ifndef DELTA
// Correct the Z height difference from z-probe position and hotend tip position.
// Correct the Z height difference from z-probe position and hotend tip position.
// The Z height on homing is measured by Z-Probe, but the probe is quite far from the hotend.
// The Z height on homing is measured by Z-Probe, but the probe is quite far from the hotend.
// When the bed is uneven, this height must be corrected.
// When the bed is uneven, this height must be corrected.
@ -2091,10 +2326,13 @@ inline void gcode_G28() {
apply_rotation_xyz ( plan_bed_level_matrix , x_tmp , y_tmp , z_tmp ) ; //Apply the correction sending the probe offset
apply_rotation_xyz ( plan_bed_level_matrix , x_tmp , y_tmp , z_tmp ) ; //Apply the correction sending the probe offset
current_position [ Z_AXIS ] = z_tmp - real_z + current_position [ Z_AXIS ] ; //The difference is added to current position and sent to planner.
current_position [ Z_AXIS ] = z_tmp - real_z + current_position [ Z_AXIS ] ; //The difference is added to current position and sent to planner.
plan_set_position ( current_position [ X_AXIS ] , current_position [ Y_AXIS ] , current_position [ Z_AXIS ] , current_position [ E_AXIS ] ) ;
plan_set_position ( current_position [ X_AXIS ] , current_position [ Y_AXIS ] , current_position [ Z_AXIS ] , current_position [ E_AXIS ] ) ;
# endif
# ifdef Z_PROBE_SLED
# ifdef Z_PROBE_SLED
dock_sled ( true , - SLED_DOCKING_OFFSET ) ; // dock the probe, correcting for over-travel
dock_sled ( true , - SLED_DOCKING_OFFSET ) ; // dock the probe, correcting for over-travel
# endif
# elif not defined(SERVO_ENDSTOPS)
retract_z_probe ( ) ;
# endif
}
}
# ifndef Z_PROBE_SLED
# ifndef Z_PROBE_SLED
@ -4920,7 +5158,64 @@ void calculate_delta(float cartesian[3])
SERIAL_ECHOPGM ( " z= " ) ; SERIAL_ECHOLN ( delta [ Z_AXIS ] ) ;
SERIAL_ECHOPGM ( " z= " ) ; SERIAL_ECHOLN ( delta [ Z_AXIS ] ) ;
*/
*/
}
}
# endif
# ifdef ENABLE_AUTO_BED_LEVELING
// Adjust print surface height by linear interpolation over the bed_level array.
int delta_grid_spacing [ 2 ] = { 0 , 0 } ;
void adjust_delta ( float cartesian [ 3 ] )
{
if ( delta_grid_spacing [ 0 ] = = 0 | | delta_grid_spacing [ 1 ] = = 0 )
return ; // G29 not done
int half = ( AUTO_BED_LEVELING_GRID_POINTS - 1 ) / 2 ;
float grid_x = max ( 0.001 - half , min ( half - 0.001 , cartesian [ X_AXIS ] / delta_grid_spacing [ 0 ] ) ) ;
float grid_y = max ( 0.001 - half , min ( half - 0.001 , cartesian [ Y_AXIS ] / delta_grid_spacing [ 1 ] ) ) ;
int floor_x = floor ( grid_x ) ;
int floor_y = floor ( grid_y ) ;
float ratio_x = grid_x - floor_x ;
float ratio_y = grid_y - floor_y ;
float z1 = bed_level [ floor_x + half ] [ floor_y + half ] ;
float z2 = bed_level [ floor_x + half ] [ floor_y + half + 1 ] ;
float z3 = bed_level [ floor_x + half + 1 ] [ floor_y + half ] ;
float z4 = bed_level [ floor_x + half + 1 ] [ floor_y + half + 1 ] ;
float left = ( 1 - ratio_y ) * z1 + ratio_y * z2 ;
float right = ( 1 - ratio_y ) * z3 + ratio_y * z4 ;
float offset = ( 1 - ratio_x ) * left + ratio_x * right ;
delta [ X_AXIS ] + = offset ;
delta [ Y_AXIS ] + = offset ;
delta [ Z_AXIS ] + = offset ;
/*
SERIAL_ECHOPGM ( " grid_x= " ) ; SERIAL_ECHO ( grid_x ) ;
SERIAL_ECHOPGM ( " grid_y= " ) ; SERIAL_ECHO ( grid_y ) ;
SERIAL_ECHOPGM ( " floor_x= " ) ; SERIAL_ECHO ( floor_x ) ;
SERIAL_ECHOPGM ( " floor_y= " ) ; SERIAL_ECHO ( floor_y ) ;
SERIAL_ECHOPGM ( " ratio_x= " ) ; SERIAL_ECHO ( ratio_x ) ;
SERIAL_ECHOPGM ( " ratio_y= " ) ; SERIAL_ECHO ( ratio_y ) ;
SERIAL_ECHOPGM ( " z1= " ) ; SERIAL_ECHO ( z1 ) ;
SERIAL_ECHOPGM ( " z2= " ) ; SERIAL_ECHO ( z2 ) ;
SERIAL_ECHOPGM ( " z3= " ) ; SERIAL_ECHO ( z3 ) ;
SERIAL_ECHOPGM ( " z4= " ) ; SERIAL_ECHO ( z4 ) ;
SERIAL_ECHOPGM ( " left= " ) ; SERIAL_ECHO ( left ) ;
SERIAL_ECHOPGM ( " right= " ) ; SERIAL_ECHO ( right ) ;
SERIAL_ECHOPGM ( " offset= " ) ; SERIAL_ECHOLN ( offset ) ;
*/
}
# endif //ENABLE_AUTO_BED_LEVELING
void prepare_move_raw ( )
{
previous_millis_cmd = millis ( ) ;
calculate_delta ( destination ) ;
plan_buffer_line ( delta [ X_AXIS ] , delta [ Y_AXIS ] , delta [ Z_AXIS ] ,
destination [ E_AXIS ] , feedrate * feedmultiply / 60 / 100.0 ,
active_extruder ) ;
for ( int8_t i = 0 ; i < NUM_AXIS ; i + + ) {
current_position [ i ] = destination [ i ] ;
}
}
# endif //DELTA
void prepare_move ( )
void prepare_move ( )
{
{