@ -26,12 +26,15 @@
# include "../gcode.h"
# include "../../module/delta.h"
# include "../../module/probe.h"
# include "../../module/motion.h"
# include "../../module/stepper.h"
# include "../../module/endstops.h"
# include "../../lcd/ultralcd.h"
# if HAS_BED_PROBE
# include "../../module/probe.h"
# endif
# if HOTENDS > 1
# include "../../module/tool_change.h"
# endif
@ -43,7 +46,7 @@
constexpr uint8_t _7P_STEP = 1 , // 7-point step - to change number of calibration points
_4P_STEP = _7P_STEP * 2 , // 4-point step
NPP = _7P_STEP * 6 ; // number of calibration points on the radius
enum CalEnum : char { // the 7 main calibration points - add definitions if needed
enum CalEnum : char { // the 7 main calibration points - add definitions if needed
CEN = 0 ,
__A = 1 ,
_AB = __A + _7P_STEP ,
@ -60,7 +63,54 @@ enum CalEnum : char { // the 7 main calibration po
# define LOOP_CAL_RAD(VAR) LOOP_CAL_PT(VAR, __A, _7P_STEP)
# define LOOP_CAL_ACT(VAR, _4P, _OP) LOOP_CAL_PT(VAR, _OP ? _AB : __A, _4P ? _4P_STEP : _7P_STEP)
static void print_signed_float ( const char * const prefix , const float & f ) {
# if HOTENDS > 1
const uint8_t old_tool_index = active_extruder ;
# define AC_CLEANUP() ac_cleanup(old_tool_index)
# else
# define AC_CLEANUP() ac_cleanup()
# endif
float lcd_probe_pt ( const float & rx , const float & ry ) ;
bool ac_home ( ) {
endstops . enable ( true ) ;
if ( ! home_delta ( ) )
return false ;
endstops . not_homing ( ) ;
return true ;
}
void ac_setup ( const bool reset_bed ) {
# if HOTENDS > 1
tool_change ( 0 , 0 , true ) ;
# endif
stepper . synchronize ( ) ;
setup_for_endstop_or_probe_move ( ) ;
# if HAS_LEVELING
if ( reset_bed ) reset_bed_level ( ) ; // After full calibration bed-level data is no longer valid
# endif
}
void ac_cleanup (
# if HOTENDS > 1
const uint8_t old_tool_index
# endif
) {
# if ENABLED(DELTA_HOME_TO_SAFE_ZONE)
do_blocking_move_to_z ( delta_clip_start_height ) ;
# endif
# if HAS_BED_PROBE
STOW_PROBE ( ) ;
# endif
clean_up_after_endstop_or_probe_move ( ) ;
# if HOTENDS > 1
tool_change ( old_tool_index , 0 , true ) ;
# endif
}
void print_signed_float ( const char * const prefix , const float & f ) {
SERIAL_PROTOCOLPGM ( " " ) ;
serialprintPGM ( prefix ) ;
SERIAL_PROTOCOLCHAR ( ' : ' ) ;
@ -68,7 +118,10 @@ static void print_signed_float(const char * const prefix, const float &f) {
SERIAL_PROTOCOL_F ( f , 2 ) ;
}
static void print_G33_settings ( const bool end_stops , const bool tower_angles ) {
/**
* - Print the delta settings
*/
static void print_calibration_settings ( const bool end_stops , const bool tower_angles ) {
SERIAL_PROTOCOLPAIR ( " .Height: " , delta_height ) ;
if ( end_stops ) {
print_signed_float ( PSTR ( " Ex " ) , delta_endstop_adj [ A_AXIS ] ) ;
@ -89,16 +142,25 @@ static void print_G33_settings(const bool end_stops, const bool tower_angles) {
if ( ( ! end_stops & & tower_angles ) | | ( end_stops & & ! tower_angles ) ) { // XOR
SERIAL_PROTOCOLPAIR ( " Radius: " , delta_radius ) ;
}
# if HAS_BED_PROBE
if ( ! end_stops & & ! tower_angles ) {
SERIAL_PROTOCOL_SP ( 30 ) ;
print_signed_float ( PSTR ( " Offset " ) , zprobe_zoffset ) ;
}
# endif
SERIAL_EOL ( ) ;
}
static void print_G33_results ( const float z_at_pt [ NPP + 1 ] , const bool tower_points , const bool opposite_points ) {
/**
* - Print the probe results
*/
static void print_calibration_results ( const float z_pt [ NPP + 1 ] , const bool tower_points , const bool opposite_points ) {
SERIAL_PROTOCOLPGM ( " . " ) ;
print_signed_float ( PSTR ( " c " ) , z_at_pt [ CEN ] ) ;
print_signed_float ( PSTR ( " c " ) , z_ pt[ CEN ] ) ;
if ( tower_points ) {
print_signed_float ( PSTR ( " x " ) , z_ at_ pt[ __A ] ) ;
print_signed_float ( PSTR ( " y " ) , z_ at_ pt[ __B ] ) ;
print_signed_float ( PSTR ( " z " ) , z_ at_ pt[ __C ] ) ;
print_signed_float ( PSTR ( " x " ) , z_ pt[ __A ] ) ;
print_signed_float ( PSTR ( " y " ) , z_ pt[ __B ] ) ;
print_signed_float ( PSTR ( " z " ) , z_ pt[ __C ] ) ;
}
if ( tower_points & & opposite_points ) {
SERIAL_EOL ( ) ;
@ -106,50 +168,63 @@ static void print_G33_results(const float z_at_pt[NPP + 1], const bool tower_poi
SERIAL_PROTOCOL_SP ( 13 ) ;
}
if ( opposite_points ) {
print_signed_float ( PSTR ( " yz " ) , z_ at_ pt[ _BC ] ) ;
print_signed_float ( PSTR ( " zx " ) , z_ at_ pt[ _CA ] ) ;
print_signed_float ( PSTR ( " xy " ) , z_ at_ pt[ _AB ] ) ;
print_signed_float ( PSTR ( " yz " ) , z_ pt[ _BC ] ) ;
print_signed_float ( PSTR ( " zx " ) , z_ pt[ _CA ] ) ;
print_signed_float ( PSTR ( " xy " ) , z_ pt[ _AB ] ) ;
}
SERIAL_EOL ( ) ;
}
/**
* After G33 :
* - Move to the print ceiling ( DELTA_HOME_TO_SAFE_ZONE only )
* - Stow the probe
* - Restore endstops state
* - Select the old tool , if needed
* - Calculate the standard deviation from the zero plane
*/
static void G33_cleanup (
# if HOTENDS > 1
const uint8_t old_tool_index
# endif
) {
# if ENABLED(DELTA_HOME_TO_SAFE_ZONE)
do_blocking_move_to_z ( delta_clip_start_height ) ;
# endif
STOW_PROBE ( ) ;
clean_up_after_endstop_or_probe_move ( ) ;
# if HOTENDS > 1
tool_change ( old_tool_index , 0 , true ) ;
# endif
static float std_dev_points ( float z_pt [ NPP + 1 ] , const bool _0p_cal , const bool _1p_cal , const bool _4p_cal , const bool _4p_opp ) {
if ( ! _0p_cal ) {
float S2 = sq ( z_pt [ CEN ] ) ;
int16_t N = 1 ;
if ( ! _1p_cal ) { // std dev from zero plane
LOOP_CAL_ACT ( rad , _4p_cal , _4p_opp ) {
S2 + = sq ( z_pt [ rad ] ) ;
N + + ;
}
return round ( SQRT ( S2 / N ) * 1000.0 ) / 1000.0 + 0.00001 ;
}
}
return 0.00001 ;
}
inline float calibration_probe ( const float nx , const float ny , const bool stow ) {
/**
* - Probe a point
*/
static float calibration_probe ( const float & nx , const float & ny , const bool stow , const bool set_up ) {
# if HAS_BED_PROBE
return probe_pt ( nx , ny , stow ? PROBE_PT_STOW : PROBE_PT_RAISE , 0 , false ) ;
return probe_pt ( nx , ny , s et_up ? PROBE_PT_BIG_RAISE : s tow ? PROBE_PT_STOW : PROBE_PT_RAISE , 0 , false ) ;
# else
UNUSED ( stow ) ;
UNUSED ( set_up ) ;
return lcd_probe_pt ( nx , ny ) ;
# endif
}
static float probe_G33_points ( float z_at_pt [ NPP + 1 ] , const int8_t probe_points , const bool towers_set , const bool stow_after_each ) {
# if HAS_BED_PROBE
static float probe_z_shift ( const float center ) {
STOW_PROBE ( ) ;
endstops . enable_z_probe ( false ) ;
float z_shift = lcd_probe_pt ( 0 , 0 ) - center ;
endstops . enable_z_probe ( true ) ;
return z_shift ;
}
# endif
/**
* - Probe a grid
*/
static bool probe_calibration_points ( float z_pt [ NPP + 1 ] , const int8_t probe_points , const bool towers_set , const bool stow_after_each , const bool set_up ) {
const bool _0p_calibration = probe_points = = 0 ,
_1p_calibration = probe_points = = 1 ,
_1p_calibration = probe_points = = 1 | | probe_points = = - 1 ,
_4p_calibration = probe_points = = 2 ,
_4p_opposite_points = _4p_calibration & & ! towers_set ,
_7p_calibration = probe_points > = 3 | | probe_points = = 0 ,
_7p_calibration = probe_points > = 3 ,
_7p_no_intermediates = probe_points = = 3 ,
_7p_1_intermediates = probe_points = = 4 ,
_7p_2_intermediates = probe_points = = 5 ,
@ -159,28 +234,28 @@ static float probe_G33_points(float z_at_pt[NPP + 1], const int8_t probe_points,
_7p_11_intermediates = probe_points = = 9 ,
_7p_14_intermediates = probe_points = = 10 ,
_7p_intermed_points = probe_points > = 4 ,
_7p_6_cent r e = probe_points > = 5 & & probe_points < = 7 ,
_7p_9_cent r e = probe_points > = 8 ;
_7p_6_cent er = probe_points > = 5 & & probe_points < = 7 ,
_7p_9_cent er = probe_points > = 8 ;
LOOP_CAL_ALL ( axis) z_at_pt [ axis ] = 0.0 ;
LOOP_CAL_ALL ( rad) z_pt [ rad ] = 0.0 ;
if ( ! _0p_calibration ) {
if ( ! _7p_no_intermediates & & ! _7p_4_intermediates & & ! _7p_11_intermediates ) { // probe the center
z_ at_ pt[ CEN ] + = calibration_probe ( 0 , 0 , stow_after_each ) ;
if ( isnan ( z_ at_ pt[ CEN ] ) ) return NAN ;
z_ pt[ CEN ] + = calibration_probe ( 0 , 0 , stow_after_each , set_up ) ;
if ( isnan ( z_ pt[ CEN ] ) ) return false ;
}
if ( _7p_calibration ) { // probe extra center points
const float start = _7p_9_cent r e ? _CA + _7P_STEP / 3.0 : _7p_6_cent r e ? _CA : __C ,
steps = _7p_9_cent r e ? _4P_STEP / 3.0 : _7p_6_cent r e ? _7P_STEP : _4P_STEP ;
I_LOOP_CAL_PT ( axis , start , steps ) {
const float a = RADIANS ( 210 + ( 360 / NPP ) * ( axis - 1 ) ) ,
const float start = _7p_9_cent er ? _CA + _7P_STEP / 3.0 : _7p_6_cent er ? _CA : __C ,
steps = _7p_9_cent er ? _4P_STEP / 3.0 : _7p_6_cent er ? _7P_STEP : _4P_STEP ;
I_LOOP_CAL_PT ( rad , start , steps ) {
const float a = RADIANS ( 210 + ( 360 / NPP ) * ( rad - 1 ) ) ,
r = delta_calibration_radius * 0.1 ;
z_ at_ pt[ CEN ] + = calibration_probe ( cos ( a ) * r , sin ( a ) * r , stow_after_each ) ;
if ( isnan ( z_ at_ pt[ CEN ] ) ) return NAN ;
z_ pt[ CEN ] + = calibration_probe ( cos ( a ) * r , sin ( a ) * r , stow_after_each , set_up ) ;
if ( isnan ( z_ pt[ CEN ] ) ) return false ;
}
z_ at_ pt[ CEN ] / = float ( _7p_2_intermediates ? 7 : probe_points ) ;
z_ pt[ CEN ] / = float ( _7p_2_intermediates ? 7 : probe_points ) ;
}
if ( ! _1p_calibration ) { // probe the radius
@ -195,182 +270,150 @@ static float probe_G33_points(float z_at_pt[NPP + 1], const int8_t probe_points,
_7p_no_intermediates ? _7P_STEP : // 1r * 6 + 3c = 9
_4P_STEP ; // .5r * 6 + 1c = 4
bool zig_zag = true ;
F_LOOP_CAL_PT ( axis , start , _7p_9_cent r e ? steps * 3 : steps ) {
const int8_t offset = _7p_9_cent re ? 1 : 0 ;
for ( int8_t circle = - offset ; circle < = offset ; circle + + ) {
const float a = RADIANS ( 210 + ( 360 / NPP ) * ( axis - 1 ) ) ,
r = delta_calibration_radius * ( 1 + 0.1 * ( zig_zag ? circle : - circle ) ) ,
interpol = fmod ( axis , 1 ) ;
const float z_temp = calibration_probe ( cos ( a ) * r , sin ( a ) * r , stow_after_each ;
if ( isnan ( z_temp ) ) return NAN ;
F_LOOP_CAL_PT ( rad , start , _7p_9_cent er ? steps * 3 : steps ) {
const int8_t offset = _7p_9_cent er ? 2 : 0 ;
for ( int8_t circle = 0 ; circle < = offset ; circle + + ) {
const float a = RADIANS ( 210 + ( 360 / NPP ) * ( rad - 1 ) ) ,
r = delta_calibration_radius * ( 1 - 0.1 * ( zig_zag ? offset - circle : circle ) ) ,
interpol = fmod ( rad , 1 ) ;
const float z_temp = calibration_probe ( cos ( a ) * r , sin ( a ) * r , stow_after_each , set_up );
if ( isnan ( z_temp ) ) return false ;
// split probe point to neighbouring calibration points
z_ at_ pt[ uint8_t ( round ( axis - interpol + NPP - 1 ) ) % NPP + 1 ] + = z_temp * sq ( cos ( RADIANS ( interpol * 90 ) ) ) ;
z_ at_ pt[ uint8_t ( round ( axis - interpol ) ) % NPP + 1 ] + = z_temp * sq ( sin ( RADIANS ( interpol * 90 ) ) ) ;
z_ pt[ uint8_t ( round ( rad - interpol + NPP - 1 ) ) % NPP + 1 ] + = z_temp * sq ( cos ( RADIANS ( interpol * 90 ) ) ) ;
z_ pt[ uint8_t ( round ( rad - interpol ) ) % NPP + 1 ] + = z_temp * sq ( sin ( RADIANS ( interpol * 90 ) ) ) ;
}
zig_zag = ! zig_zag ;
}
if ( _7p_intermed_points )
LOOP_CAL_RAD ( axis )
z_at_pt [ axis ] / = _7P_STEP / steps ;
}
LOOP_CAL_RAD ( rad )
z_pt [ rad ] / = _7P_STEP / steps ;
float S1 = z_at_pt [ CEN ] ,
S2 = sq ( z_at_pt [ CEN ] ) ;
int16_t N = 1 ;
if ( ! _1p_calibration ) { // std dev from zero plane
LOOP_CAL_ACT ( axis , _4p_calibration , _4p_opposite_points ) {
S1 + = z_at_pt [ axis ] ;
S2 + = sq ( z_at_pt [ axis ] ) ;
N + + ;
}
return round ( SQRT ( S2 / N ) * 1000.0 ) / 1000.0 + 0.00001 ;
do_blocking_move_to_xy ( 0.0 , 0.0 ) ;
}
}
return 0.00001 ;
return true ;
}
# if HAS_BED_PROBE
/**
* kinematics routines and auto tune matrix scaling parameters :
* see https : //github.com/LVD-AC/Marlin-AC/tree/1.1.x-AC/documentation for
* - formulae for approximative forward kinematics in the end - stop displacement matrix
* - definition of the matrix scaling parameters
*/
static void reverse_kinematics_probe_points ( float z_pt [ NPP + 1 ] , float mm_at_pt_axis [ NPP + 1 ] [ ABC ] ) {
float pos [ XYZ ] = { 0.0 } ;
static bool G33_auto_tune ( ) {
float z_at_pt [ NPP + 1 ] = { 0.0 } ,
z_at_pt_base [ NPP + 1 ] = { 0.0 } ,
z_temp , h_fac = 0.0 , r_fac = 0.0 , a_fac = 0.0 , norm = 0.8 ;
# define ZP(N,I) ((N) * z_at_pt[I])
# define Z06(I) ZP(6, I)
# define Z03(I) ZP(3, I)
# define Z02(I) ZP(2, I)
# define Z01(I) ZP(1, I)
# define Z32(I) ZP(3 / 2, I)
SERIAL_PROTOCOLPGM ( " AUTO TUNE baseline " ) ;
SERIAL_EOL ( ) ;
if ( isnan ( probe_G33_points ( z_at_pt_base , 3 , true , false ) ) ) return false ;
print_G33_results ( z_at_pt_base , true , true ) ;
LOOP_XYZ ( axis ) {
delta_endstop_adj [ axis ] - = 1.0 ;
recalc_delta_settings ( ) ;
endstops . enable ( true ) ;
if ( ! home_delta ( ) ) return false ;
endstops . not_homing ( ) ;
SERIAL_PROTOCOLPGM ( " Tuning E " ) ;
SERIAL_CHAR ( tolower ( axis_codes [ axis ] ) ) ;
SERIAL_EOL ( ) ;
if ( isnan ( probe_G33_points ( z_at_pt , 3 , true , false ) ) ) return false ;
LOOP_CAL_ALL ( axis ) z_at_pt [ axis ] - = z_at_pt_base [ axis ] ;
print_G33_results ( z_at_pt , true , true ) ;
delta_endstop_adj [ axis ] + = 1.0 ;
recalc_delta_settings ( ) ;
switch ( axis ) {
case A_AXIS :
h_fac + = 4.0 / ( Z03 ( CEN ) + Z01 ( __A ) + Z32 ( _CA ) + Z32 ( _AB ) ) ; // Offset by X-tower end-stop
break ;
case B_AXIS :
h_fac + = 4.0 / ( Z03 ( CEN ) + Z01 ( __B ) + Z32 ( _BC ) + Z32 ( _AB ) ) ; // Offset by Y-tower end-stop
break ;
case C_AXIS :
h_fac + = 4.0 / ( Z03 ( CEN ) + Z01 ( __C ) + Z32 ( _BC ) + Z32 ( _CA ) ) ; // Offset by Z-tower end-stop
break ;
}
}
h_fac / = 3.0 ;
h_fac * = norm ; // Normalize to 1.02 for Kossel mini
for ( int8_t zig_zag = - 1 ; zig_zag < 2 ; zig_zag + = 2 ) {
delta_radius + = 1.0 * zig_zag ;
recalc_delta_settings ( ) ;
endstops . enable ( true ) ;
if ( ! home_delta ( ) ) return false ;
endstops . not_homing ( ) ;
SERIAL_PROTOCOLPGM ( " Tuning R " ) ;
SERIAL_PROTOCOL ( zig_zag = = - 1 ? " - " : " + " ) ;
SERIAL_EOL ( ) ;
if ( isnan ( probe_G33_points ( z_at_pt , 3 , true , false ) ) ) return false ;
LOOP_CAL_ALL ( axis ) z_at_pt [ axis ] - = z_at_pt_base [ axis ] ;
print_G33_results ( z_at_pt , true , true ) ;
delta_radius - = 1.0 * zig_zag ;
recalc_delta_settings ( ) ;
r_fac - = zig_zag * 6.0 / ( Z03 ( __A ) + Z03 ( __B ) + Z03 ( __C ) + Z03 ( _BC ) + Z03 ( _CA ) + Z03 ( _AB ) ) ; // Offset by delta radius
}
r_fac / = 2.0 ;
r_fac * = 3 * norm ; // Normalize to 2.25 for Kossel mini
LOOP_XYZ ( axis ) {
delta_tower_angle_trim [ axis ] + = 1.0 ;
delta_endstop_adj [ ( axis + 1 ) % 3 ] - = 1.0 / 4.5 ;
delta_endstop_adj [ ( axis + 2 ) % 3 ] + = 1.0 / 4.5 ;
z_temp = MAX3 ( delta_endstop_adj [ A_AXIS ] , delta_endstop_adj [ B_AXIS ] , delta_endstop_adj [ C_AXIS ] ) ;
delta_height - = z_temp ;
LOOP_XYZ ( axis ) delta_endstop_adj [ axis ] - = z_temp ;
recalc_delta_settings ( ) ;
endstops . enable ( true ) ;
if ( ! home_delta ( ) ) return false ;
endstops . not_homing ( ) ;
SERIAL_PROTOCOLPGM ( " Tuning T " ) ;
SERIAL_CHAR ( tolower ( axis_codes [ axis ] ) ) ;
SERIAL_EOL ( ) ;
if ( isnan ( probe_G33_points ( z_at_pt , 3 , true , false ) ) ) return false ;
LOOP_CAL_ALL ( axis ) z_at_pt [ axis ] - = z_at_pt_base [ axis ] ;
print_G33_results ( z_at_pt , true , true ) ;
delta_tower_angle_trim [ axis ] - = 1.0 ;
delta_endstop_adj [ ( axis + 1 ) % 3 ] + = 1.0 / 4.5 ;
delta_endstop_adj [ ( axis + 2 ) % 3 ] - = 1.0 / 4.5 ;
z_temp = MAX3 ( delta_endstop_adj [ A_AXIS ] , delta_endstop_adj [ B_AXIS ] , delta_endstop_adj [ C_AXIS ] ) ;
delta_height - = z_temp ;
LOOP_XYZ ( axis ) delta_endstop_adj [ axis ] - = z_temp ;
recalc_delta_settings ( ) ;
switch ( axis ) {
case A_AXIS :
a_fac + = 4.0 / ( Z06 ( __B ) - Z06 ( __C ) + Z06 ( _CA ) - Z06 ( _AB ) ) ; // Offset by alpha tower angle
break ;
case B_AXIS :
a_fac + = 4.0 / ( - Z06 ( __A ) + Z06 ( __C ) - Z06 ( _BC ) + Z06 ( _AB ) ) ; // Offset by beta tower angle
break ;
case C_AXIS :
a_fac + = 4.0 / ( Z06 ( __A ) - Z06 ( __B ) + Z06 ( _BC ) - Z06 ( _CA ) ) ; // Offset by gamma tower angle
break ;
}
}
a_fac / = 3.0 ;
a_fac * = norm ; // Normalize to 0.83 for Kossel mini
endstops . enable ( true ) ;
if ( ! home_delta ( ) ) return false ;
endstops . not_homing ( ) ;
print_signed_float ( PSTR ( " H_FACTOR: " ) , h_fac ) ;
print_signed_float ( PSTR ( " R_FACTOR: " ) , r_fac ) ;
print_signed_float ( PSTR ( " A_FACTOR: " ) , a_fac ) ;
SERIAL_EOL ( ) ;
SERIAL_PROTOCOLPGM ( " Copy these values to Configuration.h " ) ;
SERIAL_EOL ( ) ;
return true ;
LOOP_CAL_ALL ( rad ) {
const float a = RADIANS ( 210 + ( 360 / NPP ) * ( rad - 1 ) ) ,
r = ( rad = = CEN ? 0.0 : delta_calibration_radius ) ;
pos [ X_AXIS ] = cos ( a ) * r ;
pos [ Y_AXIS ] = sin ( a ) * r ;
pos [ Z_AXIS ] = z_pt [ rad ] ;
inverse_kinematics ( pos ) ;
LOOP_XYZ ( axis ) mm_at_pt_axis [ rad ] [ axis ] = delta [ axis ] ;
}
}
# endif // HAS_BED_PROBE
static void forward_kinematics_probe_points ( float mm_at_pt_axis [ NPP + 1 ] [ ABC ] , float z_pt [ NPP + 1 ] ) {
const float r_quot = delta_calibration_radius / delta_radius ;
# define ZPP(N,I,A) ((1 / 3.0 + r_quot * (N) / 3.0 ) * mm_at_pt_axis[I][A])
# define Z00(I, A) ZPP( 0, I, A)
# define Zp1(I, A) ZPP(+1, I, A)
# define Zm1(I, A) ZPP(-1, I, A)
# define Zp2(I, A) ZPP(+2, I, A)
# define Zm2(I, A) ZPP(-2, I, A)
z_pt [ CEN ] = Z00 ( CEN , A_AXIS ) + Z00 ( CEN , B_AXIS ) + Z00 ( CEN , C_AXIS ) ;
z_pt [ __A ] = Zp2 ( __A , A_AXIS ) + Zm1 ( __A , B_AXIS ) + Zm1 ( __A , C_AXIS ) ;
z_pt [ __B ] = Zm1 ( __B , A_AXIS ) + Zp2 ( __B , B_AXIS ) + Zm1 ( __B , C_AXIS ) ;
z_pt [ __C ] = Zm1 ( __C , A_AXIS ) + Zm1 ( __C , B_AXIS ) + Zp2 ( __C , C_AXIS ) ;
z_pt [ _BC ] = Zm2 ( _BC , A_AXIS ) + Zp1 ( _BC , B_AXIS ) + Zp1 ( _BC , C_AXIS ) ;
z_pt [ _CA ] = Zp1 ( _CA , A_AXIS ) + Zm2 ( _CA , B_AXIS ) + Zp1 ( _CA , C_AXIS ) ;
z_pt [ _AB ] = Zp1 ( _AB , A_AXIS ) + Zp1 ( _AB , B_AXIS ) + Zm2 ( _AB , C_AXIS ) ;
}
static void calc_kinematics_diff_probe_points ( float z_pt [ NPP + 1 ] , float delta_e [ ABC ] , float delta_r , float delta_t [ ABC ] ) {
const float z_center = z_pt [ CEN ] ;
float diff_mm_at_pt_axis [ NPP + 1 ] [ ABC ] ,
new_mm_at_pt_axis [ NPP + 1 ] [ ABC ] ;
reverse_kinematics_probe_points ( z_pt , diff_mm_at_pt_axis ) ;
delta_radius + = delta_r ;
LOOP_XYZ ( axis ) delta_tower_angle_trim [ axis ] + = delta_t [ axis ] ;
recalc_delta_settings ( ) ;
reverse_kinematics_probe_points ( z_pt , new_mm_at_pt_axis ) ;
LOOP_XYZ ( axis ) LOOP_CAL_ALL ( rad ) diff_mm_at_pt_axis [ rad ] [ axis ] - = new_mm_at_pt_axis [ rad ] [ axis ] + delta_e [ axis ] ;
forward_kinematics_probe_points ( diff_mm_at_pt_axis , z_pt ) ;
LOOP_CAL_RAD ( rad ) z_pt [ rad ] - = z_pt [ CEN ] - z_center ;
z_pt [ CEN ] = z_center ;
delta_radius - = delta_r ;
LOOP_XYZ ( axis ) delta_tower_angle_trim [ axis ] - = delta_t [ axis ] ;
recalc_delta_settings ( ) ;
}
static float auto_tune_h ( ) {
const float r_quot = delta_calibration_radius / delta_radius ;
float h_fac = 0.0 ;
h_fac = r_quot / ( 2.0 / 3.0 ) ;
h_fac = 1.0 / h_fac ; // (2/3)/CR
return h_fac ;
}
static float auto_tune_r ( ) {
const float diff = 0.01 ;
float r_fac = 0.0 ,
z_pt [ NPP + 1 ] = { 0.0 } ,
delta_e [ ABC ] = { 0.0 } ,
delta_r = { 0.0 } ,
delta_t [ ABC ] = { 0.0 } ;
delta_r = diff ;
calc_kinematics_diff_probe_points ( z_pt , delta_e , delta_r , delta_t ) ;
r_fac = - ( z_pt [ __A ] + z_pt [ __B ] + z_pt [ __C ] + z_pt [ _BC ] + z_pt [ _CA ] + z_pt [ _AB ] ) / 6.0 ;
r_fac = diff / r_fac / 3.0 ; // 1/(3*delta_Z)
return r_fac ;
}
static float auto_tune_a ( ) {
const float diff = 0.01 ;
float a_fac = 0.0 ,
z_pt [ NPP + 1 ] = { 0.0 } ,
delta_e [ ABC ] = { 0.0 } ,
delta_r = { 0.0 } ,
delta_t [ ABC ] = { 0.0 } ;
LOOP_XYZ ( axis ) {
LOOP_XYZ ( axis_2 ) delta_t [ axis_2 ] = 0.0 ;
delta_t [ axis ] = diff ;
calc_kinematics_diff_probe_points ( z_pt , delta_e , delta_r , delta_t ) ;
a_fac + = z_pt [ uint8_t ( ( axis * _4P_STEP ) - _7P_STEP + NPP ) % NPP + 1 ] / 6.0 ;
a_fac - = z_pt [ uint8_t ( ( axis * _4P_STEP ) + 1 + _7P_STEP ) ] / 6.0 ;
}
a_fac = diff / a_fac / 3.0 ; // 1/(3*delta_Z)
return a_fac ;
}
/**
* G33 - Delta ' 1 - 4 - 7 - point ' Auto - Calibration
* Calibrate height , endstops , delta radius , and tower angles .
* Calibrate height , z_offset , endstops , delta radius , and tower angles .
*
* Parameters :
*
* S Setup mode ; disables probe protection
*
* Pn Number of probe points :
* P0 No probe . Normalize only .
* P1 Probe center and set height only .
* P2 Probe center and towers . Set height , endstops and delta radius .
* P3 Probe all positions : center , towers and opposite towers . Set all .
* P4 - P10 Probe all positions + at different intermediate locations and average them .
* P - 1 Checks the z_offset with a center probe and paper test .
* P0 Normalizes calibration .
* P1 Calibrates height only with center probe .
* P2 Probe center and towers . Calibrate height , endstops and delta radius .
* P3 Probe all positions : center , towers and opposite towers . Calibrate all .
* P4 - P10 Probe all positions at different intermediate locations and average them .
*
* T Don ' t calibrate tower angle corrections
*
@ -378,8 +421,6 @@ static float probe_G33_points(float z_at_pt[NPP + 1], const int8_t probe_points,
*
* Fn Force to run at least n iterations and take the best result
*
* A Auto - tune calibration factors ( set in Configuration . h )
*
* Vn Verbose level :
* V0 Dry - run mode . Report settings and probe results . No calibration .
* V1 Report start and end settings only
@ -390,19 +431,22 @@ static float probe_G33_points(float z_at_pt[NPP + 1], const int8_t probe_points,
*/
void GcodeSuite : : G33 ( ) {
const int8_t probe_points = parser . intval ( ' P ' , DELTA_CALIBRATION_DEFAULT_POINTS ) ;
if ( ! WITHIN ( probe_points , 0 , 10 ) ) {
SERIAL_PROTOCOLLNPGM ( " ?(P)oints is implausible (0-10). " ) ;
const bool set_up =
# if HAS_BED_PROBE
parser . seen ( ' S ' ) ;
# else
false ;
# endif
const int8_t probe_points = set_up ? 2 : parser . intval ( ' P ' , DELTA_CALIBRATION_DEFAULT_POINTS ) ;
if ( ! WITHIN ( probe_points , - 1 , 10 ) ) {
SERIAL_PROTOCOLLNPGM ( " ?(P)oints is implausible (-1 - 10). " ) ;
return ;
}
const int8_t verbose_level = parser . byteval ( ' V ' , 1 ) ;
if ( ! WITHIN ( verbose_level , 0 , 3 ) ) {
SERIAL_PROTOCOLLNPGM ( " ?(V)erbose level is implausible (0-3). " ) ;
return ;
}
const bool towers_set = ! parser . seen ( ' T ' ) ;
const float calibration_precision = . floatval ( ' C ' , 0.0 ) ;
const float calibration_precision = set_up ? Z_CLEARANCE_BETWEEN_PROBES / 5.0 : parser . floatval ( ' C ' , 0.0 ) ;
if ( calibration_precision < 0 ) {
SERIAL_PROTOCOLLNPGM ( " ?(C)alibration precision is implausible (>=0). " ) ;
return ;
@ -410,36 +454,52 @@ void GcodeSuite::G33() {
const int8_t force_iterations = parser . intval ( ' F ' , 0 ) ;
if ( ! WITHIN ( force_iterations , 0 , 30 ) ) {
SERIAL_PROTOCOLLNPGM ( " ?(F)orce iteration is implausible (0 -30)." ) ;
SERIAL_PROTOCOLLNPGM ( " ?(F)orce iteration is implausible (0 - 30)." ) ;
return ;
}
const bool towers_set = ! parser . boolval ( ' T ' ) ,
auto_tune = parser . boolval ( ' A ' ) ,
stow_after_each = parser . boolval ( ' E ' ) ,
_0p_calibration = probe_points = = 0 ,
_1p_calibration = probe_points = = 1 ,
const int8_t verbose_level = parser . byteval ( ' V ' , 1 ) ;
if ( ! WITHIN ( verbose_level , 0 , 3 ) ) {
SERIAL_PROTOCOLLNPGM ( " ?(V)erbose level is implausible (0 - 3). " ) ;
return ;
}
const bool stow_after_each = parser . seen ( ' E ' ) ;
if ( set_up ) {
delta_height = 999.99 ;
delta_radius = DELTA_PRINTABLE_RADIUS ;
ZERO ( delta_endstop_adj ) ;
ZERO ( delta_tower_angle_trim ) ;
recalc_delta_settings ( ) ;
}
const bool _0p_calibration = probe_points = = 0 ,
_1p_calibration = probe_points = = 1 | | probe_points = = - 1 ,
_4p_calibration = probe_points = = 2 ,
_7p_9_centre = probe_points > = 8 ,
_tower_results = ( _4p_calibration & & towers_set )
| | probe_points > = 3 | | probe_points = = 0 ,
_opposite_results = ( _4p_calibration & & ! towers_set )
| | probe_points > = 3 | | probe_points = = 0 ,
_endstop_results = probe_points ! = 1 ,
_angle_results = ( probe_points > = 3 | | probe_points = = 0 ) & & towers_set ;
_4p_opposite_points = _4p_calibration & & ! towers_set ,
_7p_9_center = probe_points > = 8 ,
_tower_results = ( _4p_calibration & & towers_set ) | | probe_points > = 3 ,
_opposite_results = ( _4p_calibration & & ! towers_set ) | | probe_points > = 3 ,
_endstop_results = probe_points ! = 1 & & probe_points ! = - 1 & & probe_points ! = 0 ,
_angle_results = probe_points > = 3 & & towers_set ;
const static char save_message [ ] PROGMEM = " Save with M500 and/or copy to Configuration.h " ;
int8_t iterations = 0 ;
float test_precision ,
zero_std_dev = ( verbose_level ? 999.0 : 0.0 ) , // 0.0 in dry-run mode : forced end
zero_std_dev_min = zero_std_dev ,
zero_std_dev_old = zero_std_dev ,
h_factor ,
r_factor ,
a_factor ,
e_old [ ABC ] = {
delta_endstop_adj [ A_AXIS ] ,
delta_endstop_adj [ B_AXIS ] ,
delta_endstop_adj [ C_AXIS ]
} ,
d r_old = delta_radius ,
z h_old = delta_height ,
t a_old[ ABC ] = {
= delta_radius ,
= delta_height ,
[ ABC ] = {
delta_tower_angle_trim [ A_AXIS ] ,
delta_tower_angle_trim [ B_AXIS ] ,
delta_tower_angle_trim [ C_AXIS ]
@ -447,10 +507,10 @@ void GcodeSuite::G33() {
SERIAL_PROTOCOLLNPGM ( " G33 Auto Calibrate " ) ;
if ( ! _1p_calibration & & ! _0p_calibration ) { // test if the outer radius is reachable
if ( ! _1p_calibration & & ! _0p_calibration ) { // test if the outer radius is reachable
LOOP_CAL_RAD ( axis ) {
const float a = RADIANS ( 210 + ( 360 / NPP ) * ( axis - 1 ) ) ,
r = delta_calibration_radius * ( 1 + ( _7p_9_centre ? 0.1 : 0.0 ) ) ;
r = delta_calibration_radius ;
if ( ! position_is_reachable ( cos ( a ) * r , sin ( a ) * r ) ) {
SERIAL_PROTOCOLLNPGM ( " ?(M665 B)ed radius is implausible. " ) ;
return ;
@ -458,159 +518,137 @@ void GcodeSuite::G33() {
}
}
stepper . synchronize ( ) ;
# if HAS_LEVELING
reset_bed_level ( ) ; // After calibration bed-level data is no longer valid
# endif
# if HOTENDS > 1
const uint8_t old_tool_index = active_extruder ;
tool_change ( 0 , 0 , true ) ;
# define G33_CLEANUP() G33_cleanup(old_tool_index)
# else
# define G33_CLEANUP() G33_cleanup()
# endif
setup_for_endstop_or_probe_move ( ) ;
endstops . enable ( true ) ;
if ( ! _0p_calibration ) {
if ( ! home_delta ( ) )
return ;
endstops . not_homing ( ) ;
}
if ( auto_tune ) {
# if HAS_BED_PROBE
G33_auto_tune ( ) ;
# else
SERIAL_PROTOCOLLNPGM ( " A probe is needed for auto-tune " ) ;
# endif
G33_CLEANUP ( ) ;
return ;
}
// Report settings
PGM_P checkingac = PSTR ( " Checking... AC " ) ; // TODO: Make translatable string
const char * checkingac = PSTR ( " Checking... AC " ) ;
serialprintPGM ( checkingac ) ;
if ( verbose_level = = 0 ) SERIAL_PROTOCOLPGM ( " (DRY-RUN) " ) ;
if ( set_up ) SERIAL_PROTOCOLPGM ( " (SET-UP) " ) ;
SERIAL_EOL ( ) ;
lcd_setstatusPGM ( checkingac ) ;
char mess [ 11 ] ;
strcpy_P ( mess , checkingac ) ;
lcd_setstatus ( mess ) ;
print_G33_settings ( _endstop_results , _angle_results ) ;
print_calibration_settings ( _endstop_results , _angle_results ) ;
do {
ac_setup ( ! _0p_calibration & & ! _1p_calibration ) ;
if ( ! _0p_calibration )
if ( ! ac_home ( ) ) return ;
do { // start iterations
float z_at_pt [ NPP + 1 ] = { 0.0 } ;
test_precision = zero_std_dev ;
test_precision = zero_std_dev_old ! = 999.0 ? ( zero_std_dev + zero_std_dev_old ) / 2 : zero_std_dev ;
iterations + + ;
// Probe the points
zero_std_dev = probe_G33_points ( z_at_pt , probe_points , towers_set , stow_after_each ) ;
if ( isnan ( zero_std_dev ) ) {
SERIAL_PROTOCOLPGM ( " Correct delta_radius with M665 R or end-stops with M666 X Y Z " ) ;
SERIAL_EOL ( ) ;
return G33_CLEANUP ( ) ;
zero_std_dev_old = zero_std_dev ;
if ( ! probe_calibration_points ( z_at_pt , probe_points , towers_set , stow_after_each , set_up ) ) {
SERIAL_PROTOCOLLNPGM ( " Correct delta settings with M665 and M666 " ) ;
return AC_CLEANUP ( ) ;
}
zero_std_dev = std_dev_points ( z_at_pt , _0p_calibration , _1p_calibration , _4p_calibration , _4p_opposite_points ) ;
// Solve matrices
if ( ( zero_std_dev < test_precision | | iterations < = force_iterations ) & & zero_std_dev > calibration_precision ) {
if ( zero_std_dev < zero_std_dev_min ) {
COPY ( e_old , delta_endstop_adj ) ;
dr_old = delta_radius ;
zh_old = delta_height ;
COPY ( ta_old , delta_tower_angle_trim ) ;
}
float e_delta [ ABC ] = { 0.0 } , r_delta = 0.0 , t_delta [ ABC ] = { 0.0 } ;
const float r_diff = delta_radius - delta_calibration_radius ,
h_factor = 1 / 6.0 *
# ifdef H_FACTOR
( H_FACTOR ) , // Set in Configuration.h
# else
( 1.00 + r_diff * 0.001 ) , // 1.02 for r_diff = 20mm
# endif
r_factor = 1 / 6.0 *
# ifdef R_FACTOR
- ( R_FACTOR ) , // Set in Configuration.h
# else
- ( 1.75 + 0.005 * r_diff + 0.001 * sq ( r_diff ) ) , // 2.25 for r_diff = 20mm
# endif
a_factor = 1 / 6.0 *
# ifdef A_FACTOR
( A_FACTOR ) ; // Set in Configuration.h
# else
( 66.66 / delta_calibration_radius ) ; // 0.83 for cal_rd = 80mm
# endif
# define ZP(N,I) ((N) * z_at_pt[I])
# define Z6(I) ZP(6, I)
# define Z4(I) ZP(4, I)
# define Z2(I) ZP(2, I)
# define Z1(I) ZP(1, I)
# if !HAS_BED_PROBE
test_precision = 0.00 ; // forced end
# endif
if ( zero_std_dev < zero_std_dev_min ) {
// set roll-back point
COPY ( e_old , delta_endstop_adj ) ;
r_old = delta_radius ;
h_old = delta_height ;
COPY ( a_old , delta_tower_angle_trim ) ;
}
float e_delta [ ABC ] = { 0.0 } ,
r_delta = 0.0 ,
t_delta [ ABC ] = { 0.0 } ;
/**
* convergence matrices :
* see https : //github.com/LVD-AC/Marlin-AC/tree/1.1.x-AC/documentation for
* - definition of the matrix scaling parameters
* - matrices for 4 and 7 point calibration
*/
# define ZP(N,I) ((N) * z_at_pt[I] / 4.0) // 4.0 = divider to normalize to integers
# define Z12(I) ZP(12, I)
# define Z4(I) ZP(4, I)
# define Z2(I) ZP(2, I)
# define Z1(I) ZP(1, I)
# define Z0(I) ZP(0, I)
// calculate factors
const float cr_old = delta_calibration_radius ;
if ( _7p_9_center ) delta_calibration_radius * = 0.9 ;
h_factor = auto_tune_h ( ) ;
r_factor = auto_tune_r ( ) ;
a_factor = auto_tune_a ( ) ;
delta_calibration_radius = cr_old ;
switch ( probe_points ) {
case - 1 :
# if HAS_BED_PROBE
zprobe_zoffset + = probe_z_shift ( z_at_pt [ CEN ] ) ;
# endif
case 0 :
test_precision = 0.00 ; // forced end
break ;
case 1 :
test_precision = 0.00 ; // forced end
LOOP_XYZ ( axis ) e_delta [ axis ] = Z1 ( CEN ) ;
LOOP_XYZ ( axis ) e_delta [ axis ] = + Z4 ( CEN ) ;
break ;
case 2 :
if ( towers_set ) {
e_delta [ A_AXIS ] = ( Z6 ( CEN ) + Z4 ( __A ) - Z2 ( __B ) - Z2 ( __C ) ) * h_factor ;
e_delta [ B_AXIS ] = ( Z6 ( CEN ) - Z2 ( __A ) + Z4 ( __B ) - Z2 ( __C ) ) * h_factor ;
e_delta [ C_AXIS ] = ( Z6 ( CEN ) - Z2 ( __A ) - Z2 ( __B ) + Z4 ( __C ) ) * h_factor ;
r_delta = ( Z6 ( CEN ) - Z2 ( __A ) - Z2 ( __B ) - Z2 ( __C ) ) * r_factor ;
if ( towers_set ) { // see 4 point calibration (towers) matrix
e_delta [ A_AXIS ] = ( + Z4 ( __A ) - Z2 ( __B ) - Z2 ( __C ) ) * h_factor + Z4 ( CEN ) ;
e_delta [ B_AXIS ] = ( - Z2 ( __A ) + Z4 ( __B ) - Z2 ( __C ) ) * h_factor + Z4 ( CEN ) ;
e_delta [ C_AXIS ] = ( - Z2 ( __A ) - Z2 ( __B ) + Z4 ( __C ) ) * h_factor + Z4 ( CEN ) ;
r_delta = ( + Z4 ( __A ) + Z4 ( __B ) + Z4 ( __C ) - Z12 ( CEN ) ) * r_factor ;
}
else {
e_delta [ A_AXIS ] = ( Z6 ( CEN ) - Z4 ( _BC ) + Z2 ( _CA ) + Z2 ( _AB ) ) * h_factor ;
e_delta [ B_AXIS ] = ( Z6 ( CEN ) + Z2 ( _BC ) - Z4 ( _CA ) + Z2 ( _AB ) ) * h_factor ;
e_delta [ C_AXIS ] = ( Z6 ( CEN ) + Z2 ( _BC ) + Z2 ( _CA ) - Z4 ( _AB ) ) * h_factor ;
r_delta = ( Z6 ( CEN ) - Z2 ( _BC ) - Z2 ( _CA ) - Z2 ( _AB ) ) * r_factor ;
else { // see 4 point calibration (opposites) matrix
e_delta [ A_AXIS ] = ( - Z4 ( _BC ) + Z2 ( _CA ) + Z2 ( _AB ) ) * h_factor + Z4 ( CEN ) ;
e_delta [ B_AXIS ] = ( + Z2 ( _BC ) - Z4 ( _CA ) + Z2 ( _AB ) ) * h_factor + Z4 ( CEN ) ;
e_delta [ C_AXIS ] = ( + Z2 ( _BC ) + Z2 ( _CA ) - Z4 ( _AB ) ) * h_factor + Z4 ( CEN ) ;
r_delta = ( + Z4 ( _BC ) + Z4 ( _CA ) + Z4 ( _AB ) - Z12 ( CEN ) ) * r_factor ;
}
break ;
default :
e_delta [ A_AXIS ] = ( Z6 ( CEN ) + Z2 ( __A ) - Z1 ( __B ) - Z1 ( __C ) - Z2 ( _BC ) + Z1 ( _CA ) + Z1 ( _AB ) ) * h_factor ;
e_delta [ B_AXIS ] = ( Z6 ( CEN ) - Z1 ( __A ) + Z2 ( __B ) - Z1 ( __C ) + Z1 ( _BC ) - Z2 ( _CA ) + Z1 ( _AB ) ) * h_factor ;
e_delta [ C_AXIS ] = ( Z6 ( CEN ) - Z1 ( __A ) - Z1 ( __B ) + Z2 ( __C ) + Z1 ( _BC ) + Z1 ( _CA ) - Z2 ( _AB ) ) * h_factor ;
r_delta = ( Z6 ( CEN ) - Z1 ( __A ) - Z1 ( __B ) - Z1 ( __C ) - Z1 ( _BC ) - Z1 ( _CA ) - Z1 ( _AB ) ) * r_factor ;
default : // see 7 point calibration (towers & opposites) matrix
e_delta [ A_AXIS ] = ( + Z2 ( __A ) - Z1 ( __B ) - Z1 ( __C ) - Z2 ( _BC ) + Z1 ( _CA ) + Z1 ( _AB ) ) * h_factor + Z4 ( CEN ) ;
e_delta [ B_AXIS ] = ( - Z1 ( __A ) + Z2 ( __B ) - Z1 ( __C ) + Z1 ( _BC ) - Z2 ( _CA ) + Z1 ( _AB ) ) * h_factor + Z4 ( CEN ) ;
e_delta [ C_AXIS ] = ( - Z1 ( __A ) - Z1 ( __B ) + Z2 ( __C ) + Z1 ( _BC ) + Z1 ( _CA ) - Z2 ( _AB ) ) * h_factor + Z4 ( CEN ) ;
r_delta = ( + Z2 ( __A ) + Z2 ( __B ) + Z2 ( __C ) + Z2 ( _BC ) + Z2 ( _CA ) + Z2 ( _AB ) - Z12 ( CEN ) ) * r_factor ;
if ( towers_set ) {
t_delta [ A_AXIS ] = ( - Z4 ( __B ) + Z4 ( __C ) - Z4 ( _CA ) + Z4 ( _AB ) ) * a_factor ;
t_delta [ B_AXIS ] = ( Z4 ( __A ) - Z4 ( __C ) + Z4 ( _BC ) - Z4 ( _AB ) ) * a_factor ;
t_delta [ C_AXIS ] = ( - Z4 ( __A ) + Z4 ( __B ) - Z4 ( _BC ) + Z4 ( _CA ) ) * a_factor ;
e_delta [ A_AXIS ] + = ( t_delta [ B_AXIS ] - t_delta [ C_AXIS ] ) / 4.5 ;
e_delta [ B_AXIS ] + = ( t_delta [ C_AXIS ] - t_delta [ A_AXIS ] ) / 4.5 ;
e_delta [ C_AXIS ] + = ( t_delta [ A_AXIS ] - t_delta [ B_AXIS ] ) / 4.5 ;
if ( towers_set ) { // see 7 point tower angle calibration (towers & opposites) matrix
t_delta [ A_AXIS ] = ( + Z0 ( __A ) - Z4 ( __B ) + Z4 ( __C ) + Z0 ( _BC ) - Z4 ( _CA ) + Z4 ( _AB ) + Z0 ( CEN ) ) * a_factor ;
t_delta [ B_AXIS ] = ( + Z4 ( __A ) + Z0 ( __B ) - Z4 ( __C ) + Z4 ( _BC ) + Z0 ( _CA ) - Z4 ( _AB ) + Z0 ( CEN ) ) * a_factor ;
t_delta [ C_AXIS ] = ( - Z4 ( __A ) + Z4 ( __B ) + Z0 ( __C ) - Z4 ( _BC ) + Z4 ( _CA ) + Z0 ( _AB ) + Z0 ( CEN ) ) * a_factor ;
}
break ;
}
LOOP_XYZ ( axis ) delta_endstop_adj [ axis ] + = e_delta [ axis ] ;
delta_radius + = r_delta ;
LOOP_XYZ ( axis ) delta_tower_angle_trim [ axis ] + = t_delta [ axis ] ;
}
else if ( zero_std_dev > = test_precision ) { // step one back
else if ( zero_std_dev > = test_precision ) {
// roll back
COPY ( delta_endstop_adj , e_old ) ;
delta_radius = d r_old;
delta_height = z h_old;
COPY ( delta_tower_angle_trim , t a_old) ;
delta_radius = ;
delta_height = ;
COPY ( delta_tower_angle_trim , ) ;
}
if ( verbose_level ! = 0 ) { // !dry run
// normalise angles to least squares
if ( _angle_results ) {
float a_sum = 0.0 ;
@ -628,15 +666,15 @@ void GcodeSuite::G33() {
// print report
if ( verbose_level > 2 )
print_ G33 _results( z_at_pt , _tower_results , _opposite_results ) ;
if ( verbose_level == 3 )
print_ calibration _results( z_at_pt , _tower_results , _opposite_results ) ;
if ( verbose_level ! = 0 ) { // !dry run
if ( ( zero_std_dev > = test_precision & & iterations > force_iterations ) | | zero_std_dev < = calibration_precision ) { // end iterations
if ( verbose_level ! = 0 ) { // !dry run
if ( ( zero_std_dev > = test_precision & & iterations > force_iterations ) | | zero_std_dev < = calibration_precision ) { // end iterations
SERIAL_PROTOCOLPGM ( " Calibration OK " ) ;
SERIAL_PROTOCOL_SP ( 32 ) ;
# if HAS_BED_PROBE
if ( zero_std_dev > = test_precision & & ! _1p_calibration )
if ( zero_std_dev > = test_precision & & ! _1p_calibration & & ! _0p_calibration )
SERIAL_PROTOCOLPGM ( " rolling back. " ) ;
else
# endif
@ -652,11 +690,11 @@ void GcodeSuite::G33() {
else
sprintf_P ( & mess [ 15 ] , PSTR ( " %03i.x " ) , ( int ) round ( zero_std_dev_min ) ) ;
lcd_setstatus ( mess ) ;
print_ G33 _settings( _endstop_results , _angle_results ) ;
print_ calibration _settings( _endstop_results , _angle_results ) ;
serialprintPGM ( save_message ) ;
SERIAL_EOL ( ) ;
}
else { // !end iterations
else { // !end iterations
char mess [ 15 ] ;
if ( iterations < 31 )
sprintf_P ( mess , PSTR ( " Iteration : %02i " ) , ( int ) iterations ) ;
@ -669,11 +707,11 @@ void GcodeSuite::G33() {
SERIAL_EOL ( ) ;
lcd_setstatus ( mess ) ;
if ( verbose_level > 1 )
print_ G33 _settings( _endstop_results , _angle_results ) ;
print_ calibration _settings( _endstop_results , _angle_results ) ;
}
}
else { // dry run
PGM_P enddryrun = PSTR ( " End DRY-RUN " ) ;
else { // dry run
const char * enddryrun = PSTR ( " End DRY-RUN " ) ;
serialprintPGM ( enddryrun ) ;
SERIAL_PROTOCOL_SP ( 35 ) ;
SERIAL_PROTOCOLPGM ( " std dev: " ) ;
@ -689,16 +727,11 @@ void GcodeSuite::G33() {
sprintf_P ( & mess [ 15 ] , PSTR ( " %03i.x " ) , ( int ) round ( zero_std_dev ) ) ;
lcd_setstatus ( mess ) ;
}
endstops . enable ( true ) ;
if ( ! home_delta ( ) )
return ;
endstops . not_homing ( ) ;
if ( ! ac_home ( ) ) return ;
}
while ( ( ( zero_std_dev < test_precision & & iterations < 31 ) | | iterations < = force_iterations ) & & zero_std_dev > calibration_precision ) ;
G33 _CLEANUP( ) ;
AC_CLEANUP ( ) ;
}
# endif // DELTA_AUTO_CALIBRATION
Luc Van Daele
7 years ago
committed by
Scott Lahteine