MPCNC/Firmware
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge pull request #1764 from thinkyhead/probe_leveling

Browse Source 
Fix homing and leveling
This commit is contained in:
Scott Lahteine 2015-04-01 02:00:39 -07:00
commit 9593f09b99
3 changed files with 315 additions and 319 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
Marlin/Conditionals.h
View File

@ -4,6 +4,10 @@
*/ */
#ifndef CONDITIONALS_H #ifndef CONDITIONALS_H
#ifndef M_PI
#define M_PI 3.1415926536
#endif
#ifndef CONFIGURATION_LCD // Get the LCD defines which are needed first #ifndef CONFIGURATION_LCD // Get the LCD defines which are needed first
#define CONFIGURATION_LCD #define CONFIGURATION_LCD
@ -252,7 +256,7 @@
* Advance calculated values * Advance calculated values
*/ */
#ifdef ADVANCE #ifdef ADVANCE
#define EXTRUSION_AREA (0.25 * D_FILAMENT * D_FILAMENT * 3.14159) #define EXTRUSION_AREA (0.25 * D_FILAMENT * D_FILAMENT * M_PI)
#define STEPS_PER_CUBIC_MM_E (axis_steps_per_unit[E_AXIS] / EXTRUSION_AREA) #define STEPS_PER_CUBIC_MM_E (axis_steps_per_unit[E_AXIS] / EXTRUSION_AREA)
#endif #endif

2
Marlin/Marlin.h
View File

@ -29,6 +29,8 @@
#define BIT(b) (1<<(b)) #define BIT(b) (1<<(b))
#define TEST(n,b) (((n)&BIT(b))!=0) #define TEST(n,b) (((n)&BIT(b))!=0)
#define RADIANS(d) ((d)*M_PI/180.0)
#define DEGREES(r) ((d)*180.0/M_PI)
// Arduino < 1.0.0 does not define this, so we need to do it ourselves // Arduino < 1.0.0 does not define this, so we need to do it ourselves
#ifndef analogInputToDigitalPin #ifndef analogInputToDigitalPin

376
Marlin/Marlin_main.cpp
View File

@ -1034,6 +1034,12 @@ inline void line_to_destination() {
inline void sync_plan_position() { inline void sync_plan_position() {
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]);
} }
#if defined(DELTA) || defined(SCARA)
inline void sync_plan_position_delta() {
calculate_delta(current_position);
plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]);
}
#endif
#ifdef ENABLE_AUTO_BED_LEVELING #ifdef ENABLE_AUTO_BED_LEVELING
@ -1103,14 +1109,13 @@ inline void sync_plan_position() {
destination[Z_AXIS] = -10; destination[Z_AXIS] = -10;
prepare_move_raw(); prepare_move_raw();
st_synchronize(); st_synchronize();
endstops_hit_on_purpose(); endstops_hit_on_purpose(); // clear endstop hit flags
// we have to let the planner know where we are right now as it is not where we said to go. // 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); long stop_steps = st_get_position(Z_AXIS);
float mm = start_z - float(start_steps - stop_steps) / axis_steps_per_unit[Z_AXIS]; float mm = start_z - float(start_steps - stop_steps) / axis_steps_per_unit[Z_AXIS];
current_position[Z_AXIS] = mm; current_position[Z_AXIS] = mm;
calculate_delta(current_position); sync_plan_position_delta();
plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]);
#else // !DELTA #else // !DELTA
@ -1130,7 +1135,7 @@ inline void sync_plan_position() {
zPosition += home_retract_mm(Z_AXIS); zPosition += home_retract_mm(Z_AXIS);
line_to_z(zPosition); line_to_z(zPosition);
st_synchronize(); st_synchronize();
endstops_hit_on_purpose(); endstops_hit_on_purpose(); // clear endstop hit flags
// move back down slowly to find bed // move back down slowly to find bed
if (homing_bump_divisor[Z_AXIS] >= 1) if (homing_bump_divisor[Z_AXIS] >= 1)
@ -1143,7 +1148,7 @@ inline void sync_plan_position() {
zPosition -= home_retract_mm(Z_AXIS) * 2; zPosition -= home_retract_mm(Z_AXIS) * 2;
line_to_z(zPosition); line_to_z(zPosition);
st_synchronize(); st_synchronize();
endstops_hit_on_purpose(); endstops_hit_on_purpose(); // clear endstop hit flags
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
@ -1262,7 +1267,7 @@ inline void sync_plan_position() {
if (servo_endstops[Z_AXIS] >= 0) { if (servo_endstops[Z_AXIS] >= 0) {
#if Z_RAISE_AFTER_PROBING > 0 #if Z_RAISE_AFTER_PROBING > 0
do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], Z_RAISE_AFTER_PROBING); do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] + Z_RAISE_AFTER_PROBING);
st_synchronize(); st_synchronize();
#endif #endif
@ -1345,7 +1350,7 @@ inline void sync_plan_position() {
#if Z_RAISE_BETWEEN_PROBINGS > 0 #if Z_RAISE_BETWEEN_PROBINGS > 0
if (retract_action == ProbeStay) { if (retract_action == ProbeStay) {
do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], Z_RAISE_BETWEEN_PROBINGS); do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS);
st_synchronize(); st_synchronize();
} }
#endif #endif
@ -1430,13 +1435,17 @@ inline void sync_plan_position() {
#endif // ENABLE_AUTO_BED_LEVELING #endif // ENABLE_AUTO_BED_LEVELING
/**
* Home an individual axis
*/
#define HOMEAXIS(LETTER) homeaxis(LETTER##_AXIS)
static void homeaxis(int axis) { static void homeaxis(int axis) {
#define HOMEAXIS_DO(LETTER) \ #define HOMEAXIS_DO(LETTER) \
((LETTER##_MIN_PIN > -1 && LETTER##_HOME_DIR==-1) || (LETTER##_MAX_PIN > -1 && LETTER##_HOME_DIR==1)) ((LETTER##_MIN_PIN > -1 && LETTER##_HOME_DIR==-1) || (LETTER##_MAX_PIN > -1 && LETTER##_HOME_DIR==1))
if (axis == X_AXIS ? HOMEAXIS_DO(X) : if (axis == X_AXIS ? HOMEAXIS_DO(X) : axis == Y_AXIS ? HOMEAXIS_DO(Y) : axis == Z_AXIS ? HOMEAXIS_DO(Z) : 0) {
axis == Y_AXIS ? HOMEAXIS_DO(Y) :
axis == Z_AXIS ? HOMEAXIS_DO(Z) : 0) {
int axis_home_dir; int axis_home_dir;
@ -1446,71 +1455,78 @@ static void homeaxis(int axis) {
axis_home_dir = home_dir(axis); axis_home_dir = home_dir(axis);
#endif #endif
// Set the axis position as setup for the move
current_position[axis] = 0; current_position[axis] = 0;
sync_plan_position(); sync_plan_position();
#ifndef Z_PROBE_SLED
// Engage Servo endstop if enabled // Engage Servo endstop if enabled
#ifdef SERVO_ENDSTOPS #ifdef SERVO_ENDSTOPS && !defined(Z_PROBE_SLED)
#if SERVO_LEVELING
if (axis == Z_AXIS) {
engage_z_probe();
}
else
#endif // SERVO_LEVELING
#if SERVO_LEVELING
if (axis == Z_AXIS) engage_z_probe(); else
#endif
{
if (servo_endstops[axis] > -1) if (servo_endstops[axis] > -1)
servos[servo_endstops[axis]].write(servo_endstop_angles[axis * 2]); servos[servo_endstops[axis]].write(servo_endstop_angles[axis * 2]);
}
#endif // SERVO_ENDSTOPS #endif // SERVO_ENDSTOPS && !Z_PROBE_SLED
#endif // Z_PROBE_SLED
#ifdef Z_DUAL_ENDSTOPS #ifdef Z_DUAL_ENDSTOPS
if (axis == Z_AXIS) In_Homing_Process(true); if (axis == Z_AXIS) In_Homing_Process(true);
#endif #endif
// Move towards the endstop until an endstop is triggered
destination[axis] = 1.5 * max_length(axis) * axis_home_dir; destination[axis] = 1.5 * max_length(axis) * axis_home_dir;
feedrate = homing_feedrate[axis]; feedrate = homing_feedrate[axis];
line_to_destination(); line_to_destination();
st_synchronize(); st_synchronize();
// Set the axis position as setup for the move
current_position[axis] = 0; current_position[axis] = 0;
sync_plan_position(); sync_plan_position();
// Move away from the endstop by the axis HOME_RETRACT_MM
destination[axis] = -home_retract_mm(axis) * axis_home_dir; destination[axis] = -home_retract_mm(axis) * axis_home_dir;
line_to_destination(); line_to_destination();
st_synchronize(); st_synchronize();
destination[axis] = 2 * home_retract_mm(axis) * axis_home_dir; // Slow down the feedrate for the next move
if (homing_bump_divisor[axis] >= 1) if (homing_bump_divisor[axis] >= 1)
feedrate = homing_feedrate[axis] / homing_bump_divisor[axis]; feedrate = homing_feedrate[axis] / homing_bump_divisor[axis];
else { else {
feedrate = homing_feedrate[axis] / 10; feedrate = homing_feedrate[axis] / 10;
SERIAL_ECHOLN("Warning: The Homing Bump Feedrate Divisor cannot be less than 1"); SERIAL_ECHOLNPGM("Warning: The Homing Bump Feedrate Divisor cannot be less than 1");
} }
// Move slowly towards the endstop until triggered
destination[axis] = 2 * home_retract_mm(axis) * axis_home_dir;
line_to_destination(); line_to_destination();
st_synchronize(); st_synchronize();
#ifdef Z_DUAL_ENDSTOPS #ifdef Z_DUAL_ENDSTOPS
if (axis==Z_AXIS) if (axis == Z_AXIS) {
{ float adj = fabs(z_endstop_adj);
feedrate = homing_feedrate[axis]; bool lockZ1;
sync_plan_position(); if (axis_home_dir > 0) {
if (axis_home_dir > 0) adj = -adj;
{ lockZ1 = (z_endstop_adj > 0);
destination[axis] = (-1) * fabs(z_endstop_adj);
if (z_endstop_adj > 0) Lock_z_motor(true); else Lock_z2_motor(true);
} else {
destination[axis] = fabs(z_endstop_adj);
if (z_endstop_adj < 0) Lock_z_motor(true); else Lock_z2_motor(true);
} }
else
lockZ1 = (z_endstop_adj < 0);
if (lockZ1) Lock_z_motor(true); else Lock_z2_motor(true);
sync_plan_position();
// Move to the adjusted endstop height
feedrate = homing_feedrate[axis];
destination[Z_AXIS] = adj;
line_to_destination(); line_to_destination();
st_synchronize(); st_synchronize();
Lock_z_motor(false);
Lock_z2_motor(false); if (lockZ1) Lock_z_motor(false); else Lock_z2_motor(false);
In_Homing_Process(false); In_Homing_Process(false);
} } // Z_AXIS
#endif #endif
#ifdef DELTA #ifdef DELTA
@ -1522,89 +1538,81 @@ static void homeaxis(int axis) {
st_synchronize(); st_synchronize();
} }
#endif #endif
// Set the axis position to its home position (plus home offsets)
axis_is_at_home(axis); axis_is_at_home(axis);
destination[axis] = current_position[axis]; destination[axis] = current_position[axis];
feedrate = 0.0; feedrate = 0.0;
endstops_hit_on_purpose(); endstops_hit_on_purpose(); // clear endstop hit flags
axis_known_position[axis] = true; axis_known_position[axis] = true;
// Retract Servo endstop if enabled // Retract Servo endstop if enabled
#ifdef SERVO_ENDSTOPS #ifdef SERVO_ENDSTOPS
if (servo_endstops[axis] > -1) { if (servo_endstops[axis] > -1)
servos[servo_endstops[axis]].write(servo_endstop_angles[axis * 2 + 1]); servos[servo_endstops[axis]].write(servo_endstop_angles[axis * 2 + 1]);
}
#endif #endif
#if SERVO_LEVELING
#ifndef Z_PROBE_SLED #if SERVO_LEVELING && !defined(Z_PROBE_SLED)
if (axis == Z_AXIS) retract_z_probe(); if (axis == Z_AXIS) retract_z_probe();
#endif #endif
#endif
} }
} }
#define HOMEAXIS(LETTER) homeaxis(LETTER##_AXIS)
void refresh_cmd_timeout(void) void refresh_cmd_timeout(void) { previous_millis_cmd = millis(); }
{
previous_millis_cmd = millis();
}
#ifdef FWRETRACT #ifdef FWRETRACT
void retract(bool retracting, bool swapretract = false) { void retract(bool retracting, bool swapretract = false) {
if(retracting && !retracted[active_extruder]) {
destination[X_AXIS]=current_position[X_AXIS]; if (retracting == retracted[active_extruder]) return;
destination[Y_AXIS]=current_position[Y_AXIS];
destination[Z_AXIS]=current_position[Z_AXIS];
destination[E_AXIS]=current_position[E_AXIS];
if (swapretract) {
current_position[E_AXIS]+=retract_length_swap/volumetric_multiplier[active_extruder];
} else {
current_position[E_AXIS]+=retract_length/volumetric_multiplier[active_extruder];
}
plan_set_e_position(current_position[E_AXIS]);
float oldFeedrate = feedrate; float oldFeedrate = feedrate;
for (int i = 0; i < NUM_AXIS; i++) destination[i] = current_position[i];
if (retracting) {
feedrate = retract_feedrate * 60; feedrate = retract_feedrate * 60;
retracted[active_extruder]=true; current_position[E_AXIS] += (swapretract ? retract_length_swap : retract_length) / volumetric_multiplier[active_extruder];
plan_set_e_position(current_position[E_AXIS]);
prepare_move(); prepare_move();
if (retract_zlift > 0.01) { if (retract_zlift > 0.01) {
current_position[Z_AXIS] -= retract_zlift; current_position[Z_AXIS] -= retract_zlift;
#ifdef DELTA #ifdef DELTA
calculate_delta(current_position); // change cartesian kinematic to delta kinematic; sync_plan_position_delta();
plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]);
#else #else
sync_plan_position(); sync_plan_position();
#endif #endif
prepare_move(); prepare_move();
} }
feedrate = oldFeedrate; }
} else if(!retracting && retracted[active_extruder]) { else {
destination[X_AXIS]=current_position[X_AXIS];
destination[Y_AXIS]=current_position[Y_AXIS];
destination[Z_AXIS]=current_position[Z_AXIS];
destination[E_AXIS]=current_position[E_AXIS];
if (retract_zlift > 0.01) { if (retract_zlift > 0.01) {
current_position[Z_AXIS] + =retract_zlift; current_position[Z_AXIS] + =retract_zlift;
#ifdef DELTA #ifdef DELTA
calculate_delta(current_position); // change cartesian kinematic to delta kinematic; sync_plan_position_delta();
plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]);
#else #else
sync_plan_position(); sync_plan_position();
#endif #endif
//prepare_move(); //prepare_move();
} }
if (swapretract) {
current_position[E_AXIS]-=(retract_length_swap+retract_recover_length_swap)/volumetric_multiplier[active_extruder];
} else {
current_position[E_AXIS]-=(retract_length+retract_recover_length)/volumetric_multiplier[active_extruder];
}
plan_set_e_position(current_position[E_AXIS]);
float oldFeedrate = feedrate;
feedrate = retract_recover_feedrate * 60; feedrate = retract_recover_feedrate * 60;
retracted[active_extruder] = false; float move_e = swapretract ? retract_length_swap + retract_recover_length_swap : retract_length + retract_recover_length;
current_position[E_AXIS] -= move_e / volumetric_multiplier[active_extruder];
plan_set_e_position(current_position[E_AXIS]);
prepare_move(); prepare_move();
feedrate = oldFeedrate;
} }
} //retract
feedrate = oldFeedrate;
retracted[active_extruder] = retract;
} // retract()
#endif // FWRETRACT #endif // FWRETRACT
#ifdef Z_PROBE_SLED #ifdef Z_PROBE_SLED
@ -1620,9 +1628,7 @@ void refresh_cmd_timeout(void)
// offset[in] The additional distance to move to adjust docking location // offset[in] The additional distance to move to adjust docking location
// //
static void dock_sled(bool dock, int offset=0) { static void dock_sled(bool dock, int offset=0) {
int z_loc; if (!axis_known_position[X_AXIS] || !axis_known_position[Y_AXIS]) {
if (!((axis_known_position[X_AXIS]) && (axis_known_position[Y_AXIS]))) {
LCD_MESSAGEPGM(MSG_POSITION_UNKNOWN); LCD_MESSAGEPGM(MSG_POSITION_UNKNOWN);
SERIAL_ECHO_START; SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_POSITION_UNKNOWN); SERIAL_ECHOLNPGM(MSG_POSITION_UNKNOWN);
@ -1630,23 +1636,17 @@ static void dock_sled(bool dock, int offset=0) {
} }
if (dock) { if (dock) {
do_blocking_move_to(X_MAX_POS + SLED_DOCKING_OFFSET + offset, do_blocking_move_to(X_MAX_POS + SLED_DOCKING_OFFSET + offset, current_position[Y_AXIS], current_position[Z_AXIS]);
current_position[Y_AXIS], digitalWrite(SERVO0_PIN, LOW); // turn off magnet
current_position[Z_AXIS]);
// turn off magnet
digitalWrite(SERVO0_PIN, LOW);
} else { } else {
if (current_position[Z_AXIS] < (Z_RAISE_BEFORE_PROBING + 5)) float z_loc = current_position[Z_AXIS];
z_loc = Z_RAISE_BEFORE_PROBING; if (z_loc < Z_RAISE_BEFORE_PROBING + 5) z_loc = Z_RAISE_BEFORE_PROBING;
else do_blocking_move_to(X_MAX_POS + SLED_DOCKING_OFFSET + offset, Y_PROBE_OFFSET_FROM_EXTRUDER, z_loc);
z_loc = current_position[Z_AXIS]; digitalWrite(SERVO0_PIN, HIGH); // turn on magnet
do_blocking_move_to(X_MAX_POS + SLED_DOCKING_OFFSET + offset,
Y_PROBE_OFFSET_FROM_EXTRUDER, z_loc);
// turn on magnet
digitalWrite(SERVO0_PIN, HIGH);
} }
} }
#endif
#endif // Z_PROBE_SLED
/** /**
* *
@ -1788,7 +1788,7 @@ inline void gcode_G28() {
feedrate = 1.732 * homing_feedrate[X_AXIS]; feedrate = 1.732 * homing_feedrate[X_AXIS];
line_to_destination(); line_to_destination();
st_synchronize(); st_synchronize();
endstops_hit_on_purpose(); endstops_hit_on_purpose(); // clear endstop hit flags
// Destination reached // Destination reached
for (int i = X_AXIS; i <= Z_AXIS; i++) current_position[i] = destination[i]; for (int i = X_AXIS; i <= Z_AXIS; i++) current_position[i] = destination[i];
@ -1798,8 +1798,7 @@ inline void gcode_G28() {
HOMEAXIS(Y); HOMEAXIS(Y);
HOMEAXIS(Z); HOMEAXIS(Z);
calculate_delta(current_position); sync_plan_position_delta();
plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]);
#else // NOT DELTA #else // NOT DELTA
@ -1807,7 +1806,7 @@ inline void gcode_G28() {
homeY = code_seen(axis_codes[Y_AXIS]), homeY = code_seen(axis_codes[Y_AXIS]),
homeZ = code_seen(axis_codes[Z_AXIS]); homeZ = code_seen(axis_codes[Z_AXIS]);
home_all_axis = !homeX && !homeY && !homeZ; // No parameters means home all axes home_all_axis = !(homeX || homeY || homeZ) || (homeX && homeY && homeZ);
#if Z_HOME_DIR > 0 // If homing away from BED do Z first #if Z_HOME_DIR > 0 // If homing away from BED do Z first
@ -1826,7 +1825,9 @@ inline void gcode_G28() {
#endif #endif
#ifdef QUICK_HOME #ifdef QUICK_HOME
if (home_all_axis || (homeX && homeY)) { //first diagonal move
if (home_all_axis || (homeX && homeY)) { // First diagonal move
current_position[X_AXIS] = current_position[Y_AXIS] = 0; current_position[X_AXIS] = current_position[Y_AXIS] = 0;
#ifdef DUAL_X_CARRIAGE #ifdef DUAL_X_CARRIAGE
@ -1837,27 +1838,26 @@ inline void gcode_G28() {
#endif #endif
sync_plan_position(); sync_plan_position();
destination[X_AXIS] = 1.5 * max_length(X_AXIS) * x_axis_home_dir;
destination[Y_AXIS] = 1.5 * max_length(Y_AXIS) * home_dir(Y_AXIS); float mlx = max_length(X_AXIS), mly = max_length(Y_AXIS),
feedrate = homing_feedrate[X_AXIS]; mlratio = mlx>mly ? mly/mlx : mlx/mly;
if (homing_feedrate[Y_AXIS] < feedrate) feedrate = homing_feedrate[Y_AXIS];
if (max_length(X_AXIS) > max_length(Y_AXIS)) { destination[X_AXIS] = 1.5 * mlx * x_axis_home_dir;
feedrate *= sqrt(pow(max_length(Y_AXIS) / max_length(X_AXIS), 2) + 1); destination[Y_AXIS] = 1.5 * mly * home_dir(Y_AXIS);
} else { feedrate = min(homing_feedrate[X_AXIS], homing_feedrate[Y_AXIS]) * sqrt(mlratio * mlratio + 1);
feedrate *= sqrt(pow(max_length(X_AXIS) / max_length(Y_AXIS), 2) + 1);
}
line_to_destination(); line_to_destination();
st_synchronize(); st_synchronize();
axis_is_at_home(X_AXIS); axis_is_at_home(X_AXIS);
axis_is_at_home(Y_AXIS); axis_is_at_home(Y_AXIS);
sync_plan_position(); sync_plan_position();
destination[X_AXIS] = current_position[X_AXIS]; destination[X_AXIS] = current_position[X_AXIS];
destination[Y_AXIS] = current_position[Y_AXIS]; destination[Y_AXIS] = current_position[Y_AXIS];
line_to_destination(); line_to_destination();
feedrate = 0.0; feedrate = 0.0;
st_synchronize(); st_synchronize();
endstops_hit_on_purpose(); endstops_hit_on_purpose(); // clear endstop hit flags
current_position[X_AXIS] = destination[X_AXIS]; current_position[X_AXIS] = destination[X_AXIS];
current_position[Y_AXIS] = destination[Y_AXIS]; current_position[Y_AXIS] = destination[Y_AXIS];
@ -1865,6 +1865,7 @@ inline void gcode_G28() {
current_position[Z_AXIS] = destination[Z_AXIS]; current_position[Z_AXIS] = destination[Z_AXIS];
#endif #endif
} }
#endif // QUICK_HOME #endif // QUICK_HOME
// Home X // Home X
@ -1947,7 +1948,7 @@ inline void gcode_G28() {
&& cpy >= Y_MIN_POS - Y_PROBE_OFFSET_FROM_EXTRUDER && cpy >= Y_MIN_POS - Y_PROBE_OFFSET_FROM_EXTRUDER
&& cpy <= Y_MAX_POS - Y_PROBE_OFFSET_FROM_EXTRUDER) { && cpy <= Y_MAX_POS - Y_PROBE_OFFSET_FROM_EXTRUDER) {
current_position[Z_AXIS] = 0; current_position[Z_AXIS] = 0;
plan_set_position(cpx, cpy, current_position[Z_AXIS], current_position[E_AXIS]); plan_set_position(cpx, cpy, 0, current_position[E_AXIS]);
destination[Z_AXIS] = -Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS); // Set destination away from bed destination[Z_AXIS] = -Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS); // Set destination away from bed
feedrate = max_feedrate[Z_AXIS]; feedrate = max_feedrate[Z_AXIS];
line_to_destination(); line_to_destination();
@ -1986,8 +1987,7 @@ inline void gcode_G28() {
#endif // else DELTA #endif // else DELTA
#ifdef SCARA #ifdef SCARA
calculate_delta(current_position); sync_plan_position_delta();
plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]);
#endif #endif
#ifdef ENDSTOPS_ONLY_FOR_HOMING #ifdef ENDSTOPS_ONLY_FOR_HOMING
@ -2014,7 +2014,7 @@ inline void gcode_G28() {
feedrate = saved_feedrate; feedrate = saved_feedrate;
feedmultiply = saved_feedmultiply; feedmultiply = saved_feedmultiply;
previous_millis_cmd = millis(); previous_millis_cmd = millis();
endstops_hit_on_purpose(); endstops_hit_on_purpose(); // clear endstop hit flags
} }
#if defined(MESH_BED_LEVELING) || defined(ENABLE_AUTO_BED_LEVELING) #if defined(MESH_BED_LEVELING) || defined(ENABLE_AUTO_BED_LEVELING)
@ -2186,8 +2186,7 @@ inline void gcode_G28() {
bool do_topography_map = verbose_level > 2 || code_seen('T') || code_seen('t'); bool do_topography_map = verbose_level > 2 || code_seen('T') || code_seen('t');
#endif #endif
if (verbose_level > 0) if (verbose_level > 0) {
{
SERIAL_PROTOCOLPGM("G29 Auto Bed Leveling\n"); SERIAL_PROTOCOLPGM("G29 Auto Bed Leveling\n");
if (dryrun) SERIAL_ECHOLN("Running in DRY-RUN mode"); if (dryrun) SERIAL_ECHOLN("Running in DRY-RUN mode");
} }
@ -2262,7 +2261,6 @@ inline void gcode_G28() {
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;
sync_plan_position(); sync_plan_position();
#endif // !DELTA #endif // !DELTA
} }
@ -2273,8 +2271,8 @@ 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
const 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),
const int yGridSpacing = (back_probe_bed_position - front_probe_bed_position) / (auto_bed_leveling_grid_points-1); yGridSpacing = (back_probe_bed_position - front_probe_bed_position) / (auto_bed_leveling_grid_points - 1);
#ifdef DELTA #ifdef DELTA
delta_grid_spacing[0] = xGridSpacing; delta_grid_spacing[0] = xGridSpacing;
@ -2826,9 +2824,7 @@ inline void gcode_M42() {
inline void gcode_M48() { inline void gcode_M48() {
double sum = 0.0, mean = 0.0, sigma = 0.0, sample_set[50]; double sum = 0.0, mean = 0.0, sigma = 0.0, sample_set[50];
int verbose_level = 1, n = 0, j, n_samples = 10, n_legs = 0, engage_probe_for_each_reading = 0; int verbose_level = 1, n_samples = 10, n_legs = 0;
double X_current, Y_current, Z_current;
double X_probe_location, Y_probe_location, Z_start_location, ext_position;
if (code_seen('V') || code_seen('v')) { if (code_seen('V') || code_seen('v')) {
verbose_level = code_value(); verbose_level = code_value();
@ -2849,14 +2845,14 @@ inline void gcode_M42() {
} }
} }
X_current = X_probe_location = st_get_position_mm(X_AXIS); double X_probe_location, Y_probe_location,
Y_current = Y_probe_location = st_get_position_mm(Y_AXIS); X_current = X_probe_location = st_get_position_mm(X_AXIS),
Z_current = st_get_position_mm(Z_AXIS); Y_current = Y_probe_location = st_get_position_mm(Y_AXIS),
Z_start_location = st_get_position_mm(Z_AXIS) + Z_RAISE_BEFORE_PROBING; Z_current = st_get_position_mm(Z_AXIS),
Z_start_location = Z_current + Z_RAISE_BEFORE_PROBING,
ext_position = st_get_position_mm(E_AXIS); ext_position = st_get_position_mm(E_AXIS);
if (code_seen('E') || code_seen('e')) bool engage_probe_for_each_reading = code_seen('E') || code_seen('e');
engage_probe_for_each_reading++;
if (code_seen('X') || code_seen('x')) { if (code_seen('X') || code_seen('x')) {
X_probe_location = code_value() - X_PROBE_OFFSET_FROM_EXTRUDER; X_probe_location = code_value() - X_PROBE_OFFSET_FROM_EXTRUDER;
@ -2936,33 +2932,29 @@ inline void gcode_M42() {
if (engage_probe_for_each_reading) retract_z_probe(); if (engage_probe_for_each_reading) retract_z_probe();
for (n=0; n < n_samples; n++) { for (uint16_t n=0; n < n_samples; n++) {
do_blocking_move_to(X_probe_location, Y_probe_location, Z_start_location); // Make sure we are at the probe location do_blocking_move_to(X_probe_location, Y_probe_location, Z_start_location); // Make sure we are at the probe location
if (n_legs) { if (n_legs) {
double radius=0.0, theta=0.0; unsigned long ms = millis();
int l; double radius = ms % (X_MAX_LENGTH / 4), // limit how far out to go
int rotational_direction = (unsigned long) millis() & 0x0001; // clockwise or counter clockwise theta = RADIANS(ms % 360L);
radius = (unsigned long)millis() % (long)(X_MAX_LENGTH / 4); // limit how far out to go float dir = (ms & 0x0001) ? 1 : -1; // clockwise or counter clockwise
theta = (float)((unsigned long)millis() % 360L) / (360. / (2 * 3.1415926)); // turn into radians
//SERIAL_ECHOPAIR("starting radius: ",radius); //SERIAL_ECHOPAIR("starting radius: ",radius);
//SERIAL_ECHOPAIR(" theta: ",theta); //SERIAL_ECHOPAIR(" theta: ",theta);
//SERIAL_ECHOPAIR(" direction: ",rotational_direction); //SERIAL_ECHOPAIR(" direction: ",dir);
//SERIAL_EOL; //SERIAL_EOL;
float dir = rotational_direction ? 1 : -1; for (int l = 0; l < n_legs - 1; l++) {
for (l = 0; l < n_legs - 1; l++) { ms = millis();
theta += dir * (float)((unsigned long)millis() % 20L) / (360.0/(2*3.1415926)); // turn into radians theta += RADIANS(dir * (ms % 20L));
radius += (ms % 10L) - 5L;
radius += (float)(((long)((unsigned long) millis() % 10L)) - 5L);
if (radius < 0.0) radius = -radius; if (radius < 0.0) radius = -radius;
X_current = X_probe_location + cos(theta) * radius; X_current = X_probe_location + cos(theta) * radius;
Y_current = Y_probe_location + sin(theta) * radius; Y_current = Y_probe_location + sin(theta) * radius;
// Make sure our X & Y are sane
X_current = constrain(X_current, X_MIN_POS, X_MAX_POS); X_current = constrain(X_current, X_MIN_POS, X_MAX_POS);
Y_current = constrain(Y_current, Y_MIN_POS, Y_MAX_POS); Y_current = constrain(Y_current, Y_MIN_POS, Y_MAX_POS);
@ -2973,9 +2965,12 @@ inline void gcode_M42() {
} }
do_blocking_move_to(X_current, Y_current, Z_current); do_blocking_move_to(X_current, Y_current, Z_current);
}
} // n_legs loop
do_blocking_move_to(X_probe_location, Y_probe_location, Z_start_location); // Go back to the probe location do_blocking_move_to(X_probe_location, Y_probe_location, Z_start_location); // Go back to the probe location
}
} // n_legs
if (engage_probe_for_each_reading) { if (engage_probe_for_each_reading) {
engage_z_probe(); engage_z_probe();
@ -2991,36 +2986,37 @@ inline void gcode_M42() {
// Get the current mean for the data points we have so far // Get the current mean for the data points we have so far
// //
sum = 0.0; sum = 0.0;
for (j=0; j<=n; j++) sum += sample_set[j]; for (int j = 0; j <= n; j++) sum += sample_set[j];
mean = sum / (double (n+1)); mean = sum / (n + 1);
// //
// Now, use that mean to calculate the standard deviation for the // Now, use that mean to calculate the standard deviation for the
// data points we have so far // data points we have so far
// //
sum = 0.0; sum = 0.0;
for (j=0; j<=n; j++) sum += (sample_set[j]-mean) * (sample_set[j]-mean); for (int j = 0; j <= n; j++) {
sigma = sqrt( sum / (double (n+1)) ); float ss = sample_set[j] - mean;
sum += ss * ss;
}
sigma = sqrt(sum / (n + 1));
if (verbose_level > 1) { if (verbose_level > 1) {
SERIAL_PROTOCOL(n+1); SERIAL_PROTOCOL(n+1);
SERIAL_PROTOCOL(" of "); SERIAL_PROTOCOLPGM(" of ");
SERIAL_PROTOCOL(n_samples); SERIAL_PROTOCOL(n_samples);
SERIAL_PROTOCOLPGM(" z: "); SERIAL_PROTOCOLPGM(" z: ");
SERIAL_PROTOCOL_F(current_position[Z_AXIS], 6); SERIAL_PROTOCOL_F(current_position[Z_AXIS], 6);
}
if (verbose_level > 2) { if (verbose_level > 2) {
SERIAL_PROTOCOL(" mean: "); SERIAL_PROTOCOLPGM(" mean: ");
SERIAL_PROTOCOL_F(mean,6); SERIAL_PROTOCOL_F(mean,6);
SERIAL_PROTOCOL(" sigma: "); SERIAL_PROTOCOLPGM(" sigma: ");
SERIAL_PROTOCOL_F(sigma,6); SERIAL_PROTOCOL_F(sigma,6);
} }
}
if (verbose_level > 0) SERIAL_EOL; if (verbose_level > 0) SERIAL_EOL;
plan_buffer_line(X_probe_location, Y_probe_location, Z_start_location, plan_buffer_line(X_probe_location, Y_probe_location, Z_start_location, current_position[E_AXIS], homing_feedrate[Z_AXIS]/60, active_extruder);
current_position[E_AXIS], homing_feedrate[Z_AXIS]/60, active_extruder);
st_synchronize(); st_synchronize();
if (engage_probe_for_each_reading) { if (engage_probe_for_each_reading) {
@ -3029,8 +3025,10 @@ inline void gcode_M42() {
} }
} }
if (!engage_probe_for_each_reading) {
retract_z_probe(); retract_z_probe();
delay(1000); delay(1000);
}
clean_up_after_endstop_move(); clean_up_after_endstop_move();
@ -4674,9 +4672,7 @@ inline void gcode_T() {
active_extruder = tmp_extruder; active_extruder = tmp_extruder;
#endif // !DUAL_X_CARRIAGE #endif // !DUAL_X_CARRIAGE
#ifdef DELTA #ifdef DELTA
calculate_delta(current_position); // change cartesian kinematic to delta kinematic; sync_plan_position_delta();
//sent position to plan_set_position();
plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS],current_position[E_AXIS]);
#else #else
sync_plan_position(); sync_plan_position();
#endif #endif
@ -5265,8 +5261,8 @@ void clamp_to_software_endstops(float target[3])
} }
#ifdef DELTA #ifdef DELTA
void recalc_delta_settings(float radius, float diagonal_rod)
{ void recalc_delta_settings(float radius, float diagonal_rod) {
delta_tower1_x = -SIN_60 * radius; // front left tower delta_tower1_x = -SIN_60 * radius; // front left tower
delta_tower1_y = -COS_60 * radius; delta_tower1_y = -COS_60 * radius;
delta_tower2_x = SIN_60 * radius; // front right tower delta_tower2_x = SIN_60 * radius; // front right tower
@ -5276,8 +5272,7 @@ void recalc_delta_settings(float radius, float diagonal_rod)
delta_diagonal_rod_2 = sq(diagonal_rod); delta_diagonal_rod_2 = sq(diagonal_rod);
} }
void calculate_delta(float cartesian[3]) void calculate_delta(float cartesian[3]) {
{
delta[X_AXIS] = sqrt(delta_diagonal_rod_2 delta[X_AXIS] = sqrt(delta_diagonal_rod_2
- sq(delta_tower1_x-cartesian[X_AXIS]) - sq(delta_tower1_x-cartesian[X_AXIS])
- sq(delta_tower1_y-cartesian[Y_AXIS]) - sq(delta_tower1_y-cartesian[Y_AXIS])
@ -5302,27 +5297,25 @@ void calculate_delta(float cartesian[3])
} }
#ifdef ENABLE_AUTO_BED_LEVELING #ifdef ENABLE_AUTO_BED_LEVELING
// Adjust print surface height by linear interpolation over the bed_level array. // Adjust print surface height by linear interpolation over the bed_level array.
int delta_grid_spacing[2] = { 0, 0 }; int delta_grid_spacing[2] = { 0, 0 };
void adjust_delta(float cartesian[3]) void adjust_delta(float cartesian[3]) {
{ if (delta_grid_spacing[0] == 0 || delta_grid_spacing[1] == 0) return; // G29 not done!
if (delta_grid_spacing[0] == 0 || delta_grid_spacing[1] == 0)
return; // G29 not done
int half = (AUTO_BED_LEVELING_GRID_POINTS - 1) / 2; 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 h1 = 0.001 - half, h2 = half - 0.001,
float grid_y = max(0.001-half, min(half-0.001, cartesian[Y_AXIS] / delta_grid_spacing[1])); grid_x = max(h1, min(h2, cartesian[X_AXIS] / delta_grid_spacing[0])),
int floor_x = floor(grid_x); grid_y = max(h1, min(h2, cartesian[Y_AXIS] / delta_grid_spacing[1]));
int floor_y = floor(grid_y); int floor_x = floor(grid_x), floor_y = floor(grid_y);
float ratio_x = grid_x - floor_x; float ratio_x = grid_x - floor_x, ratio_y = grid_y - floor_y,
float ratio_y = grid_y - floor_y; z1 = bed_level[floor_x + half][floor_y + half],
float z1 = bed_level[floor_x+half][floor_y+half]; z2 = bed_level[floor_x + half][floor_y + half + 1],
float z2 = bed_level[floor_x+half][floor_y+half+1]; z3 = bed_level[floor_x + half + 1][floor_y + half],
float z3 = bed_level[floor_x+half+1][floor_y+half]; z4 = bed_level[floor_x + half + 1][floor_y + half + 1],
float z4 = bed_level[floor_x+half+1][floor_y+half+1]; left = (1 - ratio_y) * z1 + ratio_y * z2,
float left = (1-ratio_y)*z1 + ratio_y*z2; right = (1 - ratio_y) * z3 + ratio_y * z4,
float right = (1-ratio_y)*z3 + ratio_y*z4; offset = (1 - ratio_x) * left + ratio_x * right;
float offset = (1-ratio_x)*left + ratio_x*right;
delta[X_AXIS] += offset; delta[X_AXIS] += offset;
delta[Y_AXIS] += offset; delta[Y_AXIS] += offset;
@ -5346,23 +5339,21 @@ void adjust_delta(float cartesian[3])
} }
#endif // ENABLE_AUTO_BED_LEVELING #endif // ENABLE_AUTO_BED_LEVELING
void prepare_move_raw() void prepare_move_raw() {
{
previous_millis_cmd = millis(); previous_millis_cmd = millis();
calculate_delta(destination); calculate_delta(destination);
plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], (feedrate/60)*(feedmultiply/100.0), active_extruder);
destination[E_AXIS], feedrate*feedmultiply/60/100.0, for (int i = 0; i < NUM_AXIS; i++) current_position[i] = destination[i];
active_extruder);
for(int8_t i=0; i < NUM_AXIS; i++) {
current_position[i] = destination[i];
}
} }
#endif // DELTA #endif // DELTA
#if defined(MESH_BED_LEVELING) #if defined(MESH_BED_LEVELING)
#if !defined(MIN) #if !defined(MIN)
#define MIN(_v1, _v2) (((_v1) < (_v2)) ? (_v1) : (_v2)) #define MIN(_v1, _v2) (((_v1) < (_v2)) ? (_v1) : (_v2))
#endif // ! MIN #endif // ! MIN
// This function is used to split lines on mesh borders so each segment is only part of one mesh area // This function is used to split lines on mesh borders so each segment is only part of one mesh area
void mesh_plan_buffer_line(float x, float y, float z, const float e, float feed_rate, const uint8_t &extruder, uint8_t x_splits=0xff, uint8_t y_splits=0xff) void mesh_plan_buffer_line(float x, float y, float z, const float e, float feed_rate, const uint8_t &extruder, uint8_t x_splits=0xff, uint8_t y_splits=0xff)
{ {
@ -5434,8 +5425,7 @@ void mesh_plan_buffer_line(float x, float y, float z, const float e, float feed_
} }
#endif // MESH_BED_LEVELING #endif // MESH_BED_LEVELING
void prepare_move() void prepare_move() {
{
clamp_to_software_endstops(destination); clamp_to_software_endstops(destination);
previous_millis_cmd = millis(); previous_millis_cmd = millis();
@ -5549,7 +5539,7 @@ void prepare_move()
} }
#endif //DUAL_X_CARRIAGE #endif //DUAL_X_CARRIAGE
#if ! (defined DELTA || defined SCARA) #if !defined(DELTA) && !defined(SCARA)
// Do not use feedmultiply for E or Z only moves // Do not use feedmultiply for E or Z only moves
if( (current_position[X_AXIS] == destination [X_AXIS]) && (current_position[Y_AXIS] == destination [Y_AXIS])) { if( (current_position[X_AXIS] == destination [X_AXIS]) && (current_position[Y_AXIS] == destination [Y_AXIS])) {
line_to_destination(); line_to_destination();