@ -31,7 +31,14 @@
extern float destination [ XYZE ] ;
extern void set_current_to_destination ( ) ;
extern float destination [ ] ;
static void debug_echo_axis ( const AxisEnum axis ) {
if ( current_position [ axis ] = = destination [ axis ] )
SERIAL_ECHOPGM ( " ------------- " ) ;
else
SERIAL_ECHO_F ( destination [ X_AXIS ] , 6 ) ;
}
void debug_current_and_destination ( char * title ) {
// if the title message starts with a '!' it is so important, we are going to
@ -67,32 +74,13 @@
SERIAL_ECHOPGM ( " , " ) ;
SERIAL_ECHO_F ( current_position [ E_AXIS ] , 6 ) ;
SERIAL_ECHOPGM ( " ) destination=( " ) ;
if ( current_position [ X_AXIS ] = = destination [ X_AXIS ] )
SERIAL_ECHOPGM ( " ------------- " ) ;
else
SERIAL_ECHO_F ( destination [ X_AXIS ] , 6 ) ;
debug_echo_axis ( X_AXIS ) ;
SERIAL_ECHOPGM ( " , " ) ;
if ( current_position [ Y_AXIS ] = = destination [ Y_AXIS ] )
SERIAL_ECHOPGM ( " ------------- " ) ;
else
SERIAL_ECHO_F ( destination [ Y_AXIS ] , 6 ) ;
debug_echo_axis ( Y_AXIS ) ;
SERIAL_ECHOPGM ( " , " ) ;
if ( current_position [ Z_AXIS ] = = destination [ Z_AXIS ] )
SERIAL_ECHOPGM ( " ------------- " ) ;
else
SERIAL_ECHO_F ( destination [ Z_AXIS ] , 6 ) ;
debug_echo_axis ( Z_AXIS ) ;
SERIAL_ECHOPGM ( " , " ) ;
if ( current_position [ E_AXIS ] = = destination [ E_AXIS ] )
SERIAL_ECHOPGM ( " ------------- " ) ;
else
SERIAL_ECHO_F ( destination [ E_AXIS ] , 6 ) ;
debug_echo_axis ( E_AXIS ) ;
SERIAL_ECHOPGM ( " ) " ) ;
SERIAL_ECHO ( title ) ;
SERIAL_EOL ;
@ -105,32 +93,37 @@
//}
}
void ubl_line_to_destination ( const float & x_end, const float & y_end , const float & z_end , const float & e_end , const float & feed_rate, uint8_t extruder ) {
void ubl_line_to_destination ( const float & , uint8_t extruder ) {
/**
* Much of the nozzle movement will be within the same cell . So we will do as little computation
* as possible to determine if this is the case . If this move is within the same cell , we will
* just do the required Z - Height correction , call the Planner ' s buffer_line ( ) routine , and leave
*/
const float x_start = current_position [ X_AXIS ] ,
y_start = current_position [ Y_AXIS ] ,
z_start = current_position [ Z_AXIS ] ,
e_start = current_position [ E_AXIS ] ;
const int cell_start_xi = ubl . get_cell_index_x ( RAW_X_POSITION ( x_start ) ) ,
cell_start_yi = ubl . get_cell_index_y ( RAW_Y_POSITION ( y_start ) ) ,
cell_dest_xi = ubl . get_cell_index_x ( RAW_X_POSITION ( x_end ) ) ,
cell_dest_yi = ubl . get_cell_index_y ( RAW_Y_POSITION ( y_end ) ) ;
const float start [ XYZE ] = {
current_position [ X_AXIS ] ,
current_position [ Y_AXIS ] ,
current_position [ Z_AXIS ] ,
current_position [ E_AXIS ]
} ,
end [ XYZE ] = {
destination [ X_AXIS ] ,
destination [ Y_AXIS ] ,
destination [ Z_AXIS ] ,
destination [ E_AXIS ]
} ;
const int cell_start_xi = ubl . get_cell_index_x ( RAW_X_POSITION ( start [ X_AXIS ] ) ) ,
cell_start_yi = ubl . get_cell_index_y ( RAW_Y_POSITION ( start [ Y_AXIS ] ) ) ,
cell_dest_xi = ubl . get_cell_index_x ( RAW_X_POSITION ( end [ X_AXIS ] ) ) ,
cell_dest_yi = ubl . get_cell_index_y ( RAW_Y_POSITION ( end [ Y_AXIS ] ) ) ;
if ( ubl . g26_debug_flag ) {
SERIAL_ECHOPGM ( " ubl_line_to_destination(xe= " ) ;
SERIAL_ECHO ( x_end ) ;
SERIAL_ECHOPGM ( " , ye= " ) ;
SERIAL_ECHO ( y_end ) ;
SERIAL_ECHOPGM ( " , ze= " ) ;
SERIAL_ECHO ( z_end ) ;
SERIAL_ECHOPGM ( " , ee= " ) ;
SERIAL_ECHO ( e_end ) ;
SERIAL_ECHOLNPGM ( " ) " ) ;
SERIAL_ECHOPAIR ( " ubl_line_to_destination(xe= " , end [ X_AXIS ] ) ;
SERIAL_ECHOPAIR ( " , ye= " , end [ Y_AXIS ] ) ;
SERIAL_ECHOPAIR ( " , ze= " , end [ Z_AXIS ] ) ;
SERIAL_ECHOPAIR ( " , ee= " , end [ E_AXIS ] ) ;
SERIAL_CHAR ( ' ) ' ) ;
SERIAL_EOL ;
debug_current_and_destination ( ( char * ) " Start of ubl_line_to_destination() " ) ;
}
@ -142,12 +135,12 @@
* But we detect it and isolate it . For now , we just pass along the request .
*/
if ( cell_dest_xi < 0 | | cell_dest_yi < 0 | | cell_dest_xi > = UBL_MESH_NUM_X_POINTS | | cell_dest_yi > = UBL_MESH_NUM_Y_POINTS ) {
if ( ! WITHIN ( cell_dest_xi , 0 , UBL_MESH_NUM_X_POINTS - 1 ) | | ! WITHIN ( cell_dest_yi , 0 , UBL_MESH_NUM_Y_POINTS - 1 ) ) {
// Note: There is no Z Correction in this case. We are off the grid and don't know what
// a reasonable correction would be.
planner . buffer_line ( x_ end, y_ end, z_ end + ubl . state . z_offset , e _e nd, feed_rate , extruder ) ;
planner . buffer_line ( [ X_AXIS ] , [ Y_AXIS ] , [ Z_AXIS ] + ubl . state . z_offset , e nd[ E_AXIS ] , feed_rate , extruder ) ;
set_current_to_destination ( ) ;
if ( ubl . g26_debug_flag )
@ -167,7 +160,7 @@
* to create a 1 - over number for us . That will allow us to do a floating point multiply instead of a floating point divide .
*/
const float xratio = ( RAW_X_POSITION ( x_ end) - ubl . mesh_index_to_xpos [ cell_dest_xi ] ) * ( 1.0 / ( MESH_X_DIST ) ) ,
const float xratio = ( RAW_X_POSITION ( [ X_AXIS ] ) - ubl . mesh_index_to_xpos [ cell_dest_xi ] ) * ( 1.0 / ( MESH_X_DIST ) ) ,
z1 = ubl . z_values [ cell_dest_xi ] [ cell_dest_yi ] + xratio *
( ubl . z_values [ cell_dest_xi + 1 ] [ cell_dest_yi ] - ubl . z_values [ cell_dest_xi ] [ cell_dest_yi ] ) ,
z2 = ubl . z_values [ cell_dest_xi ] [ cell_dest_yi + 1 ] + xratio *
@ -176,7 +169,7 @@
// we are done with the fractional X distance into the cell. Now with the two Z-Heights we have calculated, we
// are going to apply the Y-Distance into the cell to interpolate the final Z correction.
const float yratio = ( RAW_Y_POSITION ( y_ end) - ubl . mesh_index_to_ypos [ cell_dest_yi ] ) * ( 1.0 / ( MESH_Y_DIST ) ) ;
const float yratio = ( RAW_Y_POSITION ( [ Y_AXIS ] ) - ubl . mesh_index_to_ypos [ cell_dest_yi ] ) * ( 1.0 / ( MESH_Y_DIST ) ) ;
float z0 = z1 + ( z2 - z1 ) * yratio ;
@ -186,20 +179,20 @@
*/
/*
z_optimized = z0 ;
z0 = ubl . get_z_correction ( x_ end, y_ end) ;
z0 = ubl . get_z_correction ( [ X_AXIS ] , [ Y_AXIS ] ) ;
if ( fabs ( z_optimized - z0 ) > .01 | | isnan ( z0 ) | | isnan ( z_optimized ) ) {
debug_current_and_destination ( ( char * ) " FINAL_MOVE: z_correction() " ) ;
if ( isnan ( z0 ) ) SERIAL_ECHO ( " z0==NAN " ) ;
if ( isnan ( z_optimized ) ) SERIAL_ECHO ( " z_optimized==NAN " ) ;
SERIAL_ECHOPAIR ( " x_ end=" , x_ end) ;
SERIAL_ECHOPAIR ( " y_ end=" , y_ end) ;
SERIAL_ECHOPAIR ( " end[X_AXIS] =" , [ X_AXIS ] ) ;
SERIAL_ECHOPAIR ( " end[Y_AXIS] =" , [ Y_AXIS ] ) ;
SERIAL_ECHOPAIR ( " z0= " , z0 ) ;
SERIAL_ECHOPAIR ( " z_optimized= " , z_optimized ) ;
SERIAL_ECHOPAIR ( " err= " , fabs ( z_optimized - z0 ) ) ;
SERIAL_EOL ;
}
//*/
z0 * = ubl . fade_scaling_factor_for_z ( z_ end) ;
z0 * = ubl . fade_scaling_factor_for_z ( [ Z_AXIS ] ) ;
/**
* If part of the Mesh is undefined , it will show up as NAN
@ -210,7 +203,7 @@
*/
if ( isnan ( z0 ) ) z0 = 0.0 ;
planner . buffer_line ( x_ end, y_ end, z_ end + z0 + ubl . state . z_offset , e _e nd, feed_rate , extruder ) ;
planner . buffer_line ( [ X_AXIS ] , [ Y_AXIS ] , [ Z_AXIS ] + z0 + ubl . state . z_offset , e nd[ E_AXIS ] , feed_rate , extruder ) ;
if ( ubl . g26_debug_flag )
debug_current_and_destination ( ( char * ) " FINAL_MOVE in ubl_line_to_destination() " ) ;
@ -227,8 +220,8 @@
* blocks of code :
*/
const float dx = x_ end - x_ start,
dy = y_ end - y_ start;
const float dx = [ X_AXIS ] - [ X_AXIS ] ,
dy = [ Y_AXIS ] - [ Y_AXIS ] ;
const int left_flag = dx < 0.0 ? 1 : 0 ,
down_flag = dy < 0.0 ? 1 : 0 ;
@ -251,8 +244,8 @@
const bool use_x_dist = adx > ady ;
float on_axis_distance = use_x_dist ? dx : dy ,
e_position = e _e nd - e_ start,
z_position = z_ end - z_ start;
e_position = e nd[ E_AXIS ] - [ E_AXIS ] ,
z_position = [ Z_AXIS ] - [ Z_AXIS ] ;
const float e_normalized_dist = e_position / on_axis_distance ,
z_normalized_dist = z_position / on_axis_distance ;
@ -260,7 +253,7 @@
int current_xi = cell_start_xi , current_yi = cell_start_yi ;
const float m = dy / dx ,
c = y_ start - m * x_ start;
c = [ Y_AXIS ] - m * [ X_AXIS ] ;
const bool inf_normalized_flag = NEAR_ZERO ( on_axis_distance ) ,
inf_m_flag = NEAR_ZERO ( dx ) ;
@ -281,9 +274,9 @@
* else , we know the next X is the same so we can recover and continue !
* Calculate X at the next Y mesh line
*/
const float x = inf_m_flag ? x_start ( next_mesh_line_y - c ) / m ;
const float x = inf_m_flag ? start [ X_AXIS ] ( next_mesh_line_y - c ) / m ;
float z0 = ubl . get_z_correction_along_horizontal_mesh_line_at_specific_X ( x , current_xi , current_yi ) ;
float z0 = ubl . z_correction_for_x_on_horizontal_mesh_line ( x , current_xi , current_yi ) ;
/**
* Debug code to use non - optimized get_z_correction ( ) and to do a sanity check
@ -305,7 +298,7 @@
}
//*/
z0 * = ubl . fade_scaling_factor_for_z ( z_ end) ;
z0 * = ubl . fade_scaling_factor_for_z ( [ Z_AXIS ] ) ;
/**
* If part of the Mesh is undefined , it will show up as NAN
@ -324,15 +317,15 @@
* happens , it might be best to remove the check and always ' schedule ' the move because
* the planner . buffer_line ( ) routine will filter it if that happens .
*/
if ( y ! = y_ start) {
if ( y ! = [ Y_AXIS ] ) {
if ( ! inf_normalized_flag ) {
on_axis_distance = y - y_ start; // we don't need to check if the extruder position
e_position = e_ start + on_axis_distance * e_normalized_dist ; // is based on X or Y because this is a vertical move
z_position = z_ start + on_axis_distance * z_normalized_dist ;
on_axis_distance = y - [ Y_AXIS ] ; // we don't need to check if the extruder position
e_position = [ E_AXIS ] + on_axis_distance * e_normalized_dist ; // is based on X or Y because this is a vertical move
z_position = [ Z_AXIS ] + on_axis_distance * z_normalized_dist ;
}
else {
e_position = e_ start;
z_position = z_ start;
e_position = [ E_AXIS ] ;
z_position = [ Z_AXIS ] ;
}
planner . buffer_line ( x , y , z_position + z0 + ubl . state . z_offset , e_position , feed_rate , extruder ) ;
@ -345,7 +338,7 @@
//
// Check if we are at the final destination. Usually, we won't be, but if it is on a Y Mesh Line, we are done.
//
if ( current_position [ X_AXIS ] ! = x_ end | | current_position [ Y_AXIS ] ! = y_ end)
if ( current_position [ X_AXIS ] ! = [ X_AXIS ] | | current_position [ Y_AXIS ] ! = [ Y_AXIS ] )
goto FINAL_MOVE ;
set_current_to_destination ( ) ;
@ -368,7 +361,7 @@
const float next_mesh_line_x = LOGICAL_X_POSITION ( ubl . mesh_index_to_xpos [ current_xi ] ) ,
y = m * next_mesh_line_x + c ; // Calculate X at the next Y mesh line
float z0 = ubl . get_z_correction_along_vertical_mesh_line_at_specific_Y ( y , current_xi , current_yi ) ;
float z0 = ubl . z_correction_for_y_on_vertical_mesh_line ( y , current_xi , current_yi ) ;
/**
* Debug code to use non - optimized get_z_correction ( ) and to do a sanity check
@ -390,7 +383,7 @@
}
//*/
z0 = z0 * ubl . fade_scaling_factor_for_z ( z_ end) ;
z0 *= ubl . fade_scaling_factor_for_z ( [ Z_AXIS ] ) ;
/**
* If part of the Mesh is undefined , it will show up as NAN
@ -409,15 +402,15 @@
* that happens , it might be best to remove the check and always ' schedule ' the move because
* the planner . buffer_line ( ) routine will filter it if that happens .
*/
if ( x ! = x_ start) {
if ( x ! = [ X_AXIS ] ) {
if ( ! inf_normalized_flag ) {
on_axis_distance = x - x_ start; // we don't need to check if the extruder position
e_position = e_ start + on_axis_distance * e_normalized_dist ; // is based on X or Y because this is a horizontal move
z_position = z_ start + on_axis_distance * z_normalized_dist ;
on_axis_distance = x - [ X_AXIS ] ; // we don't need to check if the extruder position
e_position = [ E_AXIS ] + on_axis_distance * e_normalized_dist ; // is based on X or Y because this is a horizontal move
z_position = [ Z_AXIS ] + on_axis_distance * z_normalized_dist ;
}
else {
e_position = e_ start;
z_position = z_ start;
e_position = [ E_AXIS ] ;
z_position = [ Z_AXIS ] ;
}
planner . buffer_line ( x , y , z_position + z0 + ubl . state . z_offset , e_position , feed_rate , extruder ) ;
@ -427,7 +420,7 @@
if ( ubl . g26_debug_flag )
debug_current_and_destination ( ( char * ) " horizontal move done in ubl_line_to_destination() " ) ;
if ( current_position [ X_AXIS ] ! = x_ end | | current_position [ Y_AXIS ] ! = y_ end)
if ( current_position [ X_AXIS ] ! = [ X_AXIS ] | | current_position [ Y_AXIS ] ! = [ Y_AXIS ] )
goto FINAL_MOVE ;
set_current_to_destination ( ) ;
@ -454,16 +447,16 @@
const float next_mesh_line_x = LOGICAL_X_POSITION ( ubl . mesh_index_to_xpos [ current_xi + dxi ] ) ,
next_mesh_line_y = LOGICAL_Y_POSITION ( ubl . mesh_index_to_ypos [ current_yi + dyi ] ) ,
y = m * next_mesh_line_x + c , // Calculate Y at the next X mesh line
x = ( next_mesh_line_y - c ) / m ; // Calculate X at the next Y mesh line (we don't have to worry
// about m being equal to 0.0 If this was the case, we would have
// detected this as a vertical line move up above and we wouldn't
// be down here doing a generic type of move.
x = ( next_mesh_line_y - c ) / m ; // Calculate X at the next Y mesh line
// (No need to worry about m being zero.
// If that was the case, it was already detected
// as a vertical line move above.)
if ( left_flag = = ( x > next_mesh_line_x ) ) { // Check if we hit the Y line first
//
// Yes! Crossing a Y Mesh Line next
//
float z0 = ubl . get_z_correction_along_horizontal_mesh_line_at_specific_X ( x , current_xi - left_flag , current_yi + dyi ) ;
float z0 = ubl . z_correction_for_x_on_horizontal_mesh_line ( x , current_xi - left_flag , current_yi + dyi ) ;
/**
* Debug code to use non - optimized get_z_correction ( ) and to do a sanity check
@ -486,7 +479,7 @@
}
//*/
z0 * = ubl . fade_scaling_factor_for_z ( z_ end) ;
z0 * = ubl . fade_scaling_factor_for_z ( [ Z_AXIS ] ) ;
/**
* If part of the Mesh is undefined , it will show up as NAN
@ -498,13 +491,13 @@
if ( isnan ( z0 ) ) z0 = 0.0 ;
if ( ! inf_normalized_flag ) {
on_axis_distance = use_x_dist ? x - x_start : next_mesh_line_y - y_start ;
e_position = e_ start + on_axis_distance * e_normalized_dist ;
z_position = z_ start + on_axis_distance * z_normalized_dist ;
on_axis_distance = use_x_dist ? x - start [ X_AXIS ] : next_mesh_line_y - start [ Y_AXIS ] ;
e_position = [ E_AXIS ] + on_axis_distance * e_normalized_dist ;
z_position = [ Z_AXIS ] + on_axis_distance * z_normalized_dist ;
}
else {
e_position = e_ start;
z_position = z_ start;
e_position = [ E_AXIS ] ;
z_position = [ Z_AXIS ] ;
}
planner . buffer_line ( x , next_mesh_line_y , z_position + z0 + ubl . state . z_offset , e_position , feed_rate , extruder ) ;
current_yi + = dyi ;
@ -514,7 +507,7 @@
//
// Yes! Crossing a X Mesh Line next
//
float z0 = ubl . get_z_correction_along_vertical_mesh_line_at_specific_Y ( y , current_xi + dxi , current_yi - down_flag ) ;
float z0 = ubl . z_correction_for_y_on_vertical_mesh_line ( y , current_xi + dxi , current_yi - down_flag ) ;
/**
* Debug code to use non - optimized get_z_correction ( ) and to do a sanity check
@ -536,7 +529,7 @@
}
//*/
z0 * = ubl . fade_scaling_factor_for_z ( z_ end) ;
z0 * = ubl . fade_scaling_factor_for_z ( [ Z_AXIS ] ) ;
/**
* If part of the Mesh is undefined , it will show up as NAN
@ -548,13 +541,13 @@
if ( isnan ( z0 ) ) z0 = 0.0 ;
if ( ! inf_normalized_flag ) {
on_axis_distance = use_x_dist ? next_mesh_line_x - x_start : y - y_start ;
e_position = e_ start + on_axis_distance * e_normalized_dist ;
z_position = z_ start + on_axis_distance * z_normalized_dist ;
on_axis_distance = use_x_dist ? next_mesh_line_x - start [ X_AXIS ] : y - start [ Y_AXIS ] ;
e_position = [ E_AXIS ] + on_axis_distance * e_normalized_dist ;
z_position = [ Z_AXIS ] + on_axis_distance * z_normalized_dist ;
}
else {
e_position = e_ start;
z_position = z_ start;
e_position = [ E_AXIS ] ;
z_position = [ Z_AXIS ] ;
}
planner . buffer_line ( next_mesh_line_x , y , z_position + z0 + ubl . state . z_offset , e_position , feed_rate , extruder ) ;
@ -566,7 +559,7 @@
if ( ubl . g26_debug_flag )
debug_current_and_destination ( ( char * ) " generic move done in ubl_line_to_destination() " ) ;
if ( current_position [ 0 ] ! = x_end | | current_position [ 1 ] ! = y_end )
if ( current_position [ X_AXIS ] ! = end [ X_AXIS ] | | current_position [ Y_AXIS ] ! = end [ Y_AXIS ] )
goto FINAL_MOVE ;
set_current_to_destination ( ) ;
Scott Lahteine
8 years ago
committed by