From 413b61e64aed70f0fd7afaee7e1018c0ff7a7a43 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Sun, 11 Sep 2016 21:02:37 -0500 Subject: [PATCH 1/6] Add HYPOT2 and float comparison macros --- Marlin/macros.h | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/Marlin/macros.h b/Marlin/macros.h index 351272804d70..b098597c5253 100644 --- a/Marlin/macros.h +++ b/Marlin/macros.h @@ -55,7 +55,8 @@ #endif #define RADIANS(d) ((d)*M_PI/180.0) #define DEGREES(r) ((r)*180.0/M_PI) -#define HYPOT(x,y) sqrt(sq(x)+sq(y)) +#define HYPOT2(x,y) (sq(x)+sq(y)) +#define HYPOT(x,y) sqrt(HYPOT2(x,y)) // Macros to contrain values #define NOLESS(v,n) do{ if (v < n) v = n; }while(0) @@ -124,4 +125,8 @@ #define MAX3(a, b, c) max(max(a, b), c) #define MAX4(a, b, c, d) max(max(max(a, b), c), d) +#define UNEAR_ZERO(x) ((x) < 0.000001) +#define NEAR_ZERO(x) ((x) > -0.000001 && (x) < 0.000001) +#define NEAR(x,y) NEAR_ZERO((x)-(y)) + #endif //__MACROS_H From 21514568fd455f7e61a87c114519495e4483f84a Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Sun, 11 Sep 2016 20:14:41 -0500 Subject: [PATCH 2/6] Add enum for ALL_AXES --- Marlin/enum.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Marlin/enum.h b/Marlin/enum.h index ded1be6ec7c4..18db5a6f5f31 100644 --- a/Marlin/enum.h +++ b/Marlin/enum.h @@ -42,7 +42,8 @@ enum AxisEnum { E_AXIS = 3, X_HEAD = 4, Y_HEAD = 5, - Z_HEAD = 6 + Z_HEAD = 6, + ALL_AXES = 100 }; #define LOOP_XYZ(VAR) for (uint8_t VAR=X_AXIS; VAR<=Z_AXIS; VAR++) From 6ab54c60b151aa52492bfec1a797a5475fb9027e Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Sun, 11 Sep 2016 20:11:30 -0500 Subject: [PATCH 3/6] Add conditionals for kinematics, leveling --- Marlin/Conditionals_post.h | 52 ++++++++++----- Marlin/Marlin.h | 2 +- Marlin/Marlin_main.cpp | 115 ++++++++++++++++++--------------- Marlin/configuration_store.cpp | 8 +-- Marlin/planner.cpp | 10 +-- Marlin/planner_bezier.cpp | 2 +- Marlin/qr_solve.cpp | 2 +- Marlin/ultralcd.cpp | 4 +- 8 files changed, 114 insertions(+), 81 deletions(-) diff --git a/Marlin/Conditionals_post.h b/Marlin/Conditionals_post.h index 239780f34676..9861c2b29515 100644 --- a/Marlin/Conditionals_post.h +++ b/Marlin/Conditionals_post.h @@ -61,12 +61,16 @@ #define NORMAL_AXIS X_AXIS #endif + #define IS_SCARA (ENABLED(MORGAN_SCARA) || ENABLED(MAKERARM_SCARA)) + #define IS_KINEMATIC (ENABLED(DELTA) || IS_SCARA) + #define IS_CARTESIAN !IS_KINEMATIC + /** - * SCARA + * SCARA cannot use SLOWDOWN and requires QUICKHOME */ - #if ENABLED(SCARA) + #if IS_SCARA #undef SLOWDOWN - #define QUICK_HOME //SCARA needs Quickhome + #define QUICK_HOME #endif /** @@ -132,12 +136,6 @@ #define HOMING_Z_WITH_PROBE (HAS_BED_PROBE && Z_HOME_DIR < 0 && ENABLED(Z_MIN_PROBE_USES_Z_MIN_ENDSTOP_PIN)) - // Boundaries for probing based on set limits - #define MIN_PROBE_X (max(X_MIN_POS, X_MIN_POS + X_PROBE_OFFSET_FROM_EXTRUDER)) - #define MAX_PROBE_X (min(X_MAX_POS, X_MAX_POS + X_PROBE_OFFSET_FROM_EXTRUDER)) - #define MIN_PROBE_Y (max(Y_MIN_POS, Y_MIN_POS + Y_PROBE_OFFSET_FROM_EXTRUDER)) - #define MAX_PROBE_Y (min(Y_MAX_POS, Y_MAX_POS + Y_PROBE_OFFSET_FROM_EXTRUDER)) - #define HAS_Z_SERVO_ENDSTOP (defined(Z_ENDSTOP_SERVO_NR) && Z_ENDSTOP_SERVO_NR >= 0) /** @@ -657,18 +655,28 @@ #ifndef DELTA_DIAGONAL_ROD_TRIM_TOWER_3 #define DELTA_DIAGONAL_ROD_TRIM_TOWER_3 0.0 #endif - #if ENABLED(AUTO_BED_LEVELING_GRID) - #define DELTA_BED_LEVELING_GRID - #endif #endif /** - * When not using other bed leveling... + * Specify the exact style of auto bed leveling + * + * 3POINT - 3 Point Probing with the least-squares solution. + * LINEAR - Grid Probing with the least-squares solution. + * NONLINEAR - Grid Probing with a mesh solution. Best for large beds. */ - #if ENABLED(AUTO_BED_LEVELING_FEATURE) && DISABLED(AUTO_BED_LEVELING_GRID) && DISABLED(DELTA_BED_LEVELING_GRID) - #define AUTO_BED_LEVELING_3POINT + #if ENABLED(AUTO_BED_LEVELING_FEATURE) + #if DISABLED(AUTO_BED_LEVELING_GRID) + #define AUTO_BED_LEVELING_LINEAR + #define AUTO_BED_LEVELING_3POINT + #elif IS_KINEMATIC + #define AUTO_BED_LEVELING_NONLINEAR + #else + #define AUTO_BED_LEVELING_LINEAR + #endif #endif + #define PLANNER_LEVELING (ENABLED(MESH_BED_LEVELING) || ENABLED(AUTO_BED_LEVELING_LINEAR)) + /** * Buzzer/Speaker */ @@ -702,4 +710,18 @@ #define Z_PROBE_TRAVEL_HEIGHT Z_HOMING_HEIGHT #endif + #if IS_KINEMATIC + // Check for this in the code instead + #define MIN_PROBE_X X_MIN_POS + #define MAX_PROBE_X X_MAX_POS + #define MIN_PROBE_Y Y_MIN_POS + #define MAX_PROBE_Y Y_MAX_POS + #else + // Boundaries for probing based on set limits + #define MIN_PROBE_X (max(X_MIN_POS, X_MIN_POS + X_PROBE_OFFSET_FROM_EXTRUDER)) + #define MAX_PROBE_X (min(X_MAX_POS, X_MAX_POS + X_PROBE_OFFSET_FROM_EXTRUDER)) + #define MIN_PROBE_Y (max(Y_MIN_POS, Y_MIN_POS + Y_PROBE_OFFSET_FROM_EXTRUDER)) + #define MAX_PROBE_Y (min(Y_MAX_POS, Y_MAX_POS + Y_PROBE_OFFSET_FROM_EXTRUDER)) + #endif + #endif // CONDITIONALS_POST_H diff --git a/Marlin/Marlin.h b/Marlin/Marlin.h index da9bc0de9031..e0a540c263b8 100644 --- a/Marlin/Marlin.h +++ b/Marlin/Marlin.h @@ -313,7 +313,7 @@ float code_value_temp_diff(); extern int delta_grid_spacing[2]; void adjust_delta(float cartesian[XYZ]); #endif -#elif ENABLED(SCARA) +#elif IS_SCARA extern float delta[ABC]; extern float axis_scaling[ABC]; // Build size scaling void inverse_kinematics(const float cartesian[XYZ]); diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index 4ea114504f0f..73376b4d1c64 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -36,12 +36,11 @@ #if ENABLED(AUTO_BED_LEVELING_FEATURE) #include "vector_3.h" - #if ENABLED(AUTO_BED_LEVELING_GRID) - #include "qr_solve.h" - #endif -#endif // AUTO_BED_LEVELING_FEATURE +#endif -#if ENABLED(MESH_BED_LEVELING) +#if ENABLED(AUTO_BED_LEVELING_LINEAR) + #include "qr_solve.h" +#elif ENABLED(MESH_BED_LEVELING) #include "mesh_bed_leveling.h" #endif @@ -497,7 +496,12 @@ static uint8_t target_extruder; #endif -#if ENABLED(SCARA) +#if IS_SCARA + // Float constants for SCARA calculations + const float L1 = SCARA_LINKAGE_1, L2 = SCARA_LINKAGE_2, + L1_2 = sq(float(L1)), L1_2_2 = 2.0 * L1_2, + L2_2 = sq(float(L2)); + float delta_segments_per_second = SCARA_SEGMENTS_PER_SECOND, delta[ABC], axis_scaling[ABC] = { 1, 1, 1 }, // Build size scaling, default to 1 @@ -651,7 +655,7 @@ inline void sync_plan_position() { } inline void sync_plan_position_e() { planner.set_e_position_mm(current_position[E_AXIS]); } -#if ENABLED(DELTA) || ENABLED(SCARA) +#if IS_KINEMATIC inline void sync_plan_position_delta() { #if ENABLED(DEBUG_LEVELING_FEATURE) if (DEBUGGING(LEVELING)) DEBUG_POS("sync_plan_position_delta", current_position); @@ -2161,7 +2165,7 @@ static void clean_up_after_endstop_or_probe_move() { // Prevent stepper_inactive_time from running out and EXTRUDER_RUNOUT_PREVENT from extruding refresh_cmd_timeout(); - #if ENABLED(AUTO_BED_LEVELING_FEATURE) + #if ENABLED(AUTO_BED_LEVELING_LINEAR) planner.bed_level_matrix.set_to_identity(); #endif @@ -2272,7 +2276,7 @@ static void clean_up_after_endstop_or_probe_move() { #if ENABLED(AUTO_BED_LEVELING_FEATURE) - #if DISABLED(DELTA) + #if ENABLED(AUTO_BED_LEVELING_LINEAR) /** * Get the stepper positions, apply the rotation matrix @@ -2302,9 +2306,7 @@ static void clean_up_after_endstop_or_probe_move() { return pos; } - #endif // !DELTA - - #if ENABLED(DELTA) + #elif ENABLED(AUTO_BED_LEVELING_NONLINEAR) /** * All DELTA leveling in the Marlin uses NONLINEAR_BED_LEVELING @@ -2870,7 +2872,7 @@ inline void gcode_G4() { SERIAL_ECHOPGM("Machine Type: "); #if ENABLED(DELTA) SERIAL_ECHOLNPGM("Delta"); - #elif ENABLED(SCARA) + #elif IS_SCARA SERIAL_ECHOLNPGM("SCARA"); #elif ENABLED(COREXY) || ENABLED(COREXZ) || ENABLED(COREYZ) SERIAL_ECHOLNPGM("Core"); @@ -2947,11 +2949,12 @@ inline void gcode_G28() { stepper.synchronize(); // For auto bed leveling, clear the level matrix - #if ENABLED(AUTO_BED_LEVELING_FEATURE) + #if ENABLED(AUTO_BED_LEVELING_LINEAR) planner.bed_level_matrix.set_to_identity(); - #if ENABLED(DELTA) - reset_bed_level(); - #endif + #endif + + #if ENABLED(AUTO_BED_LEVELING_NONLINEAR) + reset_bed_level(); #endif // Always home with tool 0 active @@ -3533,7 +3536,7 @@ inline void gcode_G28() { #if ENABLED(AUTO_BED_LEVELING_GRID) - #if DISABLED(DELTA) + #if ENABLED(AUTO_BED_LEVELING_LINEAR) bool do_topography_map = verbose_level > 2 || code_seen('T'); #endif @@ -3544,7 +3547,7 @@ inline void gcode_G28() { int auto_bed_leveling_grid_points = AUTO_BED_LEVELING_GRID_POINTS; - #if DISABLED(DELTA) + #if ENABLED(AUTO_BED_LEVELING_LINEAR) if (code_seen('P')) auto_bed_leveling_grid_points = code_value_int(); if (auto_bed_leveling_grid_points < 2) { SERIAL_PROTOCOLLNPGM("?Number of probed (P)oints is implausible (2 minimum)."); @@ -3594,17 +3597,19 @@ inline void gcode_G28() { if (!dryrun) { - // Reset the bed_level_matrix because leveling - // needs to be done without leveling enabled. - planner.bed_level_matrix.set_to_identity(); + #if ENABLED(AUTO_BED_LEVELING_LINEAR) + // Reset the bed_level_matrix because leveling + // needs to be done without leveling enabled. + planner.bed_level_matrix.set_to_identity(); + #endif // // Re-orient the current position without leveling // based on where the steppers are positioned. // - #if ENABLED(DELTA) || ENABLED(SCARA) + #if IS_KINEMATIC - #if ENABLED(DELTA) + #if ENABLED(AUTO_BED_LEVELING_NONLINEAR) reset_bed_level(); #endif @@ -3639,12 +3644,14 @@ inline void gcode_G28() { const float xGridSpacing = (right_probe_bed_position - left_probe_bed_position) / (auto_bed_leveling_grid_points - 1), yGridSpacing = (back_probe_bed_position - front_probe_bed_position) / (auto_bed_leveling_grid_points - 1); - #if ENABLED(DELTA) + #if ENABLED(AUTO_BED_LEVELING_NONLINEAR) delta_grid_spacing[X_AXIS] = xGridSpacing; delta_grid_spacing[Y_AXIS] = yGridSpacing; float zoffset = zprobe_zoffset; if (code_seen('Z')) zoffset += code_value_axis_units(Z_AXIS); - #else // !DELTA + + #elif ENABLED(AUTO_BED_LEVELING_LINEAR) + /** * solve the plane equation ax + by + d = z * A is the matrix with rows [x y 1] for all the probed points @@ -3660,7 +3667,8 @@ inline void gcode_G28() { eqnBVector[abl2], // "B" vector of Z points mean = 0.0; int8_t indexIntoAB[auto_bed_leveling_grid_points][auto_bed_leveling_grid_points]; - #endif // !DELTA + + #endif // AUTO_BED_LEVELING_LINEAR int probePointCounter = 0; bool zig = auto_bed_leveling_grid_points & 1; //always end at [RIGHT_PROBE_BED_POSITION, BACK_PROBE_BED_POSITION] @@ -3694,16 +3702,19 @@ inline void gcode_G28() { float measured_z = probe_pt(xProbe, yProbe, stow_probe_after_each, verbose_level); - #if DISABLED(DELTA) - mean += measured_z; + #if ENABLED(AUTO_BED_LEVELING_LINEAR) + mean += measured_z; eqnBVector[probePointCounter] = measured_z; eqnAMatrix[probePointCounter + 0 * abl2] = xProbe; eqnAMatrix[probePointCounter + 1 * abl2] = yProbe; eqnAMatrix[probePointCounter + 2 * abl2] = 1; indexIntoAB[xCount][yCount] = probePointCounter; - #else + + #elif ENABLED(AUTO_BED_LEVELING_NONLINEAR) + bed_level[xCount][yCount] = measured_z + zoffset; + #endif probePointCounter++; @@ -3713,7 +3724,7 @@ inline void gcode_G28() { } //xProbe } //yProbe - #else // !AUTO_BED_LEVELING_GRID + #elif ENABLED(AUTO_BED_LEVELING_3POINT) #if ENABLED(DEBUG_LEVELING_FEATURE) if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("> 3-point Leveling"); @@ -3759,12 +3770,12 @@ inline void gcode_G28() { // Calculate leveling, print reports, correct the position #if ENABLED(AUTO_BED_LEVELING_GRID) - #if ENABLED(DELTA) + #if ENABLED(AUTO_BED_LEVELING_NONLINEAR) if (!dryrun) extrapolate_unprobed_bed_level(); print_bed_level(); - #else // !DELTA + #elif ENABLED(AUTO_BED_LEVELING_LINEAR) // solve lsq problem double plane_equation_coefficients[3]; @@ -3860,11 +3871,11 @@ inline void gcode_G28() { } } //do_topography_map - #endif //!DELTA + #endif // AUTO_BED_LEVELING_LINEAR #endif // AUTO_BED_LEVELING_GRID - #if DISABLED(DELTA) + #if ENABLED(AUTO_BED_LEVELING_LINEAR) if (verbose_level > 0) planner.bed_level_matrix.debug("\n\nBed Level Correction Matrix:"); @@ -4358,10 +4369,10 @@ inline void gcode_M42() { if (verbose_level > 2) SERIAL_PROTOCOLLNPGM("Positioning the probe..."); - #if ENABLED(DELTA) + #if ENABLED(AUTO_BED_LEVELING_NONLINEAR) // we don't do bed level correction in M48 because we want the raw data when we probe reset_bed_level(); - #elif ENABLED(AUTO_BED_LEVELING_FEATURE) + #elif ENABLED(AUTO_BED_LEVELING_LINEAR) // we don't do bed level correction in M48 because we want the raw data when we probe planner.bed_level_matrix.set_to_identity(); #endif @@ -6361,7 +6372,7 @@ inline void gcode_M503() { lastpos[i] = destination[i] = current_position[i]; // Define runplan for move axes - #if ENABLED(DELTA) + #if IS_KINEMATIC #define RUNPLAN(RATE_MM_S) inverse_kinematics(destination); \ planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], RATE_MM_S, active_extruder); #else @@ -6482,7 +6493,7 @@ inline void gcode_M503() { destination[E_AXIS] = lastpos[E_AXIS]; planner.set_e_position_mm(current_position[E_AXIS]); - #if ENABLED(DELTA) + #if IS_KINEMATIC // Move XYZ to starting position, then E inverse_kinematics(lastpos); planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], FILAMENT_CHANGE_XY_FEEDRATE, active_extruder); @@ -6925,7 +6936,7 @@ void tool_change(const uint8_t tmp_extruder, const float fr_mm_s/*=0.0*/, bool n * Z software endstop. But this is technically correct (and * there is no viable alternative). */ - #if ENABLED(AUTO_BED_LEVELING_FEATURE) + #if ENABLED(AUTO_BED_LEVELING_LINEAR) // Offset extruder, make sure to apply the bed level rotation matrix vector_3 tmp_offset_vec = vector_3(hotend_offset[X_AXIS][tmp_extruder], hotend_offset[Y_AXIS][tmp_extruder], @@ -7961,7 +7972,7 @@ void ok_to_send() { stepper.get_axis_position_mm(C_AXIS)); } - #if ENABLED(AUTO_BED_LEVELING_FEATURE) + #if ENABLED(AUTO_BED_LEVELING_NONLINEAR) // Adjust print surface height by linear interpolation over the bed_level array. void adjust_delta(float cartesian[XYZ]) { @@ -8001,7 +8012,7 @@ void ok_to_send() { SERIAL_ECHOPGM(" offset="); SERIAL_ECHOLN(offset); */ } - #endif // AUTO_BED_LEVELING_FEATURE + #endif // AUTO_BED_LEVELING_NONLINEAR #endif // DELTA @@ -8076,7 +8087,7 @@ void mesh_line_to_destination(float fr_mm_s, uint8_t x_splits = 0xff, uint8_t y_ } #endif // MESH_BED_LEVELING -#if ENABLED(DELTA) || ENABLED(SCARA) +#if IS_KINEMATIC inline bool prepare_kinematic_move_to(float target[NUM_AXIS]) { float difference[NUM_AXIS]; @@ -8103,7 +8114,7 @@ void mesh_line_to_destination(float fr_mm_s, uint8_t x_splits = 0xff, uint8_t y_ inverse_kinematics(target); - #if ENABLED(DELTA) && ENABLED(AUTO_BED_LEVELING_FEATURE) + #if ENABLED(DELTA) && ENABLED(AUTO_BED_LEVELING_NONLINEAR) if (!bed_leveling_in_progress) adjust_delta(target); #endif @@ -8115,7 +8126,7 @@ void mesh_line_to_destination(float fr_mm_s, uint8_t x_splits = 0xff, uint8_t y_ return true; } -#endif // DELTA || SCARA +#endif // IS_KINEMATIC #if ENABLED(DUAL_X_CARRIAGE) @@ -8161,7 +8172,7 @@ void mesh_line_to_destination(float fr_mm_s, uint8_t x_splits = 0xff, uint8_t y_ #endif // DUAL_X_CARRIAGE -#if DISABLED(DELTA) && DISABLED(SCARA) +#if !IS_KINEMATIC inline bool prepare_move_to_destination_cartesian() { // Do not use feedrate_percentage for E or Z only moves @@ -8181,7 +8192,7 @@ void mesh_line_to_destination(float fr_mm_s, uint8_t x_splits = 0xff, uint8_t y_ return true; } -#endif // !DELTA && !SCARA +#endif // !IS_KINEMATIC #if ENABLED(PREVENT_COLD_EXTRUSION) @@ -8220,7 +8231,7 @@ void prepare_move_to_destination() { prevent_dangerous_extrude(current_position[E_AXIS], destination[E_AXIS]); #endif - #if ENABLED(DELTA) || ENABLED(SCARA) + #if IS_KINEMATIC if (!prepare_kinematic_move_to(destination)) return; #else #if ENABLED(DUAL_X_CARRIAGE) @@ -8356,9 +8367,9 @@ void prepare_move_to_destination() { clamp_to_software_endstops(arc_target); - #if ENABLED(DELTA) || ENABLED(SCARA) + #if IS_KINEMATIC inverse_kinematics(arc_target); - #if ENABLED(DELTA) && ENABLED(AUTO_BED_LEVELING_FEATURE) + #if ENABLED(DELTA) && ENABLED(AUTO_BED_LEVELING_NONLINEAR) adjust_delta(arc_target); #endif planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], arc_target[E_AXIS], fr_mm_s, active_extruder); @@ -8368,9 +8379,9 @@ void prepare_move_to_destination() { } // Ensure last segment arrives at target location. - #if ENABLED(DELTA) || ENABLED(SCARA) + #if IS_KINEMATIC inverse_kinematics(target); - #if ENABLED(DELTA) && ENABLED(AUTO_BED_LEVELING_FEATURE) + #if ENABLED(DELTA) && ENABLED(AUTO_BED_LEVELING_NONLINEAR) adjust_delta(target); #endif planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], target[E_AXIS], fr_mm_s, active_extruder); diff --git a/Marlin/configuration_store.cpp b/Marlin/configuration_store.cpp index f54cd88a2f8d..0dd1ce6da402 100644 --- a/Marlin/configuration_store.cpp +++ b/Marlin/configuration_store.cpp @@ -330,7 +330,7 @@ void Config_StoreSettings() { #endif EEPROM_WRITE(lcd_contrast); - #if ENABLED(SCARA) + #if IS_SCARA EEPROM_WRITE(axis_scaling); // 3 floats #else dummy = 1.0f; @@ -520,7 +520,7 @@ void Config_RetrieveSettings() { #endif EEPROM_READ(lcd_contrast); - #if ENABLED(SCARA) + #if IS_SCARA EEPROM_READ(axis_scaling); // 3 floats #else EEPROM_READ(dummy); @@ -584,7 +584,7 @@ void Config_ResetDefault() { planner.axis_steps_per_mm[i] = tmp1[i]; planner.max_feedrate_mm_s[i] = tmp2[i]; planner.max_acceleration_mm_per_s2[i] = tmp3[i]; - #if ENABLED(SCARA) + #if IS_SCARA if (i < COUNT(axis_scaling)) axis_scaling[i] = 1; #endif @@ -716,7 +716,7 @@ void Config_PrintSettings(bool forReplay) { CONFIG_ECHO_START; - #if ENABLED(SCARA) + #if IS_SCARA if (!forReplay) { SERIAL_ECHOLNPGM("Scaling factors:"); CONFIG_ECHO_START; diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp index bcad2c9069ab..f8dc5efd2b03 100644 --- a/Marlin/planner.cpp +++ b/Marlin/planner.cpp @@ -98,7 +98,7 @@ float Planner::min_feedrate_mm_s, Planner::max_e_jerk, Planner::min_travel_feedrate_mm_s; -#if ENABLED(AUTO_BED_LEVELING_FEATURE) +#if ENABLED(AUTO_BED_LEVELING_LINEAR) matrix_3x3 Planner::bed_level_matrix; // Transform to compensate for bed level #endif @@ -138,7 +138,7 @@ void Planner::init() { memset(position, 0, sizeof(position)); // clear position LOOP_XYZE(i) previous_speed[i] = 0.0; previous_nominal_speed = 0.0; - #if ENABLED(AUTO_BED_LEVELING_FEATURE) + #if ENABLED(AUTO_BED_LEVELING_LINEAR) bed_level_matrix.set_to_identity(); #endif } @@ -521,7 +521,7 @@ void Planner::check_axes_activity() { #endif } -#if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING) +#if PLANNER_LEVELING void Planner::apply_leveling( #if ENABLED(MESH_BED_LEVELING) @@ -551,7 +551,7 @@ void Planner::check_axes_activity() { #endif } -#endif +#endif // PLANNER_LEVELING /** * Planner::buffer_line @@ -1193,7 +1193,7 @@ void Planner::reset_acceleration_rates() { // Recalculate position, steps_to_mm if axis_steps_per_mm changes! void Planner::refresh_positioning() { LOOP_XYZE(i) steps_to_mm[i] = 1.0 / axis_steps_per_mm[i]; - #if ENABLED(DELTA) || ENABLED(SCARA) + #if IS_KINEMATIC inverse_kinematics(current_position); set_position_mm(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]); #else diff --git a/Marlin/planner_bezier.cpp b/Marlin/planner_bezier.cpp index 6ca7afd1d6b6..15c80916351a 100644 --- a/Marlin/planner_bezier.cpp +++ b/Marlin/planner_bezier.cpp @@ -188,7 +188,7 @@ void cubic_b_spline(const float position[NUM_AXIS], const float target[NUM_AXIS] bez_target[E_AXIS] = interp(position[E_AXIS], target[E_AXIS], t); clamp_to_software_endstops(bez_target); - #if ENABLED(DELTA) || ENABLED(SCARA) + #if IS_KINEMATIC inverse_kinematics(bez_target); #if ENABLED(DELTA) && ENABLED(AUTO_BED_LEVELING_FEATURE) adjust_delta(bez_target); diff --git a/Marlin/qr_solve.cpp b/Marlin/qr_solve.cpp index ddafb005eadd..e60b1d3274bf 100644 --- a/Marlin/qr_solve.cpp +++ b/Marlin/qr_solve.cpp @@ -22,7 +22,7 @@ #include "qr_solve.h" -#if ENABLED(AUTO_BED_LEVELING_GRID) +#if ENABLED(AUTO_BED_LEVELING_LINEAR) #include #include diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp index f5459757dbb6..55247c15d56f 100755 --- a/Marlin/ultralcd.cpp +++ b/Marlin/ultralcd.cpp @@ -1418,7 +1418,7 @@ void kill_screen(const char* lcd_msg) { * */ - #if ENABLED(DELTA) || ENABLED(SCARA) + #if IS_KINEMATIC #define _MOVE_XYZ_ALLOWED (axis_homed[X_AXIS] && axis_homed[Y_AXIS] && axis_homed[Z_AXIS]) #else #define _MOVE_XYZ_ALLOWED true @@ -1823,7 +1823,7 @@ void kill_screen(const char* lcd_msg) { #if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED) MENU_ITEM_EDIT(bool, MSG_ENDSTOP_ABORT, &stepper.abort_on_endstop_hit); #endif - #if ENABLED(SCARA) + #if IS_SCARA MENU_ITEM_EDIT(float74, MSG_XSCALE, &axis_scaling[X_AXIS], 0.5, 2); MENU_ITEM_EDIT(float74, MSG_YSCALE, &axis_scaling[Y_AXIS], 0.5, 2); #endif From 74d7f5e57b0f2561f6ff7f768afd9efff1b4c4a5 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Sun, 11 Sep 2016 20:11:39 -0500 Subject: [PATCH 4/6] Patch SCARA example config --- Marlin/Marlin_main.cpp | 73 ++++++++----------- Marlin/SanityCheck.h | 9 ++- .../SCARA/Configuration.h | 48 ++++++------ 3 files changed, 63 insertions(+), 67 deletions(-) diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index 73376b4d1c64..cc1763372439 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -8457,18 +8457,18 @@ void prepare_move_to_destination() { //SERIAL_ECHOPGM("f_delta x="); SERIAL_ECHO(f_scara[X_AXIS]); //SERIAL_ECHOPGM(" y="); SERIAL_ECHO(f_scara[Y_AXIS]); - x_sin = sin(f_scara[X_AXIS] / SCARA_RAD2DEG) * Linkage_1; - x_cos = cos(f_scara[X_AXIS] / SCARA_RAD2DEG) * Linkage_1; - y_sin = sin(f_scara[Y_AXIS] / SCARA_RAD2DEG) * Linkage_2; - y_cos = cos(f_scara[Y_AXIS] / SCARA_RAD2DEG) * Linkage_2; + x_sin = sin(RADIANS(f_scara[X_AXIS])) * L1; + x_cos = cos(RADIANS(f_scara[X_AXIS])) * L1; + y_sin = sin(RADIANS(f_scara[Y_AXIS])) * L2; + y_cos = cos(RADIANS(f_scara[Y_AXIS])) * L2; //SERIAL_ECHOPGM(" x_sin="); SERIAL_ECHO(x_sin); //SERIAL_ECHOPGM(" x_cos="); SERIAL_ECHO(x_cos); //SERIAL_ECHOPGM(" y_sin="); SERIAL_ECHO(y_sin); //SERIAL_ECHOPGM(" y_cos="); SERIAL_ECHOLN(y_cos); - delta[X_AXIS] = x_cos + y_cos + SCARA_offset_x; //theta - delta[Y_AXIS] = x_sin + y_sin + SCARA_offset_y; //theta+phi + delta[X_AXIS] = x_cos + y_cos + SCARA_OFFSET_X; //theta + delta[Y_AXIS] = x_sin + y_sin + SCARA_OFFSET_Y; //theta+phi //SERIAL_ECHOPGM(" delta[X_AXIS]="); SERIAL_ECHO(delta[X_AXIS]); //SERIAL_ECHOPGM(" delta[Y_AXIS]="); SERIAL_ECHOLN(delta[Y_AXIS]); @@ -8480,51 +8480,42 @@ void prepare_move_to_destination() { // The maths and first version were done by QHARLEY. // Integrated, tweaked by Joachim Cerny in June 2014. - float SCARA_pos[2]; - static float SCARA_C2, SCARA_S2, SCARA_K1, SCARA_K2, SCARA_theta, SCARA_psi; + static float C2, S2, SK1, SK2, THETA, PSI; - SCARA_pos[X_AXIS] = RAW_X_POSITION(cartesian[X_AXIS]) * axis_scaling[X_AXIS] - SCARA_offset_x; //Translate SCARA to standard X Y - SCARA_pos[Y_AXIS] = RAW_Y_POSITION(cartesian[Y_AXIS]) * axis_scaling[Y_AXIS] - SCARA_offset_y; // With scaling factor. + float sx = RAW_X_POSITION(cartesian[X_AXIS]) * axis_scaling[X_AXIS] - SCARA_OFFSET_X, //Translate SCARA to standard X Y + sy = RAW_Y_POSITION(cartesian[Y_AXIS]) * axis_scaling[Y_AXIS] - SCARA_OFFSET_Y; // With scaling factor. - #if (Linkage_1 == Linkage_2) - SCARA_C2 = ((sq(SCARA_pos[X_AXIS]) + sq(SCARA_pos[Y_AXIS])) / (2 * (float)L1_2)) - 1; + #if (L1 == L2) + C2 = HYPOT2(sx, sy) / (2 * L1_2) - 1; #else - SCARA_C2 = (sq(SCARA_pos[X_AXIS]) + sq(SCARA_pos[Y_AXIS]) - (float)L1_2 - (float)L2_2) / 45000; + C2 = (HYPOT2(sx, sy) - L1_2 - L2_2) / 45000; #endif - SCARA_S2 = sqrt(1 - sq(SCARA_C2)); + S2 = sqrt(1 - sq(C2)); - SCARA_K1 = Linkage_1 + Linkage_2 * SCARA_C2; - SCARA_K2 = Linkage_2 * SCARA_S2; + SK1 = L1 + L2 * C2; + SK2 = L2 * S2; - SCARA_theta = (atan2(SCARA_pos[X_AXIS], SCARA_pos[Y_AXIS]) - atan2(SCARA_K1, SCARA_K2)) * -1; - SCARA_psi = atan2(SCARA_S2, SCARA_C2); + THETA = (atan2(sx, sy) - atan2(SK1, SK2)) * -1; + PSI = atan2(S2, C2); - delta[X_AXIS] = SCARA_theta * SCARA_RAD2DEG; // Multiply by 180/Pi - theta is support arm angle - delta[Y_AXIS] = (SCARA_theta + SCARA_psi) * SCARA_RAD2DEG; // - equal to sub arm angle (inverted motor) - delta[Z_AXIS] = RAW_Z_POSITION(cartesian[Z_AXIS]); + delta[A_AXIS] = DEGREES(THETA); // theta is support arm angle + delta[B_AXIS] = DEGREES(THETA + PSI); // equal to sub arm angle (inverted motor) + delta[Z_AXIS] = cartesian[Z_AXIS]; /** - SERIAL_ECHOPGM("cartesian x="); SERIAL_ECHO(cartesian[X_AXIS]); - SERIAL_ECHOPGM(" y="); SERIAL_ECHO(cartesian[Y_AXIS]); - SERIAL_ECHOPGM(" z="); SERIAL_ECHOLN(cartesian[Z_AXIS]); - - SERIAL_ECHOPGM("scara x="); SERIAL_ECHO(SCARA_pos[X_AXIS]); - SERIAL_ECHOPGM(" y="); SERIAL_ECHOLN(SCARA_pos[Y_AXIS]); - - SERIAL_ECHOPGM("delta x="); SERIAL_ECHO(delta[X_AXIS]); - SERIAL_ECHOPGM(" y="); SERIAL_ECHO(delta[Y_AXIS]); - SERIAL_ECHOPGM(" z="); SERIAL_ECHOLN(delta[Z_AXIS]); - - SERIAL_ECHOPGM("C2="); SERIAL_ECHO(SCARA_C2); - SERIAL_ECHOPGM(" S2="); SERIAL_ECHO(SCARA_S2); - SERIAL_ECHOPGM(" Theta="); SERIAL_ECHO(SCARA_theta); - SERIAL_ECHOPGM(" Psi="); SERIAL_ECHOLN(SCARA_psi); - SERIAL_EOL; - */ - } - -#endif // SCARA + DEBUG_POS("SCARA IK", cartesian); + DEBUG_POS("SCARA IK", delta); + SERIAL_ECHOPAIR(" SCARA (x,y) ", sx); + SERIAL_ECHOPAIR(",", sy); + SERIAL_ECHOPAIR(" C2=", C2); + SERIAL_ECHOPAIR(" S2=", S2); + SERIAL_ECHOPAIR(" Theta=", THETA); + SERIAL_ECHOLNPAIR(" Phi=", PHI); + //*/ + } + +#endif // MORGAN_SCARA #if ENABLED(TEMP_STAT_LEDS) diff --git a/Marlin/SanityCheck.h b/Marlin/SanityCheck.h index 0234baee1cc7..d04ae6992c64 100644 --- a/Marlin/SanityCheck.h +++ b/Marlin/SanityCheck.h @@ -137,6 +137,8 @@ #error Please replace "const int dropsegments" with "#define MIN_STEPS_PER_SEGMENT" (and increase by 1) in Configuration_adv.h. #elif defined(PREVENT_DANGEROUS_EXTRUDE) #error "PREVENT_DANGEROUS_EXTRUDE is now PREVENT_COLD_EXTRUSION. Please update your configuration." +#elif defined(SCARA) + #error "SCARA is now MORGAN_SCARA. Please update your configuration." #endif /** @@ -573,11 +575,12 @@ /** * Don't set more than one kinematic type */ -#if (ENABLED(DELTA) && (ENABLED(SCARA) || ENABLED(COREXY) || ENABLED(COREXZ) || ENABLED(COREYZ))) \ +#if (ENABLED(DELTA) && (ENABLED(MORGAN_SCARA) || ENABLED(MAKERARM_SCARA) || ENABLED(COREXY) || ENABLED(COREXZ) || ENABLED(COREYZ))) \ + || (ENABLED(DELTA) && (ENABLED(MAKERARM_SCARA) || ENABLED(COREXY) || ENABLED(COREXZ) || ENABLED(COREYZ))) \ || (ENABLED(SCARA) && (ENABLED(COREXY) || ENABLED(COREXZ) || ENABLED(COREYZ))) \ || (ENABLED(COREXY) && (ENABLED(COREXZ) || ENABLED(COREYZ))) \ || (ENABLED(COREXZ) && ENABLED(COREYZ)) - #error "Please enable only one of DELTA, SCARA, COREXY, COREXZ, or COREYZ." + #error "Please enable only one of DELTA, MORGAN_SCARA, MAKERARM_SCARA, COREXY, COREXZ, or COREYZ." #endif /** @@ -750,7 +753,7 @@ #elif ENABLED(DELTA) #error "Z_DUAL_ENDSTOPS is not compatible with DELTA." #endif -#elif DISABLED(SCARA) +#elif !IS_SCARA #if X_HOME_DIR < 0 && DISABLED(USE_XMIN_PLUG) #error "Enable USE_XMIN_PLUG when homing X to MIN." #elif X_HOME_DIR > 0 && DISABLED(USE_XMAX_PLUG) diff --git a/Marlin/example_configurations/SCARA/Configuration.h b/Marlin/example_configurations/SCARA/Configuration.h index 085880e210e6..3c2e96a69695 100644 --- a/Marlin/example_configurations/SCARA/Configuration.h +++ b/Marlin/example_configurations/SCARA/Configuration.h @@ -75,35 +75,37 @@ // //=========================================================================== -//========================= SCARA Settings ================================== +//============================= SCARA Printer =============================== //=========================================================================== -// SCARA-mode for Marlin has been developed by QHARLEY in ZA in 2012/2013. Implemented +// MORGAN_SCARA for Marlin was developed by QHARLEY in ZA in 2012/2013. Implemented // and slightly reworked by JCERNY in 06/2014 with the goal to bring it into Master-Branch // QHARLEYS Autobedlevelling has not been ported, because Marlin has now Bed-levelling // You might need Z-Min endstop on SCARA-Printer to use this feature. Actually untested! -// Uncomment to use Morgan scara mode -#define SCARA -#define SCARA_SEGMENTS_PER_SECOND 200 // If movement is choppy try lowering this value -// Length of inner support arm -#define Linkage_1 150 //mm Preprocessor cannot handle decimal point... -// Length of outer support arm Measure arm lengths precisely and enter -#define Linkage_2 150 //mm - -// SCARA tower offset (position of Tower relative to bed zero position) -// This needs to be reasonably accurate as it defines the printbed position in the SCARA space. -#define SCARA_offset_x 100 //mm -#define SCARA_offset_y -56 //mm -#define SCARA_RAD2DEG 57.2957795 // to convert RAD to degrees - -#define THETA_HOMING_OFFSET 0 //calculatated from Calibration Guide and command M360 / M114 see picture in http://reprap.harleystudio.co.za/?page_id=1073 -#define PSI_HOMING_OFFSET 0 //calculatated from Calibration Guide and command M364 / M114 see picture in http://reprap.harleystudio.co.za/?page_id=1073 - -//some helper variables to make kinematics faster -#define L1_2 sq(Linkage_1) // do not change -#define L2_2 sq(Linkage_2) // do not change + +// Specify the specific SCARA model +#define MORGAN_SCARA +//#define MAKERARM_SCARA + +#if ENABLED(MORGAN_SCARA) || ENABLED(MAKERARM_SCARA) + //#define DEBUG_SCARA_KINEMATICS + + #define SCARA_SEGMENTS_PER_SECOND 200 // If movement is choppy try lowering this value + // Length of inner support arm + #define SCARA_LINKAGE_1 150 //mm Preprocessor cannot handle decimal point... + // Length of outer support arm Measure arm lengths precisely and enter + #define SCARA_LINKAGE_2 150 //mm + + // SCARA tower offset (position of Tower relative to bed zero position) + // This needs to be reasonably accurate as it defines the printbed position in the SCARA space. + #define SCARA_OFFSET_X 100 //mm + #define SCARA_OFFSET_Y -56 //mm + + #define THETA_HOMING_OFFSET 0 //calculatated from Calibration Guide and command M360 / M114 see picture in http://reprap.harleystudio.co.za/?page_id=1073 + #define PSI_HOMING_OFFSET 0 //calculatated from Calibration Guide and command M364 / M114 see picture in http://reprap.harleystudio.co.za/?page_id=1073 +#endif //=========================================================================== -//========================= SCARA Settings end ============================== +//==================== END ==== SCARA Printer ==== END ====================== //=========================================================================== // @section info From 8ff338c2b99e2f880cfa60031e48545f59631d77 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Sun, 11 Sep 2016 20:18:52 -0500 Subject: [PATCH 5/6] Patch stepper.h for SCARA --- Marlin/stepper.h | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/Marlin/stepper.h b/Marlin/stepper.h index 4a2287c7d720..d7507aff2c92 100644 --- a/Marlin/stepper.h +++ b/Marlin/stepper.h @@ -91,6 +91,11 @@ class Stepper { static bool performing_homing; #endif + // + // Positions of stepper motors, in step units + // + static volatile long count_position[NUM_AXIS]; + private: static unsigned char last_direction_bits; // The next stepping-bits to be output @@ -138,11 +143,6 @@ class Stepper { static constexpr int motor_current_setting[3] = PWM_MOTOR_CURRENT; #endif - // - // Positions of stepper motors, in step units - // - static volatile long count_position[NUM_AXIS]; - // // Current direction of stepper motors (+1 or -1) // @@ -211,6 +211,13 @@ class Stepper { // static float get_axis_position_mm(AxisEnum axis); + // + // SCARA AB axes are in degrees, not mm + // + #if IS_SCARA + static FORCE_INLINE float get_axis_position_degrees(AxisEnum axis) { return get_axis_position_mm(axis); } + #endif + // // The stepper subsystem goes to sleep when it runs out of things to execute. Call this // to notify the subsystem that it is time to go to work. From f9a192c7e340286093c891092f3f5986e5c1f36d Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Sun, 11 Sep 2016 20:21:54 -0500 Subject: [PATCH 6/6] Move setup() and loop() to the end --- Marlin/Marlin_main.cpp | 428 ++++++++++++++++++++--------------------- 1 file changed, 214 insertions(+), 214 deletions(-) diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index cc1763372439..e0b19118c99a 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -850,10 +850,6 @@ void servo_init() { */ STOW_Z_SERVO(); #endif - - #if HAS_BED_PROBE - endstops.enable_z_probe(false); - #endif } /** @@ -879,216 +875,6 @@ void servo_init() { #endif -/** - * Marlin entry-point: Set up before the program loop - * - Set up the kill pin, filament runout, power hold - * - Start the serial port - * - Print startup messages and diagnostics - * - Get EEPROM or default settings - * - Initialize managers for: - * • temperature - * • planner - * • watchdog - * • stepper - * • photo pin - * • servos - * • LCD controller - * • Digipot I2C - * • Z probe sled - * • status LEDs - */ -void setup() { - - #ifdef DISABLE_JTAG - // Disable JTAG on AT90USB chips to free up pins for IO - MCUCR = 0x80; - MCUCR = 0x80; - #endif - - #if ENABLED(FILAMENT_RUNOUT_SENSOR) - setup_filrunoutpin(); - #endif - - setup_killpin(); - - setup_powerhold(); - - #if HAS_STEPPER_RESET - disableStepperDrivers(); - #endif - - MYSERIAL.begin(BAUDRATE); - SERIAL_PROTOCOLLNPGM("start"); - SERIAL_ECHO_START; - - // Check startup - does nothing if bootloader sets MCUSR to 0 - byte mcu = MCUSR; - if (mcu & 1) SERIAL_ECHOLNPGM(MSG_POWERUP); - if (mcu & 2) SERIAL_ECHOLNPGM(MSG_EXTERNAL_RESET); - if (mcu & 4) SERIAL_ECHOLNPGM(MSG_BROWNOUT_RESET); - if (mcu & 8) SERIAL_ECHOLNPGM(MSG_WATCHDOG_RESET); - if (mcu & 32) SERIAL_ECHOLNPGM(MSG_SOFTWARE_RESET); - MCUSR = 0; - - SERIAL_ECHOPGM(MSG_MARLIN); - SERIAL_ECHOLNPGM(" " SHORT_BUILD_VERSION); - - #ifdef STRING_DISTRIBUTION_DATE - #ifdef STRING_CONFIG_H_AUTHOR - SERIAL_ECHO_START; - SERIAL_ECHOPGM(MSG_CONFIGURATION_VER); - SERIAL_ECHOPGM(STRING_DISTRIBUTION_DATE); - SERIAL_ECHOPGM(MSG_AUTHOR); - SERIAL_ECHOLNPGM(STRING_CONFIG_H_AUTHOR); - SERIAL_ECHOPGM("Compiled: "); - SERIAL_ECHOLNPGM(__DATE__); - #endif // STRING_CONFIG_H_AUTHOR - #endif // STRING_DISTRIBUTION_DATE - - SERIAL_ECHO_START; - SERIAL_ECHOPGM(MSG_FREE_MEMORY); - SERIAL_ECHO(freeMemory()); - SERIAL_ECHOPGM(MSG_PLANNER_BUFFER_BYTES); - SERIAL_ECHOLN((int)sizeof(block_t)*BLOCK_BUFFER_SIZE); - - // Send "ok" after commands by default - for (int8_t i = 0; i < BUFSIZE; i++) send_ok[i] = true; - - // Load data from EEPROM if available (or use defaults) - // This also updates variables in the planner, elsewhere - Config_RetrieveSettings(); - - // Initialize current position based on home_offset - memcpy(current_position, home_offset, sizeof(home_offset)); - - // Vital to init stepper/planner equivalent for current_position - SYNC_PLAN_POSITION_KINEMATIC(); - - thermalManager.init(); // Initialize temperature loop - - #if ENABLED(USE_WATCHDOG) - watchdog_init(); - #endif - - stepper.init(); // Initialize stepper, this enables interrupts! - setup_photpin(); - servo_init(); - - #if HAS_CONTROLLERFAN - SET_OUTPUT(CONTROLLERFAN_PIN); //Set pin used for driver cooling fan - #endif - - #if HAS_STEPPER_RESET - enableStepperDrivers(); - #endif - - #if ENABLED(DIGIPOT_I2C) - digipot_i2c_init(); - #endif - - #if ENABLED(DAC_STEPPER_CURRENT) - dac_init(); - #endif - - #if ENABLED(Z_PROBE_SLED) && PIN_EXISTS(SLED) - pinMode(SLED_PIN, OUTPUT); - digitalWrite(SLED_PIN, LOW); // turn it off - #endif // Z_PROBE_SLED - - setup_homepin(); - - #ifdef STAT_LED_RED - pinMode(STAT_LED_RED, OUTPUT); - digitalWrite(STAT_LED_RED, LOW); // turn it off - #endif - - #ifdef STAT_LED_BLUE - pinMode(STAT_LED_BLUE, OUTPUT); - digitalWrite(STAT_LED_BLUE, LOW); // turn it off - #endif - - lcd_init(); - #if ENABLED(SHOW_BOOTSCREEN) - #if ENABLED(DOGLCD) - safe_delay(BOOTSCREEN_TIMEOUT); - #elif ENABLED(ULTRA_LCD) - bootscreen(); - lcd_init(); - #endif - #endif - - #if ENABLED(MIXING_EXTRUDER) && MIXING_VIRTUAL_TOOLS > 1 - // Initialize mixing to 100% color 1 - for (uint8_t i = 0; i < MIXING_STEPPERS; i++) - mixing_factor[i] = (i == 0) ? 1 : 0; - for (uint8_t t = 0; t < MIXING_VIRTUAL_TOOLS; t++) - for (uint8_t i = 0; i < MIXING_STEPPERS; i++) - mixing_virtual_tool_mix[t][i] = mixing_factor[i]; - #endif - - #if ENABLED(EXPERIMENTAL_I2CBUS) && I2C_SLAVE_ADDRESS > 0 - i2c.onReceive(i2c_on_receive); - i2c.onRequest(i2c_on_request); - #endif -} - -/** - * The main Marlin program loop - * - * - Save or log commands to SD - * - Process available commands (if not saving) - * - Call heater manager - * - Call inactivity manager - * - Call endstop manager - * - Call LCD update - */ -void loop() { - if (commands_in_queue < BUFSIZE) get_available_commands(); - - #if ENABLED(SDSUPPORT) - card.checkautostart(false); - #endif - - if (commands_in_queue) { - - #if ENABLED(SDSUPPORT) - - if (card.saving) { - char* command = command_queue[cmd_queue_index_r]; - if (strstr_P(command, PSTR("M29"))) { - // M29 closes the file - card.closefile(); - SERIAL_PROTOCOLLNPGM(MSG_FILE_SAVED); - ok_to_send(); - } - else { - // Write the string from the read buffer to SD - card.write_command(command); - if (card.logging) - process_next_command(); // The card is saving because it's logging - else - ok_to_send(); - } - } - else - process_next_command(); - - #else - - process_next_command(); - - #endif // SDSUPPORT - - // The queue may be reset by a command handler or by code invoked by idle() within a handler - if (commands_in_queue) { - --commands_in_queue; - cmd_queue_index_r = (cmd_queue_index_r + 1) % BUFSIZE; - } - } - endstops.report_state(); - idle(); -} - void gcode_line_error(const char* err, bool doFlush = true) { SERIAL_ERROR_START; serialprintPGM(err); @@ -8889,3 +8675,217 @@ void calculate_volumetric_multipliers() { for (uint8_t i = 0; i < COUNT(filament_size); i++) volumetric_multiplier[i] = calculate_volumetric_multiplier(filament_size[i]); } + +/** + * Marlin entry-point: Set up before the program loop + * - Set up the kill pin, filament runout, power hold + * - Start the serial port + * - Print startup messages and diagnostics + * - Get EEPROM or default settings + * - Initialize managers for: + * • temperature + * • planner + * • watchdog + * • stepper + * • photo pin + * • servos + * • LCD controller + * • Digipot I2C + * • Z probe sled + * • status LEDs + */ +void setup() { + + #ifdef DISABLE_JTAG + // Disable JTAG on AT90USB chips to free up pins for IO + MCUCR = 0x80; + MCUCR = 0x80; + #endif + + #if ENABLED(FILAMENT_RUNOUT_SENSOR) + setup_filrunoutpin(); + #endif + + setup_killpin(); + + setup_powerhold(); + + #if HAS_STEPPER_RESET + disableStepperDrivers(); + #endif + + MYSERIAL.begin(BAUDRATE); + SERIAL_PROTOCOLLNPGM("start"); + SERIAL_ECHO_START; + + // Check startup - does nothing if bootloader sets MCUSR to 0 + byte mcu = MCUSR; + if (mcu & 1) SERIAL_ECHOLNPGM(MSG_POWERUP); + if (mcu & 2) SERIAL_ECHOLNPGM(MSG_EXTERNAL_RESET); + if (mcu & 4) SERIAL_ECHOLNPGM(MSG_BROWNOUT_RESET); + if (mcu & 8) SERIAL_ECHOLNPGM(MSG_WATCHDOG_RESET); + if (mcu & 32) SERIAL_ECHOLNPGM(MSG_SOFTWARE_RESET); + MCUSR = 0; + + SERIAL_ECHOPGM(MSG_MARLIN); + SERIAL_ECHOLNPGM(" " SHORT_BUILD_VERSION); + + #ifdef STRING_DISTRIBUTION_DATE + #ifdef STRING_CONFIG_H_AUTHOR + SERIAL_ECHO_START; + SERIAL_ECHOPGM(MSG_CONFIGURATION_VER); + SERIAL_ECHOPGM(STRING_DISTRIBUTION_DATE); + SERIAL_ECHOPGM(MSG_AUTHOR); + SERIAL_ECHOLNPGM(STRING_CONFIG_H_AUTHOR); + SERIAL_ECHOPGM("Compiled: "); + SERIAL_ECHOLNPGM(__DATE__); + #endif // STRING_CONFIG_H_AUTHOR + #endif // STRING_DISTRIBUTION_DATE + + SERIAL_ECHO_START; + SERIAL_ECHOPGM(MSG_FREE_MEMORY); + SERIAL_ECHO(freeMemory()); + SERIAL_ECHOPGM(MSG_PLANNER_BUFFER_BYTES); + SERIAL_ECHOLN((int)sizeof(block_t)*BLOCK_BUFFER_SIZE); + + // Send "ok" after commands by default + for (int8_t i = 0; i < BUFSIZE; i++) send_ok[i] = true; + + // Load data from EEPROM if available (or use defaults) + // This also updates variables in the planner, elsewhere + Config_RetrieveSettings(); + + // Initialize current position based on home_offset + memcpy(current_position, home_offset, sizeof(home_offset)); + + // Vital to init stepper/planner equivalent for current_position + SYNC_PLAN_POSITION_KINEMATIC(); + + thermalManager.init(); // Initialize temperature loop + + #if ENABLED(USE_WATCHDOG) + watchdog_init(); + #endif + + stepper.init(); // Initialize stepper, this enables interrupts! + setup_photpin(); + servo_init(); + + #if HAS_BED_PROBE + endstops.enable_z_probe(false); + #endif + + #if HAS_CONTROLLERFAN + SET_OUTPUT(CONTROLLERFAN_PIN); //Set pin used for driver cooling fan + #endif + + #if HAS_STEPPER_RESET + enableStepperDrivers(); + #endif + + #if ENABLED(DIGIPOT_I2C) + digipot_i2c_init(); + #endif + + #if ENABLED(DAC_STEPPER_CURRENT) + dac_init(); + #endif + + #if ENABLED(Z_PROBE_SLED) && PIN_EXISTS(SLED) + pinMode(SLED_PIN, OUTPUT); + digitalWrite(SLED_PIN, LOW); // turn it off + #endif // Z_PROBE_SLED + + setup_homepin(); + + #ifdef STAT_LED_RED + pinMode(STAT_LED_RED, OUTPUT); + digitalWrite(STAT_LED_RED, LOW); // turn it off + #endif + + #ifdef STAT_LED_BLUE + pinMode(STAT_LED_BLUE, OUTPUT); + digitalWrite(STAT_LED_BLUE, LOW); // turn it off + #endif + + lcd_init(); + #if ENABLED(SHOW_BOOTSCREEN) + #if ENABLED(DOGLCD) + safe_delay(BOOTSCREEN_TIMEOUT); + #elif ENABLED(ULTRA_LCD) + bootscreen(); + lcd_init(); + #endif + #endif + + #if ENABLED(MIXING_EXTRUDER) && MIXING_VIRTUAL_TOOLS > 1 + // Initialize mixing to 100% color 1 + for (uint8_t i = 0; i < MIXING_STEPPERS; i++) + mixing_factor[i] = (i == 0) ? 1 : 0; + for (uint8_t t = 0; t < MIXING_VIRTUAL_TOOLS; t++) + for (uint8_t i = 0; i < MIXING_STEPPERS; i++) + mixing_virtual_tool_mix[t][i] = mixing_factor[i]; + #endif + + #if ENABLED(EXPERIMENTAL_I2CBUS) && I2C_SLAVE_ADDRESS > 0 + i2c.onReceive(i2c_on_receive); + i2c.onRequest(i2c_on_request); + #endif +} + +/** + * The main Marlin program loop + * + * - Save or log commands to SD + * - Process available commands (if not saving) + * - Call heater manager + * - Call inactivity manager + * - Call endstop manager + * - Call LCD update + */ +void loop() { + if (commands_in_queue < BUFSIZE) get_available_commands(); + + #if ENABLED(SDSUPPORT) + card.checkautostart(false); + #endif + + if (commands_in_queue) { + + #if ENABLED(SDSUPPORT) + + if (card.saving) { + char* command = command_queue[cmd_queue_index_r]; + if (strstr_P(command, PSTR("M29"))) { + // M29 closes the file + card.closefile(); + SERIAL_PROTOCOLLNPGM(MSG_FILE_SAVED); + ok_to_send(); + } + else { + // Write the string from the read buffer to SD + card.write_command(command); + if (card.logging) + process_next_command(); // The card is saving because it's logging + else + ok_to_send(); + } + } + else + process_next_command(); + + #else + + process_next_command(); + + #endif // SDSUPPORT + + // The queue may be reset by a command handler or by code invoked by idle() within a handler + if (commands_in_queue) { + --commands_in_queue; + cmd_queue_index_r = (cmd_queue_index_r + 1) % BUFSIZE; + } + } + endstops.report_state(); + idle(); +}