Skip to content

M
MarlinKimbra
Commit 55a2c790 authored by Stefy Lanza (nextime / spora )'s avatar Stefy Lanza (nextime / spora )
Browse files
Options

Merge branch 'dev'

parents 18664abd e5d046c6
Pipeline #89 skipped
Show whitespace changes
Inline Side-by-side
Showing with 3435 additions and 2874 deletions
MK/Configuration_Cartesian.h
View file @ 55a2c790
...@@ -15,8 +15,8 @@ ...@@ -15,8 +15,8 @@
* - Disables axis * - Disables axis
* - Travel limits * - Travel limits
* - Axis relative mode * - Axis relative mode
* - MBL or ABL * - Mesh Bed Leveling (MBL)
* - Auto bed levelling * - Auto Bed Leveling (ABL)
* - Z probe endstop * - Z probe endstop
* - Safe Z homing * - Safe Z homing
* - Manual home positions * - Manual home positions
...@@ -28,6 +28,7 @@ ...@@ -28,6 +28,7 @@
* - Cartesian Correction * - Cartesian Correction
* *
* Basic-settings can be found in Configuration_Basic.h * Basic-settings can be found in Configuration_Basic.h
* Temperature-settings can be found in Configuration_Temperature.h
* Feature-settings can be found in Configuration_Feature.h * Feature-settings can be found in Configuration_Feature.h
* Pins-settings can be found in "Configuration_Pins.h" * Pins-settings can be found in "Configuration_Pins.h"
*/ */
...@@ -229,24 +230,7 @@ ...@@ -229,24 +230,7 @@
/***************************************************************************************** /*****************************************************************************************
************************************** MBL or ABL *************************************** ******************************* Mesh Bed Leveling ***************************************
*****************************************************************************************
* *
* Manual Bed Leveling (MBL) or Auto Bed Leveling (ABL) settings *
* Set the rectangle in which to probe in MBL or ABL. *
* *
*****************************************************************************************/
#define LEFT_PROBE_BED_POSITION 20
#define RIGHT_PROBE_BED_POSITION 180
#define FRONT_PROBE_BED_POSITION 20
#define BACK_PROBE_BED_POSITION 180
#define XY_TRAVEL_SPEED 10000 // X and Y axis travel speed between probes, in mm/min
/*****************************************************************************************/
/*****************************************************************************************
******************************* Mesh bed leveling ***************************************
*****************************************************************************************/ *****************************************************************************************/
//#define MESH_BED_LEVELING //#define MESH_BED_LEVELING
...@@ -266,7 +250,7 @@ ...@@ -266,7 +250,7 @@
/***************************************************************************************** /*****************************************************************************************
******************************* Auto bed leveling *************************************** ******************************* Auto Bed Leveling ***************************************
***************************************************************************************** *****************************************************************************************
* * * *
* There are 2 different ways to specify probing locations * * There are 2 different ways to specify probing locations *
...@@ -281,7 +265,7 @@ ...@@ -281,7 +265,7 @@
* You specify the XY coordinates of all 3 points. * * You specify the XY coordinates of all 3 points. *
* * * *
* * * *
* Uncomment AUTO_BED_LEVELING_FEATURE to enable * * Uncomment AUTO BED LEVELING FEATURE to enable *
* * * *
*****************************************************************************************/ *****************************************************************************************/
//#define AUTO_BED_LEVELING_FEATURE //#define AUTO_BED_LEVELING_FEATURE
...@@ -291,14 +275,22 @@ ...@@ -291,14 +275,22 @@
// Note: this feature generates 10KB extra code size // Note: this feature generates 10KB extra code size
#define AUTO_BED_LEVELING_GRID #define AUTO_BED_LEVELING_GRID
// yes AUTO_BED_LEVELING_GRID // START yes AUTO BED LEVELING GRID
#define MIN_PROBE_EDGE 10 // The probe square sides can be no smaller than this #define LEFT_PROBE_BED_POSITION 20
#define RIGHT_PROBE_BED_POSITION 180
#define FRONT_PROBE_BED_POSITION 20
#define BACK_PROBE_BED_POSITION 180
// The probe square sides can be no smaller than this
#define MIN_PROBE_EDGE 10
// Set the number of grid points per dimension // Set the number of grid points per dimension
// You probably don't need more than 3 (squared=9) // You probably don't need more than 3 (squared=9)
#define AUTO_BED_LEVELING_GRID_POINTS 2 #define AUTO_BED_LEVELING_GRID_POINTS 2
// yes AUTO_BED_LEVELING_GRID // END yes AUTO BED LEVELING GRID
// no AUTO_BED_LEVELING_GRID // START no AUTO BED LEVELING GRID
// Arbitrary points to probe. A simple cross-product // Arbitrary points to probe. A simple cross-product
// is used to estimate the plane of the bed. // is used to estimate the plane of the bed.
#define ABL_PROBE_PT_1_X 15 #define ABL_PROBE_PT_1_X 15
...@@ -307,7 +299,7 @@ ...@@ -307,7 +299,7 @@
#define ABL_PROBE_PT_2_Y 15 #define ABL_PROBE_PT_2_Y 15
#define ABL_PROBE_PT_3_X 180 #define ABL_PROBE_PT_3_X 180
#define ABL_PROBE_PT_3_Y 15 #define ABL_PROBE_PT_3_Y 15
// no AUTO_BED_LEVELING_GRID // END no AUTO BED LEVELING GRID
// Offsets to the probe relative to the extruder tip (Hotend - Probe) // Offsets to the probe relative to the extruder tip (Hotend - Probe)
// X and Y offsets MUST be INTEGERS // X and Y offsets MUST be INTEGERS
...@@ -321,10 +313,13 @@ ...@@ -321,10 +313,13 @@
// | P (-) | T <-- probe (-10,-10) // | P (-) | T <-- probe (-10,-10)
// | | // | |
// O-- FRONT --+ // O-- FRONT --+
// (0,0)
#define X_PROBE_OFFSET_FROM_EXTRUDER 0 // X offset: -left [of the nozzle] +right #define X_PROBE_OFFSET_FROM_EXTRUDER 0 // X offset: -left [of the nozzle] +right
#define Y_PROBE_OFFSET_FROM_EXTRUDER 0 // Y offset: -front [of the nozzle] +behind #define Y_PROBE_OFFSET_FROM_EXTRUDER 0 // Y offset: -front [of the nozzle] +behind
#define Z_PROBE_OFFSET_FROM_EXTRUDER -1 // Z offset: -below [of the nozzle] (always negative!) #define Z_PROBE_OFFSET_FROM_EXTRUDER -1 // Z offset: -below [of the nozzle] (always negative!)
#define XY_TRAVEL_SPEED 10000 // X and Y axis travel speed between probes, in mm/min
#define Z_RAISE_BEFORE_PROBING 10 //How much the extruder will be raised before travelling to the first probing point. #define Z_RAISE_BEFORE_PROBING 10 //How much the extruder will be raised before travelling to the first probing point.
#define Z_RAISE_BETWEEN_PROBINGS 5 //How much the extruder will be raised when travelling from between next probing points #define Z_RAISE_BETWEEN_PROBINGS 5 //How much the extruder will be raised when travelling from between next probing points
#define Z_RAISE_AFTER_PROBING 5 //How much the extruder will be raised after the last probing point. #define Z_RAISE_AFTER_PROBING 5 //How much the extruder will be raised after the last probing point.
......
MK/Configuration_Core.h
View file @ 55a2c790
...@@ -16,8 +16,8 @@ ...@@ -16,8 +16,8 @@
* - Disables axis * - Disables axis
* - Travel limits * - Travel limits
* - Axis relative mode * - Axis relative mode
* - MBL or ABL * - Mesh Bed Leveling (MBL)
* - Auto bed levelling * - Auto Bed Leveling (ABL)
* - Z probe endstop * - Z probe endstop
* - Safe Z homing * - Safe Z homing
* - Manual home positions * - Manual home positions
...@@ -28,6 +28,7 @@ ...@@ -28,6 +28,7 @@
* - Hotend offset * - Hotend offset
* *
* Basic-settings can be found in Configuration_Basic.h * Basic-settings can be found in Configuration_Basic.h
* Temperature-settings can be found in Configuration_Temperature.h
* Feature-settings can be found in Configuration_Feature.h * Feature-settings can be found in Configuration_Feature.h
* Pins-settings can be found in "Configuration_Pins.h" * Pins-settings can be found in "Configuration_Pins.h"
*/ */
...@@ -251,24 +252,7 @@ ...@@ -251,24 +252,7 @@
/***************************************************************************************** /*****************************************************************************************
************************************** MBL or ABL *************************************** ******************************* Mesh Bed Leveling ***************************************
*****************************************************************************************
* *
* Manual Bed Leveling (MBL) or Auto Bed Leveling (ABL) settings *
* Set the rectangle in which to probe in MBL or ABL. *
* *
*****************************************************************************************/
#define LEFT_PROBE_BED_POSITION 20
#define RIGHT_PROBE_BED_POSITION 180
#define FRONT_PROBE_BED_POSITION 20
#define BACK_PROBE_BED_POSITION 180
#define XY_TRAVEL_SPEED 10000 // X and Y axis travel speed between probes, in mm/min
/*****************************************************************************************/
/*****************************************************************************************
******************************* Mesh bed leveling ***************************************
*****************************************************************************************/ *****************************************************************************************/
//#define MESH_BED_LEVELING //#define MESH_BED_LEVELING
...@@ -288,7 +272,7 @@ ...@@ -288,7 +272,7 @@
/***************************************************************************************** /*****************************************************************************************
******************************* Auto bed leveling *************************************** ******************************* Auto Bed Leveling ***************************************
***************************************************************************************** *****************************************************************************************
* * * *
* There are 2 different ways to specify probing locations * * There are 2 different ways to specify probing locations *
...@@ -303,7 +287,7 @@ ...@@ -303,7 +287,7 @@
* You specify the XY coordinates of all 3 points. * * You specify the XY coordinates of all 3 points. *
* * * *
* * * *
* Uncomment AUTO_BED_LEVELING_FEATURE to enable * * Uncomment AUTO BED LEVELING FEATURE to enable *
* * * *
*****************************************************************************************/ *****************************************************************************************/
//#define AUTO_BED_LEVELING_FEATURE //#define AUTO_BED_LEVELING_FEATURE
...@@ -313,14 +297,22 @@ ...@@ -313,14 +297,22 @@
// Note: this feature generates 10KB extra code size // Note: this feature generates 10KB extra code size
#define AUTO_BED_LEVELING_GRID #define AUTO_BED_LEVELING_GRID
// yes AUTO_BED_LEVELING_GRID // START yes AUTO BED LEVELING GRID
#define MIN_PROBE_EDGE 10 // The probe square sides can be no smaller than this #define LEFT_PROBE_BED_POSITION 20
#define RIGHT_PROBE_BED_POSITION 180
#define FRONT_PROBE_BED_POSITION 20
#define BACK_PROBE_BED_POSITION 180
// The probe square sides can be no smaller than this
#define MIN_PROBE_EDGE 10
// Set the number of grid points per dimension // Set the number of grid points per dimension
// You probably don't need more than 3 (squared=9) // You probably don't need more than 3 (squared=9)
#define AUTO_BED_LEVELING_GRID_POINTS 2 #define AUTO_BED_LEVELING_GRID_POINTS 2
// yes AUTO_BED_LEVELING_GRID // END yes AUTO BED LEVELING GRID
// no AUTO_BED_LEVELING_GRID // START no AUTO BED LEVELING GRID
// Arbitrary points to probe. A simple cross-product // Arbitrary points to probe. A simple cross-product
// is used to estimate the plane of the bed. // is used to estimate the plane of the bed.
#define ABL_PROBE_PT_1_X 15 #define ABL_PROBE_PT_1_X 15
...@@ -329,7 +321,7 @@ ...@@ -329,7 +321,7 @@
#define ABL_PROBE_PT_2_Y 15 #define ABL_PROBE_PT_2_Y 15
#define ABL_PROBE_PT_3_X 180 #define ABL_PROBE_PT_3_X 180
#define ABL_PROBE_PT_3_Y 15 #define ABL_PROBE_PT_3_Y 15
// no AUTO_BED_LEVELING_GRID // END no AUTO BED LEVELING GRID
// Offsets to the probe relative to the extruder tip (Hotend - Probe) // Offsets to the probe relative to the extruder tip (Hotend - Probe)
// X and Y offsets MUST be INTEGERS // X and Y offsets MUST be INTEGERS
...@@ -343,10 +335,13 @@ ...@@ -343,10 +335,13 @@
// | P (-) | T <-- probe (-10,-10) // | P (-) | T <-- probe (-10,-10)
// | | // | |
// O-- FRONT --+ // O-- FRONT --+
// (0,0)
#define X_PROBE_OFFSET_FROM_EXTRUDER 0 // X offset: -left [of the nozzle] +right #define X_PROBE_OFFSET_FROM_EXTRUDER 0 // X offset: -left [of the nozzle] +right
#define Y_PROBE_OFFSET_FROM_EXTRUDER 0 // Y offset: -front [of the nozzle] +behind #define Y_PROBE_OFFSET_FROM_EXTRUDER 0 // Y offset: -front [of the nozzle] +behind
#define Z_PROBE_OFFSET_FROM_EXTRUDER -1 // Z offset: -below [of the nozzle] (always negative!) #define Z_PROBE_OFFSET_FROM_EXTRUDER -1 // Z offset: -below [of the nozzle] (always negative!)
#define XY_TRAVEL_SPEED 10000 // X and Y axis travel speed between probes, in mm/min
#define Z_RAISE_BEFORE_PROBING 10 //How much the extruder will be raised before travelling to the first probing point. #define Z_RAISE_BEFORE_PROBING 10 //How much the extruder will be raised before travelling to the first probing point.
#define Z_RAISE_BETWEEN_PROBINGS 5 //How much the extruder will be raised when travelling from between next probing points #define Z_RAISE_BETWEEN_PROBINGS 5 //How much the extruder will be raised when travelling from between next probing points
#define Z_RAISE_AFTER_PROBING 5 //How much the extruder will be raised after the last probing point. #define Z_RAISE_AFTER_PROBING 5 //How much the extruder will be raised after the last probing point.
......
MK/Configuration_Scara.h
View file @ 55a2c790
...@@ -9,6 +9,7 @@ ...@@ -9,6 +9,7 @@
* - Endstop pullup resistors * - Endstop pullup resistors
* - Endstops logic * - Endstops logic
* - Endstops min or max * - Endstops min or max
* - Min Z height for homing
* - Stepper enable logic * - Stepper enable logic
* - Stepper step logic * - Stepper step logic
* - Stepper direction * - Stepper direction
...@@ -24,6 +25,7 @@ ...@@ -24,6 +25,7 @@
* - Hotend offset * - Hotend offset
* *
* Basic-settings can be found in Configuration_Basic.h * Basic-settings can be found in Configuration_Basic.h
* Temperature-settings can be found in Configuration_Temperature.h
* Feature-settings can be found in Configuration_Feature.h * Feature-settings can be found in Configuration_Feature.h
* Pins-settings can be found in "Configuration_Pins.h" * Pins-settings can be found in "Configuration_Pins.h"
*/ */
...@@ -124,6 +126,18 @@ ...@@ -124,6 +126,18 @@
/*****************************************************************************************/ /*****************************************************************************************/
/*****************************************************************************************
***************************** MIN Z HEIGHT FOR HOMING **********************************
*****************************************************************************************
* *
* (in mm) Minimal z height before homing (G28) for Z clearance above the bed, clamps, *
* Be sure you have this distance over your Z_MAX_POS in case. *
* *
*****************************************************************************************/
#define MIN_Z_HEIGHT_FOR_HOMING 0
/*****************************************************************************************/
/***************************************************************************************** /*****************************************************************************************
********************************* Stepper enable logic ********************************** ********************************* Stepper enable logic **********************************
***************************************************************************************** *****************************************************************************************
......
MK/Configuration_Store.cpp
View file @ 55a2c790
...@@ -43,21 +43,30 @@ ...@@ -43,21 +43,30 @@
/** /**
* MKV428 EEPROM Layout: * MKV428 EEPROM Layout:
* *
* ver * Version
* M92 XYZ E0 ... axis_steps_per_unit X,Y,Z,E0 ... (per extruder) *
* M203 XYZ E0 ... max_feedrate X,Y,Z,E0 ... (per extruder) * M92 XYZ E0 ... planner.axis_steps_per_unit X,Y,Z,E0 ... (per extruder)
* M201 XYZ E0 ... max_acceleration_units_per_sq_second X,Y,Z,E0 ... (per extruder) * M203 XYZ E0 ... planner.max_feedrate X,Y,Z,E0 ... (per extruder)
* M204 P acceleration * M201 XYZ E0 ... planner.max_acceleration_units_per_sq_second X,Y,Z,E0 ... (per extruder)
* M204 R E0 ... retract_acceleration (per extruder) * M204 P planner.acceleration
* M204 T travel_acceleration * M204 R E0 ... planner.retract_acceleration (per extruder)
* M205 S minimumfeedrate * M204 T planner.travel_acceleration
* M205 T mintravelfeedrate * M205 S planner.min_feedrate
* M205 B minsegmenttime * M205 T planner.min_travel_feedrate
* M205 X max_xy_jerk * M205 B planner.min_segment_time
* M205 Z max_z_jerk * M205 X planner.max_xy_jerk
* M205 E E0 ... max_e_jerk (per extruder) * M205 Z planner.max_z_jerk
* M205 E E0 ... planner.max_e_jerk (per extruder)
* M206 XYZ home_offset (x3) * M206 XYZ home_offset (x3)
* M218 T XY hotend_offset (x4) (T0..3) * M218 T XY hotend_offset (x4) (T0..3)
*
* Mesh bed leveling:
* M420 S status (uint8)
* z_offset (float)
* mesh_num_x (uint8 as set in firmware)
* mesh_num_y (uint8 as set in firmware)
* G29 S3 XYZ z_values[][] (float)
*
* M666 P zprobe_zoffset * M666 P zprobe_zoffset
* *
* HOTENDS AD595: * HOTENDS AD595:
...@@ -164,26 +173,31 @@ void Config_StoreSettings() { ...@@ -164,26 +173,31 @@ void Config_StoreSettings() {
char ver[7] = "000000"; char ver[7] = "000000";
int i = EEPROM_OFFSET; int i = EEPROM_OFFSET;
EEPROM_WRITE_VAR(i, ver); // invalidate data first EEPROM_WRITE_VAR(i, ver); // invalidate data first
EEPROM_WRITE_VAR(i, axis_steps_per_unit); EEPROM_WRITE_VAR(i, planner.axis_steps_per_unit);
EEPROM_WRITE_VAR(i, max_feedrate); EEPROM_WRITE_VAR(i, planner.max_feedrate);
EEPROM_WRITE_VAR(i, max_acceleration_units_per_sq_second); EEPROM_WRITE_VAR(i, planner.max_acceleration_units_per_sq_second);
EEPROM_WRITE_VAR(i, acceleration); EEPROM_WRITE_VAR(i, planner.acceleration);
EEPROM_WRITE_VAR(i, retract_acceleration); EEPROM_WRITE_VAR(i, planner.retract_acceleration);
EEPROM_WRITE_VAR(i, travel_acceleration); EEPROM_WRITE_VAR(i, planner.travel_acceleration);
EEPROM_WRITE_VAR(i, minimumfeedrate); EEPROM_WRITE_VAR(i, planner.min_feedrate);
EEPROM_WRITE_VAR(i, mintravelfeedrate); EEPROM_WRITE_VAR(i, planner.min_travel_feedrate);
EEPROM_WRITE_VAR(i, minsegmenttime); EEPROM_WRITE_VAR(i, planner.min_segment_time);
EEPROM_WRITE_VAR(i, max_xy_jerk); EEPROM_WRITE_VAR(i, planner.max_xy_jerk);
EEPROM_WRITE_VAR(i, max_z_jerk); EEPROM_WRITE_VAR(i, planner.max_z_jerk);
EEPROM_WRITE_VAR(i, max_e_jerk); EEPROM_WRITE_VAR(i, planner.max_e_jerk);
EEPROM_WRITE_VAR(i, home_offset); EEPROM_WRITE_VAR(i, home_offset);
EEPROM_WRITE_VAR(i, hotend_offset); EEPROM_WRITE_VAR(i, hotend_offset);
#if ENABLED(MESH_BED_LEVELING) #if ENABLED(MESH_BED_LEVELING)
// Compile time test that sizeof(mbl.z_values) is as expected // Compile time test that sizeof(mbl.z_values) is as expected
typedef char c_assert[(sizeof(mbl.z_values) == (MESH_NUM_X_POINTS) * (MESH_NUM_Y_POINTS) * sizeof(dummy)) ? 1 : -1]; typedef char c_assert[(sizeof(mbl.z_values) == (MESH_NUM_X_POINTS) * (MESH_NUM_Y_POINTS) * sizeof(dummy)) ? 1 : -1];
EEPROM_WRITE_VAR(i, mbl.active); uint8_t mesh_num_x = MESH_NUM_X_POINTS,
mesh_num_y = MESH_NUM_Y_POINTS,
dummy_uint8 = mbl.status & _BV(MBL_STATUS_HAS_MESH_BIT);
EEPROM_WRITE_VAR(i, dummy_uint8);
EEPROM_WRITE_VAR(i, mbl.z_offset); EEPROM_WRITE_VAR(i, mbl.z_offset);
EEPROM_WRITE_VAR(i, mesh_num_x);
EEPROM_WRITE_VAR(i, mesh_num_y);
EEPROM_WRITE_VAR(i, mbl.z_values); EEPROM_WRITE_VAR(i, mbl.z_values);
#endif #endif
...@@ -327,28 +341,31 @@ void Config_RetrieveSettings() { ...@@ -327,28 +341,31 @@ void Config_RetrieveSettings() {
float dummy = 0; float dummy = 0;
// version number match // version number match
EEPROM_READ_VAR(i, axis_steps_per_unit); EEPROM_READ_VAR(i, planner.axis_steps_per_unit);
EEPROM_READ_VAR(i, max_feedrate); EEPROM_READ_VAR(i, planner.max_feedrate);
EEPROM_READ_VAR(i, max_acceleration_units_per_sq_second); EEPROM_READ_VAR(i, planner.max_acceleration_units_per_sq_second);
// steps per sq second need to be updated to agree with the units per sq second (as they are what is used in the planner) // steps per sq second need to be updated to agree with the units per sq second (as they are what is used in the planner)
reset_acceleration_rates(); planner.reset_acceleration_rates();
EEPROM_READ_VAR(i, acceleration); EEPROM_READ_VAR(i, planner.acceleration);
EEPROM_READ_VAR(i, retract_acceleration); EEPROM_READ_VAR(i, planner.retract_acceleration);
EEPROM_READ_VAR(i, travel_acceleration); EEPROM_READ_VAR(i, planner.travel_acceleration);
EEPROM_READ_VAR(i, minimumfeedrate); EEPROM_READ_VAR(i, planner.min_feedrate);
EEPROM_READ_VAR(i, mintravelfeedrate); EEPROM_READ_VAR(i, planner.min_travel_feedrate);
EEPROM_READ_VAR(i, minsegmenttime); EEPROM_READ_VAR(i, planner.min_segment_time);
EEPROM_READ_VAR(i, max_xy_jerk); EEPROM_READ_VAR(i, planner.max_xy_jerk);
EEPROM_READ_VAR(i, max_z_jerk); EEPROM_READ_VAR(i, planner.max_z_jerk);
EEPROM_READ_VAR(i, max_e_jerk); EEPROM_READ_VAR(i, planner.max_e_jerk);
EEPROM_READ_VAR(i, home_offset); EEPROM_READ_VAR(i, home_offset);
EEPROM_READ_VAR(i, hotend_offset); EEPROM_READ_VAR(i, hotend_offset);
#if ENABLED(MESH_BED_LEVELING) #if ENABLED(MESH_BED_LEVELING)
EPROM_READ_VAR(i, mbl.active); uint8_t mesh_num_x = 0, mesh_num_y = 0;
EEPROM_READ_VAR(i, mbl.status);
EEPROM_READ_VAR(i, mbl.z_offset); EEPROM_READ_VAR(i, mbl.z_offset);
EEPROM_READ_VAR(i, mesh_num_x);
EEPROM_READ_VAR(i, mesh_num_y);
EEPROM_READ_VAR(i, mbl.z_values); EEPROM_READ_VAR(i, mbl.z_values);
#endif #endif
...@@ -508,14 +525,14 @@ void Config_ResetDefault() { ...@@ -508,14 +525,14 @@ void Config_ResetDefault() {
#endif #endif
for (int8_t i = 0; i < 3 + EXTRUDERS; i++) { for (int8_t i = 0; i < 3 + EXTRUDERS; i++) {
axis_steps_per_unit[i] = tmp1[i]; planner.axis_steps_per_unit[i] = tmp1[i];
max_feedrate[i] = tmp2[i]; planner.max_feedrate[i] = tmp2[i];
max_acceleration_units_per_sq_second[i] = tmp3[i]; planner.max_acceleration_units_per_sq_second[i] = tmp3[i];
} }
for (int8_t i = 0; i < EXTRUDERS; i++) { for (int8_t i = 0; i < EXTRUDERS; i++) {
retract_acceleration[i] = tmp4[i]; planner.retract_acceleration[i] = tmp4[i];
max_e_jerk[i] = tmp5[i]; planner.max_e_jerk[i] = tmp5[i];
} }
#if MB(ALLIGATOR) #if MB(ALLIGATOR)
...@@ -543,19 +560,19 @@ void Config_ResetDefault() { ...@@ -543,19 +560,19 @@ void Config_ResetDefault() {
#endif #endif
// steps per sq second need to be updated to agree with the units per sq second // steps per sq second need to be updated to agree with the units per sq second
reset_acceleration_rates(); planner.reset_acceleration_rates();
acceleration = DEFAULT_ACCELERATION; planner.acceleration = DEFAULT_ACCELERATION;
travel_acceleration = DEFAULT_TRAVEL_ACCELERATION; planner.travel_acceleration = DEFAULT_TRAVEL_ACCELERATION;
minimumfeedrate = DEFAULT_MINIMUMFEEDRATE; planner.min_feedrate = DEFAULT_MINIMUMFEEDRATE;
minsegmenttime = DEFAULT_MINSEGMENTTIME; planner.min_segment_time = DEFAULT_MINSEGMENTTIME;
mintravelfeedrate = DEFAULT_MINTRAVELFEEDRATE; planner.min_travel_feedrate = DEFAULT_MINTRAVELFEEDRATE;
max_xy_jerk = DEFAULT_XYJERK; planner.max_xy_jerk = DEFAULT_XYJERK;
max_z_jerk = DEFAULT_ZJERK; planner.max_z_jerk = DEFAULT_ZJERK;
home_offset[X_AXIS] = home_offset[Y_AXIS] = home_offset[Z_AXIS] = 0; home_offset[X_AXIS] = home_offset[Y_AXIS] = home_offset[Z_AXIS] = 0;
#if ENABLED(MESH_BED_LEVELING) #if ENABLED(MESH_BED_LEVELING)
mbl.active = false; mbl.reset();
#endif #endif
#if ENABLED(AUTO_BED_LEVELING_FEATURE) #if ENABLED(AUTO_BED_LEVELING_FEATURE)
...@@ -669,14 +686,14 @@ void Config_ResetDefault() { ...@@ -669,14 +686,14 @@ void Config_ResetDefault() {
if (!forReplay) { if (!forReplay) {
ECHO_LM(CFG, "Steps per unit:"); ECHO_LM(CFG, "Steps per unit:");
} }
ECHO_SMV(CFG, " M92 X", axis_steps_per_unit[X_AXIS]); ECHO_SMV(CFG, " M92 X", planner.axis_steps_per_unit[X_AXIS]);
ECHO_MV(" Y", axis_steps_per_unit[Y_AXIS]); ECHO_MV(" Y", planner.axis_steps_per_unit[Y_AXIS]);
ECHO_MV(" Z", axis_steps_per_unit[Z_AXIS]); ECHO_MV(" Z", planner.axis_steps_per_unit[Z_AXIS]);
ECHO_EMV(" E", axis_steps_per_unit[E_AXIS]); ECHO_EMV(" E", planner.axis_steps_per_unit[E_AXIS]);
#if EXTRUDERS > 1 #if EXTRUDERS > 1
for (short i = 1; i < EXTRUDERS; i++) { for (short i = 1; i < EXTRUDERS; i++) {
ECHO_SMV(CFG, " M92 T", i); ECHO_SMV(CFG, " M92 T", i);
ECHO_EMV(" E", axis_steps_per_unit[E_AXIS + i]); ECHO_EMV(" E", planner.axis_steps_per_unit[E_AXIS + i]);
} }
#endif //EXTRUDERS > 1 #endif //EXTRUDERS > 1
...@@ -692,28 +709,28 @@ void Config_ResetDefault() { ...@@ -692,28 +709,28 @@ void Config_ResetDefault() {
if (!forReplay) { if (!forReplay) {
ECHO_LM(CFG, "Maximum feedrates (mm/s):"); ECHO_LM(CFG, "Maximum feedrates (mm/s):");
} }
ECHO_SMV(CFG, " M203 X", max_feedrate[X_AXIS]); ECHO_SMV(CFG, " M203 X", planner.max_feedrate[X_AXIS]);
ECHO_MV(" Y", max_feedrate[Y_AXIS] ); ECHO_MV(" Y", planner.max_feedrate[Y_AXIS] );
ECHO_MV(" Z", max_feedrate[Z_AXIS] ); ECHO_MV(" Z", planner.max_feedrate[Z_AXIS] );
ECHO_EMV(" E", max_feedrate[E_AXIS]); ECHO_EMV(" E", planner.max_feedrate[E_AXIS]);
#if EXTRUDERS > 1 #if EXTRUDERS > 1
for (short i = 1; i < EXTRUDERS; i++) { for (short i = 1; i < EXTRUDERS; i++) {
ECHO_SMV(CFG, " M203 T", i); ECHO_SMV(CFG, " M203 T", i);
ECHO_EMV(" E", max_feedrate[E_AXIS + i]); ECHO_EMV(" E", planner.max_acceleration_units_per_sq_second[E_AXIS + i]);
} }
#endif //EXTRUDERS > 1 #endif //EXTRUDERS > 1
if (!forReplay) { if (!forReplay) {
ECHO_LM(CFG, "Maximum Acceleration (mm/s2):"); ECHO_LM(CFG, "Maximum Acceleration (mm/s2):");
} }
ECHO_SMV(CFG, " M201 X", max_acceleration_units_per_sq_second[X_AXIS] ); ECHO_SMV(CFG, " M201 X", planner.max_acceleration_units_per_sq_second[X_AXIS] );
ECHO_MV(" Y", max_acceleration_units_per_sq_second[Y_AXIS] ); ECHO_MV(" Y", planner.max_acceleration_units_per_sq_second[Y_AXIS] );
ECHO_MV(" Z", max_acceleration_units_per_sq_second[Z_AXIS] ); ECHO_MV(" Z", planner.max_acceleration_units_per_sq_second[Z_AXIS] );
ECHO_EMV(" E", max_acceleration_units_per_sq_second[E_AXIS]); ECHO_EMV(" E", planner.max_acceleration_units_per_sq_second[E_AXIS]);
#if EXTRUDERS > 1 #if EXTRUDERS > 1
for (int8_t i = 1; i < EXTRUDERS; i++) { for (int8_t i = 1; i < EXTRUDERS; i++) {
ECHO_SMV(CFG, " M201 T", i); ECHO_SMV(CFG, " M201 T", i);
ECHO_EMV(" E", max_acceleration_units_per_sq_second[E_AXIS + i]); ECHO_EMV(" E", planner.max_acceleration_units_per_sq_second[E_AXIS + i]);
} }
#endif //EXTRUDERS > 1 #endif //EXTRUDERS > 1
ECHO_E; ECHO_E;
...@@ -721,28 +738,28 @@ void Config_ResetDefault() { ...@@ -721,28 +738,28 @@ void Config_ResetDefault() {
if (!forReplay) { if (!forReplay) {
ECHO_LM(CFG, "Accelerations: P=printing, V=travel and T* R=retract"); ECHO_LM(CFG, "Accelerations: P=printing, V=travel and T* R=retract");
} }
ECHO_SMV(CFG," M204 P", acceleration); ECHO_SMV(CFG," M204 P", planner.acceleration);
ECHO_EMV(" V", travel_acceleration); ECHO_EMV(" V", planner.travel_acceleration);
#if EXTRUDERS > 0 #if EXTRUDERS > 0
for (int8_t i = 0; i < EXTRUDERS; i++) { for (int8_t i = 0; i < EXTRUDERS; i++) {
ECHO_SMV(CFG, " M204 T", i); ECHO_SMV(CFG, " M204 T", i);
ECHO_EMV(" R", retract_acceleration[i]); ECHO_EMV(" R", planner.retract_acceleration[i]);
} }
#endif #endif
if (!forReplay) { if (!forReplay) {
ECHO_LM(CFG, "Advanced variables: S=Min feedrate (mm/s), V=Min travel feedrate (mm/s), B=minimum segment time (ms), X=maximum XY jerk (mm/s), Z=maximum Z jerk (mm/s), E=maximum E jerk (mm/s)"); ECHO_LM(CFG, "Advanced variables: S=Min feedrate (mm/s), V=Min travel feedrate (mm/s), B=minimum segment time (ms), X=maximum XY jerk (mm/s), Z=maximum Z jerk (mm/s), E=maximum E jerk (mm/s)");
} }
ECHO_SMV(CFG, " M205 S", minimumfeedrate ); ECHO_SMV(CFG, " M205 S", planner.min_feedrate );
ECHO_MV(" V", mintravelfeedrate ); ECHO_MV(" V", planner.min_travel_feedrate );
ECHO_MV(" B", minsegmenttime ); ECHO_MV(" B", planner.min_segment_time );
ECHO_MV(" X", max_xy_jerk ); ECHO_MV(" X", planner.max_xy_jerk );
ECHO_MV(" Z", max_z_jerk); ECHO_MV(" Z", planner.max_z_jerk);
ECHO_EMV(" E", max_e_jerk[0]); ECHO_EMV(" E", planner.max_e_jerk[0]);
#if (EXTRUDERS > 1) #if (EXTRUDERS > 1)
for(int8_t i = 1; i < EXTRUDERS; i++) { for(int8_t i = 1; i < EXTRUDERS; i++) {
ECHO_SMV(CFG, " M205 T", i); ECHO_SMV(CFG, " M205 T", i);
ECHO_EMV(" E" , max_e_jerk[i]); ECHO_EMV(" E" , planner.max_e_jerk[i]);
} }
#endif #endif
...@@ -767,12 +784,13 @@ void Config_ResetDefault() { ...@@ -767,12 +784,13 @@ void Config_ResetDefault() {
if (!forReplay) { if (!forReplay) {
ECHO_LM(CFG, "Mesh bed leveling:"); ECHO_LM(CFG, "Mesh bed leveling:");
} }
ECHO_SMV(CFG, " M420 S", mbl.active); ECHO_SMV(CFG, " M420 S", mbl.has_mesh() ? 1 : 0);
ECHO_MV(" X", MESH_NUM_X_POINTS); ECHO_MV(" X", MESH_NUM_X_POINTS);
ECHO_EMV(" Y", MESH_NUM_Y_POINTS); ECHO_MV(" Y", MESH_NUM_Y_POINTS);
ECHO_E;
for (int py = 1; py <= MESH_NUM_Y_POINTS; py++) { for (uint8_t py = 1; py <= MESH_NUM_Y_POINTS; py++) {
for (int px = 1; px <= MESH_NUM_X_POINTS; px++) { for (uint8_t px = 1; px <= MESH_NUM_X_POINTS; px++) {
ECHO_SMV(CFG, " G29 S3 X", px); ECHO_SMV(CFG, " G29 S3 X", px);
ECHO_MV(" Y", py); ECHO_MV(" Y", py);
ECHO_EMV(" Z", mbl.z_values[py-1][px-1], 5); ECHO_EMV(" Z", mbl.z_values[py-1][px-1], 5);
......
MK/base.h
View file @ 55a2c790
...@@ -61,12 +61,10 @@ ...@@ -61,12 +61,10 @@
#include "module/language/language.h" #include "module/language/language.h"
#include "module/printcounter/printcounter.h" #include "module/printcounter/printcounter.h"
#include "module/MK_Main.h" #include "module/MK_Main.h"
#include "module/motion/planner.h" #include "module/planner/planner.h"
#include "module/endstop/endstops.h"
#include "module/motion/stepper_indirection.h" #include "module/motion/stepper_indirection.h"
#include "module/motion/stepper.h" #include "module/motion/stepper.h"
#include "module/motion/endstops.h"
#include "module/motion/vector_3.h"
#include "module/motion/qr_solve.h"
#include "module/motion/cartesian_correction.h" #include "module/motion/cartesian_correction.h"
#include "module/temperature/temperature.h" #include "module/temperature/temperature.h"
#include "module/sensor/flowmeter.h" #include "module/sensor/flowmeter.h"
......
MK/module/MK_Main.cpp
View file @ 55a2c790
...@@ -30,6 +30,13 @@ ...@@ -30,6 +30,13 @@
#include "../base.h" #include "../base.h"
#if ENABLED(AUTO_BED_LEVELING_FEATURE)
#include "planner/vector_3.h"
#if ENABLED(AUTO_BED_LEVELING_GRID)
#include "planner/qr_solve.h"
#endif
#endif // AUTO_BED_LEVELING_FEATURE
#if ENABLED(RFID_MODULE) #if ENABLED(RFID_MODULE)
MFRC522 RFID522; MFRC522 RFID522;
#endif #endif
...@@ -127,12 +134,12 @@ static uint8_t target_extruder; ...@@ -127,12 +134,12 @@ static uint8_t target_extruder;
bool software_endstops = true; bool software_endstops = true;
#if !MECH(DELTA) #if NOMECH(DELTA)
int xy_travel_speed = XY_TRAVEL_SPEED; int xy_travel_speed = XY_TRAVEL_SPEED;
float zprobe_zoffset = 0; float zprobe_zoffset = 0;
#endif #endif
#if ENABLED(Z_DUAL_ENDSTOPS) && !MECH(DELTA) #if ENABLED(Z_DUAL_ENDSTOPS) && NOMECH(DELTA)
float z_endstop_adj = 0; float z_endstop_adj = 0;
#endif #endif
...@@ -692,7 +699,7 @@ void setup() { ...@@ -692,7 +699,7 @@ void setup() {
lcd_init(); // Initialize LCD lcd_init(); // Initialize LCD
tp_init(); // Initialize temperature loop tp_init(); // Initialize temperature loop
plan_init(); // Initialize planner; planner.init(); // Initialize planner;
#if ENABLED(USE_WATCHDOG) #if ENABLED(USE_WATCHDOG)
watchdog_init(); watchdog_init();
...@@ -1304,28 +1311,28 @@ inline void set_homing_bump_feedrate(AxisEnum axis) { ...@@ -1304,28 +1311,28 @@ inline void set_homing_bump_feedrate(AxisEnum axis) {
feedrate = homing_feedrate[axis] / hbd; feedrate = homing_feedrate[axis] / hbd;
} }
inline void line_to_current_position() { inline void line_to_current_position() {
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], feedrate/60, active_extruder, active_driver); planner.buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], feedrate/60, active_extruder, active_driver);
} }
inline void line_to_z(float zPosition) { inline void line_to_z(float zPosition) {
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS], feedrate/60, active_extruder, active_driver); planner.buffer_line(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS], feedrate/60, active_extruder, active_driver);
} }
inline void line_to_destination(float mm_m) { inline void line_to_destination(float mm_m) {
plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], mm_m/60, active_extruder, active_driver); planner.buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], mm_m/60, active_extruder, active_driver);
} }
inline void line_to_destination() { inline void line_to_destination() {
line_to_destination(feedrate); line_to_destination(feedrate);
} }
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]); planner.set_position_mm(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
} }
#if MECH(DELTA) || MECH(SCARA) #if MECH(DELTA) || MECH(SCARA)
inline void sync_plan_position_delta() { inline void sync_plan_position_delta() {
if (DEBUGGING(INFO)) DEBUG_POS(" > sync_plan_position_delta", current_position); if (DEBUGGING(INFO)) DEBUG_POS(" > sync_plan_position_delta", current_position);
calculate_delta(current_position); calculate_delta(current_position);
plan_set_position(delta[TOWER_1], delta[TOWER_2], delta[TOWER_3], current_position[E_AXIS]); planner.set_position_mm(delta[TOWER_1], delta[TOWER_2], delta[TOWER_3], current_position[E_AXIS]);
} }
#endif #endif
inline void sync_plan_position_e() { plan_set_e_position(current_position[E_AXIS]); } inline void sync_plan_position_e() { planner.set_e_position_mm(current_position[E_AXIS]); }
inline void set_current_to_destination() { memcpy(current_position, destination, sizeof(current_position)); } inline void set_current_to_destination() { memcpy(current_position, destination, sizeof(current_position)); }
inline void set_destination_to_current() { memcpy(destination, current_position, sizeof(destination)); } inline void set_destination_to_current() { memcpy(destination, current_position, sizeof(destination)); }
...@@ -1404,41 +1411,63 @@ inline void do_blocking_move_to_xy(float x, float y) { do_blocking_move_to(x, y, ...@@ -1404,41 +1411,63 @@ inline void do_blocking_move_to_xy(float x, float y) { do_blocking_move_to(x, y,
inline void do_blocking_move_to_x(float x) { do_blocking_move_to(x, current_position[Y_AXIS], current_position[Z_AXIS]); } inline void do_blocking_move_to_x(float x) { do_blocking_move_to(x, current_position[Y_AXIS], current_position[Z_AXIS]); }
inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], z); } inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], z); }
#if ENABLED(AUTO_BED_LEVELING_FEATURE)
inline void raise_z_after_probing() {
#if Z_RAISE_AFTER_PROBING > 0
if (DEBUGGING(INFO)) ECHO_LM(INFO, "raise_z_after_probing()");
do_blocking_move_to_z(current_position[Z_AXIS] + Z_RAISE_AFTER_PROBING);
#endif
}
#endif
#if NOMECH(DELTA) && NOMECH(SCARA)
void set_current_position_from_planner() {
st_synchronize();
#if ENABLED(AUTO_BED_LEVELING_FEATURE)
vector_3 pos = planner.adjusted_position(); // values directly from steppers...
current_position[X_AXIS] = pos.x;
current_position[Y_AXIS] = pos.y;
current_position[Z_AXIS] = pos.z;
#else
current_position[X_AXIS] = st_get_axis_position_mm(X_AXIS);
current_position[Y_AXIS] = st_get_axis_position_mm(Y_AXIS);
current_position[Z_AXIS] = st_get_axis_position_mm(Z_AXIS);
#endif
sync_plan_position(); // ...re-apply to planner position
}
#endif
#if MECH(CARTESIAN) || MECH(COREXY) || MECH(COREYX) || MECH(COREXZ) || MECH(COREZX) || MECH(SCARA) #if MECH(CARTESIAN) || MECH(COREXY) || MECH(COREYX) || MECH(COREXZ) || MECH(COREZX) || MECH(SCARA)
#if ENABLED(AUTO_BED_LEVELING_FEATURE) #if ENABLED(AUTO_BED_LEVELING_FEATURE)
#if ENABLED(AUTO_BED_LEVELING_GRID) #if ENABLED(AUTO_BED_LEVELING_GRID)
static void set_bed_level_equation_lsq(double *plane_equation_coefficients) { static void set_bed_level_equation_lsq(double *plane_equation_coefficients) {
if (DEBUGGING(INFO)) { if (DEBUGGING(INFO)) {
plan_bed_level_matrix.set_to_identity(); planner.bed_level_matrix.set_to_identity();
vector_3 uncorrected_position = plan_adjusted_position(); vector_3 uncorrected_position = planner.adjusted_position();
DEBUG_POS(">>> set_bed_level_equation_lsq", uncorrected_position); DEBUG_POS(">>> set_bed_level_equation_lsq", uncorrected_position);
DEBUG_POS(">>> set_bed_level_equation_lsq", current_position); DEBUG_POS(">>> set_bed_level_equation_lsq", current_position);
} }
vector_3 planeNormal = vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1);
planeNormal.debug("planeNormal");
plan_bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
//bedLevel.debug("bedLevel");
//plan_bed_level_matrix.debug("bed level before"); vector_3 planeNormal = vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1);
//vector_3 uncorrected_position = plan_get_position_mm(); planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
//uncorrected_position.debug("position before");
vector_3 corrected_position = plan_adjusted_position(); vector_3 corrected_position = planner.adjusted_position();
//corrected_position.debug("position after");
current_position[X_AXIS] = corrected_position.x; current_position[X_AXIS] = corrected_position.x;
current_position[Y_AXIS] = corrected_position.y; current_position[Y_AXIS] = corrected_position.y;
current_position[Z_AXIS] = corrected_position.z; current_position[Z_AXIS] = corrected_position.z;
if (DEBUGGING(INFO)) if (DEBUGGING(INFO))
DEBUG_POS("set_bed_level_equation_lsq", current_position); DEBUG_POS("<<< set_bed_level_equation_lsq", current_position);
sync_plan_position(); sync_plan_position();
} }
#else // not AUTO_BED_LEVELING_GRID #else // not AUTO_BED_LEVELING_GRID
static void set_bed_level_equation_3pts(float z_at_pt_1, float z_at_pt_2, float z_at_pt_3) { static void set_bed_level_equation_3pts(float z_at_pt_1, float z_at_pt_2, float z_at_pt_3) {
plan_bed_level_matrix.set_to_identity(); planner.bed_level_matrix.set_to_identity();
vector_3 pt1 = vector_3(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, z_at_pt_1); vector_3 pt1 = vector_3(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, z_at_pt_1);
vector_3 pt2 = vector_3(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, z_at_pt_2); vector_3 pt2 = vector_3(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, z_at_pt_2);
...@@ -1451,9 +1480,15 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio ...@@ -1451,9 +1480,15 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio
planeNormal.z = -planeNormal.z; planeNormal.z = -planeNormal.z;
} }
plan_bed_level_matrix = matrix_3x3::create_look_at(planeNormal); planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
vector_3 corrected_position = planner.adjusted_position();
if (DEBUGGING(INFO)) {
vector_3 uncorrected_position = corrected_position;
DEBUG_POS("set_bed_level_equation_3pts", uncorrected_position);
}
vector_3 corrected_position = plan_adjusted_position();
current_position[X_AXIS] = corrected_position.x; current_position[X_AXIS] = corrected_position.x;
current_position[Y_AXIS] = corrected_position.y; current_position[Y_AXIS] = corrected_position.y;
current_position[Z_AXIS] = corrected_position.z; current_position[Z_AXIS] = corrected_position.z;
...@@ -1474,7 +1509,7 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio ...@@ -1474,7 +1509,7 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio
*/ */
refresh_cmd_timeout(); refresh_cmd_timeout();
plan_bed_level_matrix.set_to_identity(); planner.bed_level_matrix.set_to_identity();
feedrate = homing_feedrate[Z_AXIS]; feedrate = homing_feedrate[Z_AXIS];
// Move down until the probe (or endstop?) is triggered // Move down until the probe (or endstop?) is triggered
...@@ -1484,7 +1519,7 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio ...@@ -1484,7 +1519,7 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio
// Tell the planner where we ended up - Get this from the stepper handler // Tell the planner where we ended up - Get this from the stepper handler
zPosition = st_get_axis_position_mm(Z_AXIS); zPosition = st_get_axis_position_mm(Z_AXIS);
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS]); planner.set_position_mm(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS]);
// move up the retract distance // move up the retract distance
zPosition += home_bump_mm(Z_AXIS); zPosition += home_bump_mm(Z_AXIS);
...@@ -1523,6 +1558,10 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio ...@@ -1523,6 +1558,10 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio
} }
static void stow_z_probe(bool doRaise = true) { static void stow_z_probe(bool doRaise = true) {
#if !(HAS(SERVO_ENDSTOPS) && (Z_RAISE_AFTER_PROBING > 0))
UNUSED(doRaise);
#endif
if (DEBUGGING(INFO)) if (DEBUGGING(INFO))
DEBUG_POS("stow_z_probe", current_position); DEBUG_POS("stow_z_probe", current_position);
...@@ -1534,11 +1573,7 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio ...@@ -1534,11 +1573,7 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio
#if Z_RAISE_AFTER_PROBING > 0 #if Z_RAISE_AFTER_PROBING > 0
if (doRaise) { if (doRaise) {
if (DEBUGGING(INFO)) { raise_z_after_probing(); // this also updates current_position
ECHO_LMV(INFO, "Raise Z (after) by ", (float)Z_RAISE_AFTER_PROBING);
ECHO_LMV(INFO, "> SERVO_ENDSTOPS > do_blocking_move_to_z ", current_position[Z_AXIS] + Z_RAISE_AFTER_PROBING);
}
do_blocking_move_to_z(current_position[Z_AXIS] + Z_RAISE_AFTER_PROBING); // this also updates current_position
st_synchronize(); st_synchronize();
} }
#endif #endif
...@@ -1562,7 +1597,7 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio ...@@ -1562,7 +1597,7 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio
static float probe_pt(float x, float y, float z_before, ProbeAction probe_action = ProbeDeployAndStow, int verbose_level = 1) { static float probe_pt(float x, float y, float z_before, ProbeAction probe_action = ProbeDeployAndStow, int verbose_level = 1) {
if (DEBUGGING(INFO)) { if (DEBUGGING(INFO)) {
ECHO_LM(INFO, "probe_pt >>>"); ECHO_LM(INFO, "probe_pt >>>");
ECHO_LMV(INFO, "> ProbeAction:", (unsigned long)probe_action); ECHO_LMV(INFO, "> ProbeAction:", probe_action);
DEBUG_POS("", current_position); DEBUG_POS("", current_position);
ECHO_SMV(INFO, "Z Raise to z_before ", z_before); ECHO_SMV(INFO, "Z Raise to z_before ", z_before);
ECHO_EMV(" > do_blocking_move_to_z ", z_before); ECHO_EMV(" > do_blocking_move_to_z ", z_before);
...@@ -1573,10 +1608,10 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio ...@@ -1573,10 +1608,10 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio
if (DEBUGGING(INFO)) { if (DEBUGGING(INFO)) {
ECHO_SMV(INFO, " > do_blocking_move_to_xy ", x - (X_PROBE_OFFSET_FROM_EXTRUDER)); ECHO_SMV(INFO, " > do_blocking_move_to_xy ", x - (X_PROBE_OFFSET_FROM_EXTRUDER));
ECHO_EMV(", ", y - Y_PROBE_OFFSET_FROM_EXTRUDER); ECHO_EMV(", ", y - (Y_PROBE_OFFSET_FROM_EXTRUDER));
} }
do_blocking_move_to_xy(x - X_PROBE_OFFSET_FROM_EXTRUDER, y - (Y_PROBE_OFFSET_FROM_EXTRUDER)); // this also updates current_position do_blocking_move_to_xy(x - (X_PROBE_OFFSET_FROM_EXTRUDER), y - (Y_PROBE_OFFSET_FROM_EXTRUDER)); // this also updates current_position
#if HASNT(Z_PROBE_SLED) #if HASNT(Z_PROBE_SLED)
if (probe_action & ProbeDeploy) { if (probe_action & ProbeDeploy) {
...@@ -1647,28 +1682,40 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio ...@@ -1647,28 +1682,40 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio
sync_plan_position(); sync_plan_position();
#if HAS(Z_PROBE_SLED) #if HAS(Z_PROBE_SLED)
// Get Probe #define _Z_SERVO_TEST (axis != Z_AXIS) // already deployed Z
if (axis == Z_AXIS) { #define _Z_SERVO_SUBTEST false // Z will never be invoked
if (axis_home_dir < 0) dock_sled(false); #define _Z_DEPLOY (dock_sled(false))
} #define _Z_STOW (dock_sled(true))
#elif SERVO_LEVELING
#define _Z_SERVO_TEST (axis != Z_AXIS) // already deployed Z
#define _Z_SERVO_SUBTEST false // Z will never be invoked
#define _Z_DEPLOY (deploy_z_probe())
#define _Z_STOW (stow_z_probe())
#elif HAS(SERVO_ENDSTOPS)
#define _Z_SERVO_TEST true // Z not deployed yet
#define _Z_SERVO_SUBTEST (axis == Z_AXIS) // Z is a probe
#endif #endif
#if HAS(SERVO_ENDSTOPS) && HASNT(Z_PROBE_SLED) // If there's a Z probe that needs deployment...
// Deploy a probe if there is one, and homing towards the bed #if HAS(Z_PROBE_SLED) || SERVO_LEVELING
if (axis == Z_AXIS) { // ...and homing Z towards the bed? Deploy it.
if (axis_home_dir < 0) deploy_z_probe(); if (axis == Z_AXIS && axis_home_dir < 0) {
if (DEBUGGING(INFO)) ECHO_LM(INFO, "> SERVO_LEVELING > " STRINGIFY(_Z_DEPLOY));
_Z_DEPLOY;
} }
#endif #endif
#if HAS(SERVO_ENDSTOPS) #if HAS(SERVO_ENDSTOPS)
// Engage Servo endstop if enabled // Engage an X, Y (or Z) Servo endstop if enabled
if (axis != Z_AXIS && servo_endstop_id[axis] >= 0) if (_Z_SERVO_TEST && servo_endstop_id[axis] >= 0) {
servo[servo_endstop_id[axis]].move(servo_endstop_angle[axis][0]); servo[servo_endstop_id[axis]].move(servo_endstop_angle[axis][0]);
if (_Z_SERVO_SUBTEST) endstops.z_probe_enabled = true;
}
#endif #endif
// Set a flag for Z motor locking // Set a flag for Z motor locking
#if ENABLED(Z_DUAL_ENDSTOPS) #if ENABLED(Z_DUAL_ENDSTOPS)
if (axis == Z_AXIS) In_Homing_Process(true); if (axis == Z_AXIS) set_homing_flag(true);
#endif #endif
// Move towards the endstop until an endstop is triggered // Move towards the endstop until an endstop is triggered
...@@ -1681,6 +1728,7 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio ...@@ -1681,6 +1728,7 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio
current_position[axis] = 0; current_position[axis] = 0;
sync_plan_position(); sync_plan_position();
if (DEBUGGING(INFO)) ECHO_LM(INFO, "> endstops.enable(false)");
endstops.enable(false); // Disable endstops while moving away endstops.enable(false); // Disable endstops while moving away
// Move away from the endstop by the axis HOME_BUMP_MM // Move away from the endstop by the axis HOME_BUMP_MM
...@@ -1688,6 +1736,7 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio ...@@ -1688,6 +1736,7 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio
line_to_destination(); line_to_destination();
st_synchronize(); st_synchronize();
if (DEBUGGING(INFO)) ECHO_LM(INFO, "> endstops.enable(true)");
endstops.enable(); // Enable endstops for next homing move endstops.enable(); // Enable endstops for next homing move
// Slow down the feedrate for the next move // Slow down the feedrate for the next move
...@@ -1698,8 +1747,7 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio ...@@ -1698,8 +1747,7 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio
line_to_destination(); line_to_destination();
st_synchronize(); st_synchronize();
if (DEBUGGING(INFO)) if (DEBUGGING(INFO)) DEBUG_POS(" > TRIGGER ENDSTOP", current_position);
DEBUG_POS(" > TRIGGER ENDSTOP", current_position);
#if ENABLED(Z_DUAL_ENDSTOPS) #if ENABLED(Z_DUAL_ENDSTOPS)
if (axis == Z_AXIS) { if (axis == Z_AXIS) {
...@@ -1712,7 +1760,7 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio ...@@ -1712,7 +1760,7 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio
else else
lockZ1 = (z_endstop_adj < 0); lockZ1 = (z_endstop_adj < 0);
if (lockZ1) Lock_z_motor(true); else Lock_z2_motor(true); if (lockZ1) set_z_lock(true); else set_z2_lock(true);
sync_plan_position(); sync_plan_position();
// Move to the adjusted endstop height // Move to the adjusted endstop height
...@@ -1721,8 +1769,8 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio ...@@ -1721,8 +1769,8 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio
line_to_destination(); line_to_destination();
st_synchronize(); st_synchronize();
if (lockZ1) Lock_z_motor(false); else Lock_z2_motor(false); if (lockZ1) set_z_lock(false); else set_z2_lock(false);
In_Homing_Process(false); set_homing_flag(false);
} // Z_AXIS } // Z_AXIS
#endif #endif
...@@ -1730,8 +1778,7 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio ...@@ -1730,8 +1778,7 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio
set_axis_is_at_home(axis); set_axis_is_at_home(axis);
sync_plan_position(); sync_plan_position();
if (DEBUGGING(INFO)) if (DEBUGGING(INFO)) DEBUG_POS("> AFTER set_axis_is_at_home", current_position);
DEBUG_POS(" > AFTER set_axis_is_at_home", current_position);
destination[axis] = current_position[axis]; destination[axis] = current_position[axis];
feedrate = 0.0; feedrate = 0.0;
...@@ -1739,32 +1786,35 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio ...@@ -1739,32 +1786,35 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio
axis_known_position[axis] = true; axis_known_position[axis] = true;
axis_homed[axis] = true; axis_homed[axis] = true;
#if ENABLED(Z_PROBE_SLED) // Put away the Z probe with a function
// bring probe back #if HAS(Z_PROBE_SLED) || SERVO_LEVELING
if (axis == Z_AXIS) { if (axis == Z_AXIS && axis_home_dir < 0) {
if (axis_home_dir < 0) dock_sled(true); if (DEBUGGING(INFO)) ECHO_LM(INFO, "> SERVO_LEVELING > " STRINGIFY(_Z_STOW));
_Z_STOW;
} }
#endif #endif
#if HAS(SERVO_ENDSTOPS) && HASNT(Z_PROBE_SLED) #if HAS(SERVO_ENDSTOPS)
// Deploy a probe if there is one, and homing towards the bed if (_Z_SERVO_TEST && servo_endstop_id[axis] >= 0) {
// Raise the servo probe before stow outside ABL context.
// This is a workaround to allow use of a Servo Probe without
// ABL until more global probe handling is implemented.
#if Z_RAISE_AFTER_PROBING > 0
if (axis == Z_AXIS) { if (axis == Z_AXIS) {
if (axis_home_dir < 0) { if (DEBUGGING(INFO)) ECHO_LMV(INFO, "Raise Z (after) by ", Z_RAISE_AFTER_PROBING);
if (DEBUGGING(INFO)) ECHO_LM(INFO, "> SERVO_LEVELING > stow_z_probe"); current_position[Z_AXIS] = Z_RAISE_AFTER_PROBING;
stow_z_probe(); feedrate = homing_feedrate[Z_AXIS];
} line_to_current_position();
st_synchronize();
} }
else
#endif #endif
{
#if HAS(SERVO_ENDSTOPS)
// Retract Servo endstop if enabled
if (servo_endstop_id[axis] >= 0) {
if (DEBUGGING(INFO)) ECHO_LM(INFO, "> SERVO_ENDSTOPS > Stow with servo.move()"); if (DEBUGGING(INFO)) ECHO_LM(INFO, "> SERVO_ENDSTOPS > Stow with servo.move()");
servo[servo_endstop_id[axis]].move(servo_endstop_angle[axis][1]); servo[servo_endstop_id[axis]].move(servo_endstop_angle[axis][1]);
if (_Z_SERVO_SUBTEST) endstops.enable_z_probe(false);
} }
#endif #endif // HAS(SERVO_ENDSTOPS)
}
} }
#if ENABLED(LASERBEAM) && (LASER_HAS_FOCUS == false) #if ENABLED(LASERBEAM) && (LASER_HAS_FOCUS == false)
...@@ -2011,7 +2061,7 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio ...@@ -2011,7 +2061,7 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio
endstops.enable(false); endstops.enable(false);
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) / planner.axis_steps_per_unit[Z_AXIS];
current_position[Z_AXIS] = mm; current_position[Z_AXIS] = mm;
sync_plan_position_delta(); sync_plan_position_delta();
} }
...@@ -2073,7 +2123,7 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio ...@@ -2073,7 +2123,7 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio
endstop_adj[Z_AXIS] += z_endstop; endstop_adj[Z_AXIS] += z_endstop;
calculate_delta(current_position); calculate_delta(current_position);
plan_set_position(delta[TOWER_1] - (endstop_adj[X_AXIS] - saved_endstop_adj[X_AXIS]) , delta[TOWER_2] - (endstop_adj[Y_AXIS] - saved_endstop_adj[Y_AXIS]), delta[TOWER_3] - (endstop_adj[Z_AXIS] - saved_endstop_adj[Z_AXIS]), current_position[E_AXIS]); planner.set_position_mm(delta[TOWER_1] - (endstop_adj[X_AXIS] - saved_endstop_adj[X_AXIS]) , delta[TOWER_2] - (endstop_adj[Y_AXIS] - saved_endstop_adj[Y_AXIS]), delta[TOWER_3] - (endstop_adj[Z_AXIS] - saved_endstop_adj[Z_AXIS]), current_position[E_AXIS]);
st_synchronize(); st_synchronize();
} }
...@@ -2678,7 +2728,7 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio ...@@ -2678,7 +2728,7 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio
refresh_cmd_timeout(); refresh_cmd_timeout();
calculate_delta(destination); calculate_delta(destination);
plan_buffer_line(delta[TOWER_1], delta[TOWER_2], delta[TOWER_3], destination[E_AXIS], feedrate * feedrate_multiplier / 60 / 100.0, active_extruder, active_driver); planner.buffer_line(delta[TOWER_1], delta[TOWER_2], delta[TOWER_3], destination[E_AXIS], feedrate * feedrate_multiplier / 60 / 100.0, active_extruder, active_driver);
set_current_to_destination(); set_current_to_destination();
} }
...@@ -2776,7 +2826,7 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio ...@@ -2776,7 +2826,7 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio
set_destination_to_current(); set_destination_to_current();
current_position[E_AXIS] += IDLE_OOZING_LENGTH / volumetric_multiplier[active_extruder]; current_position[E_AXIS] += IDLE_OOZING_LENGTH / volumetric_multiplier[active_extruder];
feedrate = IDLE_OOZING_FEEDRATE * 60; feedrate = IDLE_OOZING_FEEDRATE * 60;
plan_set_e_position(current_position[E_AXIS]); planner.set_e_position_mm(current_position[E_AXIS]);
prepare_move(); prepare_move();
feedrate = oldFeedrate; feedrate = oldFeedrate;
IDLE_OOZING_retracted[active_extruder] = true; IDLE_OOZING_retracted[active_extruder] = true;
...@@ -2787,7 +2837,7 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio ...@@ -2787,7 +2837,7 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio
set_destination_to_current(); set_destination_to_current();
current_position[E_AXIS] -= (IDLE_OOZING_LENGTH+IDLE_OOZING_RECOVER_LENGTH) / volumetric_multiplier[active_extruder]; current_position[E_AXIS] -= (IDLE_OOZING_LENGTH+IDLE_OOZING_RECOVER_LENGTH) / volumetric_multiplier[active_extruder];
feedrate = IDLE_OOZING_RECOVER_FEEDRATE * 60; feedrate = IDLE_OOZING_RECOVER_FEEDRATE * 60;
plan_set_e_position(current_position[E_AXIS]); planner.set_e_position_mm(current_position[E_AXIS]);
prepare_move(); prepare_move();
feedrate = oldFeedrate; feedrate = oldFeedrate;
IDLE_OOZING_retracted[active_extruder] = false; IDLE_OOZING_retracted[active_extruder] = false;
...@@ -2834,7 +2884,7 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio ...@@ -2834,7 +2884,7 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio
feedrate = retract_recover_feedrate * 60; feedrate = retract_recover_feedrate * 60;
float move_e = swapping ? retract_length_swap + retract_recover_length_swap : retract_length + retract_recover_length; float move_e = swapping ? retract_length_swap + retract_recover_length_swap : retract_length + retract_recover_length;
current_position[E_AXIS] -= move_e / volumetric_multiplier[active_extruder]; current_position[E_AXIS] -= move_e / volumetric_multiplier[active_extruder];
plan_set_e_position(current_position[E_AXIS]); planner.set_e_position_mm(current_position[E_AXIS]);
prepare_move(); prepare_move();
} }
...@@ -2859,16 +2909,17 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio ...@@ -2859,16 +2909,17 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio
static void dock_sled(bool dock, int offset=0) { static void dock_sled(bool dock, int offset=0) {
if (DEBUGGING(INFO)) ECHO_LMV(INFO, "dock_sled", dock); if (DEBUGGING(INFO)) ECHO_LMV(INFO, "dock_sled", dock);
if (!axis_homed[X_AXIS] || !axis_homed[Y_AXIS]) { if (!axis_homed[X_AXIS] || !axis_homed[Y_AXIS] || !axis_homed[Z_AXIS]) {
axis_unhomed_error(); axis_unhomed_error(true);
return; return;
} }
if (endstops.z_probe_enabled == !dock) return; // already docked/undocked?
float oldXpos = current_position[X_AXIS]; // save x position float oldXpos = current_position[X_AXIS]; // save x position
if (dock) { if (dock) {
#if Z_RAISE_AFTER_PROBING > 0 raise_z_after_probing(); // raise Z
do_blocking_move_to_z(current_position[Z_AXIS] + Z_RAISE_AFTER_PROBING); // raise Z // Dock sled a bit closer to ensure proper capturing
#endif
do_blocking_move_to_x(X_MAX_POS + SLED_DOCKING_OFFSET + offset - 1); // Dock sled a bit closer to ensure proper capturing do_blocking_move_to_x(X_MAX_POS + SLED_DOCKING_OFFSET + offset - 1); // Dock sled a bit closer to ensure proper capturing
digitalWrite(SLED_PIN, LOW); // turn off magnet digitalWrite(SLED_PIN, LOW); // turn off magnet
} }
...@@ -2879,6 +2930,8 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio ...@@ -2879,6 +2930,8 @@ inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_positio
digitalWrite(SLED_PIN, HIGH); // turn on magnet digitalWrite(SLED_PIN, HIGH); // turn on magnet
} }
do_blocking_move_to_x(oldXpos); // return to position before docking do_blocking_move_to_x(oldXpos); // return to position before docking
endstops.enable_z_probe(!dock); // logically disable docked probe
} }
#endif // Z_PROBE_SLED #endif // Z_PROBE_SLED
...@@ -3382,7 +3435,7 @@ inline void gcode_G0_G1(bool lfire) { ...@@ -3382,7 +3435,7 @@ inline void gcode_G0_G1(bool lfire) {
// Is this move an attempt to retract or recover? // Is this move an attempt to retract or recover?
if ((echange < -MIN_RETRACT && !retracted[active_extruder]) || (echange > MIN_RETRACT && retracted[active_extruder])) { if ((echange < -MIN_RETRACT && !retracted[active_extruder]) || (echange > MIN_RETRACT && retracted[active_extruder])) {
current_position[E_AXIS] = destination[E_AXIS]; // hide the slicer-generated retract/recover from calculations current_position[E_AXIS] = destination[E_AXIS]; // hide the slicer-generated retract/recover from calculations
plan_set_e_position(current_position[E_AXIS]); // AND from the planner planner.set_e_position_mm(current_position[E_AXIS]); // AND from the planner
retract(!retracted[active_extruder]); retract(!retracted[active_extruder]);
return; return;
} }
...@@ -3670,7 +3723,7 @@ inline void gcode_G28() { ...@@ -3670,7 +3723,7 @@ inline void gcode_G28() {
// For auto bed leveling, clear the level matrix // For auto bed leveling, clear the level matrix
#if ENABLED(AUTO_BED_LEVELING_FEATURE) #if ENABLED(AUTO_BED_LEVELING_FEATURE)
plan_bed_level_matrix.set_to_identity(); planner.bed_level_matrix.set_to_identity();
#elif MECH(DELTA) #elif MECH(DELTA)
reset_bed_level(); reset_bed_level();
#endif #endif
...@@ -3680,8 +3733,16 @@ inline void gcode_G28() { ...@@ -3680,8 +3733,16 @@ inline void gcode_G28() {
* on again when homing all axis * on again when homing all axis
*/ */
#if ENABLED(MESH_BED_LEVELING) #if ENABLED(MESH_BED_LEVELING)
uint8_t mbl_was_active = mbl.active; float pre_home_z = MESH_HOME_SEARCH_Z;
mbl.active = false; if (mbl.active()) {
// Save known Z position if already homed
if (axis_homed[X_AXIS] && axis_homed[Y_AXIS] && axis_homed[Z_AXIS]) {
pre_home_z = current_position[Z_AXIS];
pre_home_z += mbl.get_z(current_position[X_AXIS] - home_offset[X_AXIS],
current_position[Y_AXIS] - home_offset[Y_AXIS]);
}
mbl.set_active(false);
}
#endif #endif
setup_for_endstop_move(); setup_for_endstop_move();
...@@ -3712,7 +3773,7 @@ inline void gcode_G28() { ...@@ -3712,7 +3773,7 @@ inline void gcode_G28() {
#if ENABLED(NPR2) #if ENABLED(NPR2)
if((home_all_axis) || (code_seen(axis_codes[E_AXIS]))) { if((home_all_axis) || (code_seen(axis_codes[E_AXIS]))) {
active_driver = active_extruder = 1; active_driver = active_extruder = 1;
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], -200, COLOR_HOMERATE, active_extruder, active_driver); planner.buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], -200, COLOR_HOMERATE, active_extruder, active_driver);
st_synchronize(); st_synchronize();
old_color = 99; old_color = 99;
active_driver = active_extruder = 0; active_driver = active_extruder = 0;
...@@ -3745,7 +3806,7 @@ inline void gcode_G28() { ...@@ -3745,7 +3806,7 @@ inline void gcode_G28() {
// Raise Z before homing any other axes and z is not already high enough (never lower z) // Raise Z before homing any other axes and z is not already high enough (never lower z)
if (current_position[Z_AXIS] <= MIN_Z_HEIGHT_FOR_HOMING) { if (current_position[Z_AXIS] <= MIN_Z_HEIGHT_FOR_HOMING) {
destination[Z_AXIS] = MIN_Z_HEIGHT_FOR_HOMING; destination[Z_AXIS] = MIN_Z_HEIGHT_FOR_HOMING;
feedrate = max_feedrate[Z_AXIS] * 60; // feedrate (mm/m) = max_feedrate (mm/s) feedrate = planner.max_planner.acceleration_units_per_sq_second[Z_AXIS] * 60; // feedrate (mm/m) = planner.max_planner.acceleration_units_per_sq_second (mm/s)
if (DEBUGGING(INFO)) { if (DEBUGGING(INFO)) {
ECHO_EMV("Raise Z (before homing) to ", (MIN_Z_HEIGHT_FOR_HOMING)); ECHO_EMV("Raise Z (before homing) to ", (MIN_Z_HEIGHT_FOR_HOMING));
DEBUG_POS("> (home_all_axis || homeZ)", current_position); DEBUG_POS("> (home_all_axis || homeZ)", current_position);
...@@ -3806,7 +3867,7 @@ inline void gcode_G28() { ...@@ -3806,7 +3867,7 @@ inline void gcode_G28() {
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];
#if !MECH(SCARA) #if NOMECH(SCARA)
current_position[Z_AXIS] = destination[Z_AXIS]; current_position[Z_AXIS] = destination[Z_AXIS];
#endif #endif
...@@ -3881,11 +3942,10 @@ inline void gcode_G28() { ...@@ -3881,11 +3942,10 @@ inline void gcode_G28() {
*/ */
destination[X_AXIS] = round(Z_SAFE_HOMING_X_POINT - (X_PROBE_OFFSET_FROM_EXTRUDER)); destination[X_AXIS] = round(Z_SAFE_HOMING_X_POINT - (X_PROBE_OFFSET_FROM_EXTRUDER));
destination[Y_AXIS] = round(Z_SAFE_HOMING_Y_POINT - (Y_PROBE_OFFSET_FROM_EXTRUDER)); destination[Y_AXIS] = round(Z_SAFE_HOMING_Y_POINT - (Y_PROBE_OFFSET_FROM_EXTRUDER));
destination[Z_AXIS] = -(Z_RAISE_BEFORE_HOMING) * home_dir(Z_AXIS); // Set destination away from bed destination[Z_AXIS] = current_position[Z_AXIS]; //z is already at the right height
feedrate = xy_travel_speed; feedrate = xy_travel_speed;
if (DEBUGGING(INFO)) { if (DEBUGGING(INFO)) {
ECHO_LMV(INFO, "Raise Z (before homing) by ", (float)Z_RAISE_BEFORE_HOMING);
DEBUG_POS("> Z_SAFE_HOMING > home_all_axis", current_position); DEBUG_POS("> Z_SAFE_HOMING > home_all_axis", current_position);
DEBUG_POS("> Z_SAFE_HOMING > home_all_axis", destination); DEBUG_POS("> Z_SAFE_HOMING > home_all_axis", destination);
} }
...@@ -4000,20 +4060,31 @@ inline void gcode_G28() { ...@@ -4000,20 +4060,31 @@ inline void gcode_G28() {
// Enable mesh leveling again // Enable mesh leveling again
#if ENABLED(MESH_BED_LEVELING) #if ENABLED(MESH_BED_LEVELING)
if (mbl_was_active && home_all_axis) { if (mbl.has_mesh()) {
if (home_all_axis || (axis_homed[X_AXIS] && axis_homed[Y_AXIS] && homeZ)) {
current_position[Z_AXIS] = MESH_HOME_SEARCH_Z; current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
sync_plan_position(); sync_plan_position();
mbl.active = 1; mbl.set_active(true);
#if ENABLED(MESH_G28_REST_ORIGIN) #if ENABLED(MESH_G28_REST_ORIGIN)
current_position[Z_AXIS] = 0.0; current_position[Z_AXIS] = 0.0;
set_destination_to_current(); set_destination_to_current();
feedrate = homing_feedrate[Z_AXIS]; feedrate = homing_feedrate[Z_AXIS];
line_to_destination(); line_to_destination();
st_synchronize(); st_synchronize();
#else
current_position[Z_AXIS] = MESH_HOME_SEARCH_Z -
mbl.get_z(current_position[X_AXIS] - home_offset[X_AXIS],
current_position[Y_AXIS] - home_offset[Y_AXIS]);
#endif #endif
#if ENABLED(DEBUG_LEVELING_FEATURE) }
if (DEBUGGING(INFO)) DEBUG_POS("mbl_was_active", current_position); else if ((axis_homed[X_AXIS] && axis_homed[Y_AXIS] && axis_homed[Z_AXIS]) && (homeX || homeY)) {
#endif current_position[Z_AXIS] = pre_home_z;
sync_plan_position();
mbl.set_active(true);
current_position[Z_AXIS] = pre_home_z -
mbl.get_z(current_position[X_AXIS] - home_offset[X_AXIS],
current_position[Y_AXIS] - home_offset[Y_AXIS]);
}
} }
#endif #endif
...@@ -4065,7 +4136,7 @@ inline void gcode_G28() { ...@@ -4065,7 +4136,7 @@ inline void gcode_G28() {
#if ENABLED(MESH_BED_LEVELING) #if ENABLED(MESH_BED_LEVELING)
enum MeshLevelingState { MeshReport, MeshStart, MeshNext, MeshSet, MeshSetZOffset }; enum MeshLevelingState { MeshReport, MeshStart, MeshNext, MeshSet, MeshSetZOffset, MeshReset };
inline void _mbl_goto_xy(float x, float y) { inline void _mbl_goto_xy(float x, float y) {
saved_feedrate = feedrate; saved_feedrate = feedrate;
...@@ -4102,6 +4173,7 @@ inline void gcode_G28() { ...@@ -4102,6 +4173,7 @@ inline void gcode_G28() {
* S2 Probe the next mesh point * S2 Probe the next mesh point
* S3 Xn Yn Zn.nn Manually modify a single point * S3 Xn Yn Zn.nn Manually modify a single point
* S4 Zn.nn Set z offset. Positive away from bed, negative closer to bed. * S4 Zn.nn Set z offset. Positive away from bed, negative closer to bed.
* S5 Reset and disable mesh
* *
* The S0 report the points as below * The S0 report the points as below
* *
...@@ -4115,8 +4187,8 @@ inline void gcode_G28() { ...@@ -4115,8 +4187,8 @@ inline void gcode_G28() {
static int probe_point = -1; static int probe_point = -1;
MeshLevelingState state = code_seen('S') ? (MeshLevelingState)code_value_short() : MeshReport; MeshLevelingState state = code_seen('S') ? (MeshLevelingState)code_value_short() : MeshReport;
if (state < 0 || state > 4) { if (state < 0 || state > 5) {
ECHO_M("S out of range (0-4)."); ECHO_M("S out of range (0-5).");
return; return;
} }
...@@ -4125,12 +4197,20 @@ inline void gcode_G28() { ...@@ -4125,12 +4197,20 @@ inline void gcode_G28() {
switch (state) { switch (state) {
case MeshReport: case MeshReport:
if (mbl.active) { if (mbl.has_mesh()) {
ECHO_MV("Num X,Y: ", MESH_NUM_X_POINTS); ECHO_M("State: ");
if (mbl.active())
ECHO_M("On");
else
ECHO_EM("Off");
ECHO_M("Num X,Y: ");
ECHO_V(MESH_NUM_X_POINTS);
ECHO_C(','); ECHO_C(',');
ECHO_V(MESH_NUM_Y_POINTS); ECHO_V(MESH_NUM_Y_POINTS);
ECHO_MV("\nZ search height: ", MESH_HOME_SEARCH_Z); ECHO_M("\nZ search height: ");
ECHO_MV("\nZ offset: ", mbl.z_offset, 5); ECHO_V(MESH_HOME_SEARCH_Z);
ECHO_M("\nZ offset: ");
ECHO_V(mbl.z_offset, 5);
ECHO_EM("\nMeasured points:"); ECHO_EM("\nMeasured points:");
for (py = 0; py < MESH_NUM_Y_POINTS; py++) { for (py = 0; py < MESH_NUM_Y_POINTS; py++) {
for (px = 0; px < MESH_NUM_X_POINTS; px++) { for (px = 0; px < MESH_NUM_X_POINTS; px++) {
...@@ -4183,7 +4263,7 @@ inline void gcode_G28() { ...@@ -4183,7 +4263,7 @@ inline void gcode_G28() {
// After recording the last point, activate the mbl and home // After recording the last point, activate the mbl and home
ECHO_EM("Mesh probing done."); ECHO_EM("Mesh probing done.");
probe_point = -1; probe_point = -1;
mbl.active = true; mbl.set_has_mesh(true);
enqueue_and_echo_commands_P(PSTR("G28")); enqueue_and_echo_commands_P(PSTR("G28"));
} }
break; break;
...@@ -4230,6 +4310,19 @@ inline void gcode_G28() { ...@@ -4230,6 +4310,19 @@ inline void gcode_G28() {
return; return;
} }
mbl.z_offset = z; mbl.z_offset = z;
break;
case MeshReset:
if (mbl.active()) {
current_position[Z_AXIS] +=
mbl.get_z(current_position[X_AXIS] - home_offset[X_AXIS],
current_position[Y_AXIS] - home_offset[Y_AXIS]) - MESH_HOME_SEARCH_Z;
mbl.reset();
sync_plan_position();
}
else
mbl.reset();
} // switch(state) } // switch(state)
report_current_position(); report_current_position();
...@@ -4358,11 +4451,11 @@ inline void gcode_G28() { ...@@ -4358,11 +4451,11 @@ inline void gcode_G28() {
if (!dryrun) { if (!dryrun) {
// make sure the bed_level_rotation_matrix is identity or the planner will get it wrong // make sure the bed_level_rotation_matrix is identity or the planner will get it wrong
plan_bed_level_matrix.set_to_identity(); planner.bed_level_matrix.set_to_identity();
// vector_3 corrected_position = plan_get_position_mm(); // vector_3 corrected_position = planner.get_position_mm();
// corrected_position.debug("position before G29"); // corrected_position.debug("position before G29");
vector_3 uncorrected_position = plan_adjusted_position(); vector_3 uncorrected_position = planner.adjusted_position();
// uncorrected_position.debug("position during G29"); // uncorrected_position.debug("position during G29");
current_position[X_AXIS] = uncorrected_position.x; current_position[X_AXIS] = uncorrected_position.x;
current_position[Y_AXIS] = uncorrected_position.y; current_position[Y_AXIS] = uncorrected_position.y;
...@@ -4498,7 +4591,7 @@ inline void gcode_G28() { ...@@ -4498,7 +4591,7 @@ inline void gcode_G28() {
y_tmp = eqnAMatrix[ind + 1 * abl2], y_tmp = eqnAMatrix[ind + 1 * abl2],
z_tmp = 0; z_tmp = 0;
apply_rotation_xyz(plan_bed_level_matrix, x_tmp, y_tmp, z_tmp); apply_rotation_xyz(planner.bed_level_matrix, x_tmp, y_tmp, z_tmp);
NOMORE(min_diff, eqnBVector[ind] - z_tmp); NOMORE(min_diff, eqnBVector[ind] - z_tmp);
...@@ -4522,7 +4615,7 @@ inline void gcode_G28() { ...@@ -4522,7 +4615,7 @@ inline void gcode_G28() {
y_tmp = eqnAMatrix[ind + 1 * abl2], y_tmp = eqnAMatrix[ind + 1 * abl2],
z_tmp = 0; z_tmp = 0;
apply_rotation_xyz(plan_bed_level_matrix, x_tmp, y_tmp, z_tmp); apply_rotation_xyz(planner.bed_level_matrix, x_tmp, y_tmp, z_tmp);
float diff = eqnBVector[ind] - min_diff; float diff = eqnBVector[ind] - min_diff;
if (diff >= 0.0) if (diff >= 0.0)
...@@ -4558,7 +4651,7 @@ inline void gcode_G28() { ...@@ -4558,7 +4651,7 @@ inline void gcode_G28() {
#endif // !AUTO_BED_LEVELING_GRID #endif // !AUTO_BED_LEVELING_GRID
if (verbose_level > 0) if (verbose_level > 0)
plan_bed_level_matrix.debug(" Bed Level Correction Matrix:"); planner.bed_level_matrix.debug(" Bed Level Correction Matrix:");
if (!dryrun) { if (!dryrun) {
// Correct the Z height difference from Z probe position and nozzle tip position. // Correct the Z height difference from Z probe position and nozzle tip position.
...@@ -4567,18 +4660,18 @@ inline void gcode_G28() { ...@@ -4567,18 +4660,18 @@ inline void gcode_G28() {
float x_tmp = current_position[X_AXIS] + X_PROBE_OFFSET_FROM_EXTRUDER, float x_tmp = current_position[X_AXIS] + X_PROBE_OFFSET_FROM_EXTRUDER,
y_tmp = current_position[Y_AXIS] + Y_PROBE_OFFSET_FROM_EXTRUDER, y_tmp = current_position[Y_AXIS] + Y_PROBE_OFFSET_FROM_EXTRUDER,
z_tmp = current_position[Z_AXIS], z_tmp = current_position[Z_AXIS],
real_z = st_get_axis_position_mm(Z_AXIS); //get the real Z (since plan_adjusted_position is now correcting the plane) real_z = st_get_axis_position_mm(Z_AXIS); //get the real Z (since planner.adjusted_position is now correcting the plane)
if (DEBUGGING(INFO)) { if (DEBUGGING(INFO)) {
ECHO_LMV(INFO, "> BEFORE apply_rotation_xyz > z_tmp = ", z_tmp); ECHO_LMV(INFO, "> BEFORE apply_rotation_xyz > z_tmp = ", z_tmp);
ECHO_LMV(INFO, "> BEFORE apply_rotation_xyz > real_z = ", real_z); ECHO_LMV(INFO, "> BEFORE apply_rotation_xyz > real_z = ", real_z);
} }
apply_rotation_xyz(plan_bed_level_matrix, x_tmp, y_tmp, z_tmp); // Apply the correction sending the Z probe offset apply_rotation_xyz(planner.bed_level_matrix, x_tmp, y_tmp, z_tmp); // Apply the correction sending the Z probe offset
// Get the current Z position and send it to the planner. // Get the current Z position and send it to the planner.
// //
// >> (z_tmp - real_z) : The rotated current Z minus the uncorrected Z (most recent plan_set_position/sync_plan_position) // >> (z_tmp - real_z) : The rotated current Z minus the uncorrected Z (most recent planner.set_position_mm/sync_plan_position)
// //
// >> zprobe_zoffset : Z distance from nozzle to Z probe (set by default, M851, EEPROM, or Menu) // >> zprobe_zoffset : Z distance from nozzle to Z probe (set by default, M851, EEPROM, or Menu)
// //
...@@ -4992,7 +5085,7 @@ inline void gcode_G92() { ...@@ -4992,7 +5085,7 @@ inline void gcode_G92() {
current_position[i] = v; current_position[i] = v;
if (i == E_AXIS) { if (i == E_AXIS) {
plan_set_e_position(v); planner.set_e_position_mm(v);
} }
else { else {
position_shift[i] += v - p; // Offset the coordinate space position_shift[i] += v - p; // Offset the coordinate space
...@@ -5384,8 +5477,8 @@ inline void gcode_M42() { ...@@ -5384,8 +5477,8 @@ inline void gcode_M42() {
// //
st_synchronize(); st_synchronize();
plan_bed_level_matrix.set_to_identity(); planner.bed_level_matrix.set_to_identity();
plan_buffer_line(X_current, Y_current, Z_start_location, E_current, homing_feedrate[Z_AXIS]/60, active_extruder, active_driver); planner.buffer_line(X_current, Y_current, Z_start_location, E_current, homing_feedrate[Z_AXIS]/60, active_extruder, active_driver);
st_synchronize(); st_synchronize();
// //
...@@ -5396,7 +5489,7 @@ inline void gcode_M42() { ...@@ -5396,7 +5489,7 @@ inline void gcode_M42() {
if (verbose_level > 2) if (verbose_level > 2)
ECHO_LM(DB, "Positioning the probe..."); ECHO_LM(DB, "Positioning the probe...");
plan_buffer_line(X_probe_location, Y_probe_location, Z_start_location, E_current, homing_feedrate[X_AXIS]/60, active_extruder, active_driver); planner.buffer_line(X_probe_location, Y_probe_location, Z_start_location, E_current, homing_feedrate[X_AXIS]/60, active_extruder, active_driver);
st_synchronize(); st_synchronize();
current_position[X_AXIS] = X_current = st_get_axis_position_mm(X_AXIS); current_position[X_AXIS] = X_current = st_get_axis_position_mm(X_AXIS);
...@@ -5417,7 +5510,7 @@ inline void gcode_M42() { ...@@ -5417,7 +5510,7 @@ inline void gcode_M42() {
current_position[Z_AXIS] = Z_current = st_get_axis_position_mm(Z_AXIS); current_position[Z_AXIS] = Z_current = st_get_axis_position_mm(Z_AXIS);
Z_start_location = st_get_axis_position_mm(Z_AXIS) + Z_RAISE_BEFORE_PROBING; Z_start_location = st_get_axis_position_mm(Z_AXIS) + Z_RAISE_BEFORE_PROBING;
plan_buffer_line(X_probe_location, Y_probe_location, Z_start_location, E_current, homing_feedrate[X_AXIS]/60, active_extruder, active_driver); planner.buffer_line(X_probe_location, Y_probe_location, Z_start_location, E_current, homing_feedrate[X_AXIS]/60, active_extruder, active_driver);
st_synchronize(); st_synchronize();
current_position[Z_AXIS] = Z_current = st_get_axis_position_mm(Z_AXIS); current_position[Z_AXIS] = Z_current = st_get_axis_position_mm(Z_AXIS);
...@@ -5500,7 +5593,7 @@ inline void gcode_M42() { ...@@ -5500,7 +5593,7 @@ inline void gcode_M42() {
if (verbose_level > 0) ECHO_E; if (verbose_level > 0) ECHO_E;
plan_buffer_line(X_probe_location, Y_probe_location, Z_start_location, E_current, homing_feedrate[Z_AXIS]/60, active_extruder, active_driver); planner.buffer_line(X_probe_location, Y_probe_location, Z_start_location, E_current, homing_feedrate[Z_AXIS]/60, active_extruder, active_driver);
st_synchronize(); st_synchronize();
if (deploy_probe_for_each_reading) { if (deploy_probe_for_each_reading) {
...@@ -5693,7 +5786,7 @@ inline void gcode_M85() { ...@@ -5693,7 +5786,7 @@ inline void gcode_M85() {
} }
/** /**
* M92: Set axis_steps_per_unit * M92: Set planner.axis_steps_per_unit
*/ */
inline void gcode_M92() { inline void gcode_M92() {
if (get_target_extruder_from_command(92)) return; if (get_target_extruder_from_command(92)) return;
...@@ -5701,14 +5794,14 @@ inline void gcode_M92() { ...@@ -5701,14 +5794,14 @@ inline void gcode_M92() {
for(uint8_t i = 0; i < NUM_AXIS; i++) { for(uint8_t i = 0; i < NUM_AXIS; i++) {
if (code_seen(axis_codes[i])) { if (code_seen(axis_codes[i])) {
if (i == E_AXIS) if (i == E_AXIS)
axis_steps_per_unit[i + target_extruder] = code_value(); planner.axis_steps_per_unit[i + target_extruder] = code_value();
else else
axis_steps_per_unit[i] = code_value(); planner.axis_steps_per_unit[i] = code_value();
} }
} }
st_synchronize(); st_synchronize();
// This recalculates position in steps in case user has changed steps/unit // This recalculates position in steps in case user has changed steps/unit
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]); planner.set_position_mm(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
} }
#if ENABLED(ZWOBBLE) #if ENABLED(ZWOBBLE)
...@@ -6053,10 +6146,7 @@ inline void gcode_M109() { ...@@ -6053,10 +6146,7 @@ inline void gcode_M109() {
} }
#if ENABLED(AUTOTEMP) #if ENABLED(AUTOTEMP)
autotemp_enabled = code_seen('F'); planner.autotemp_M109();
if (autotemp_enabled) autotemp_factor = code_value();
if (code_seen('S')) autotemp_min = code_value();
if (code_seen('B')) autotemp_max = code_value();
#endif #endif
wait_heater(no_wait_for_cooling); wait_heater(no_wait_for_cooling);
...@@ -6452,17 +6542,17 @@ inline void gcode_M200() { ...@@ -6452,17 +6542,17 @@ inline void gcode_M200() {
inline void gcode_M201() { inline void gcode_M201() {
for (uint8_t i = 0; i < NUM_AXIS; i++) { for (uint8_t i = 0; i < NUM_AXIS; i++) {
if (code_seen(axis_codes[i])) { if (code_seen(axis_codes[i])) {
max_acceleration_units_per_sq_second[i] = code_value(); planner.max_acceleration_units_per_sq_second[i] = code_value();
} }
} }
// steps per sq second need to be updated to agree with the units per sq second (as they are what is used in the planner) // steps per sq second need to be updated to agree with the units per sq second (as they are what is used in the planner)
reset_acceleration_rates(); planner.reset_acceleration_rates();
} }
#if 0 // Not used for Sprinter/grbl gen6 #if 0 // Not used for Sprinter/grbl gen6
inline void gcode_M202() { inline void gcode_M202() {
for(uint8_t i = 0; i < NUM_AXIS; i++) { for(uint8_t i = 0; i < NUM_AXIS; i++) {
if(code_seen(axis_codes[i])) axis_travel_steps_per_sqr_second[i] = code_value() * axis_steps_per_unit[i]; if(code_seen(axis_codes[i])) axis_travel_steps_per_sqr_second[i] = code_value() * planner.axis_steps_per_unit[i];
} }
} }
#endif #endif
...@@ -6480,15 +6570,15 @@ inline void gcode_M203() { ...@@ -6480,15 +6570,15 @@ inline void gcode_M203() {
for(uint8_t i = 0; i < NUM_AXIS; i++) { for(uint8_t i = 0; i < NUM_AXIS; i++) {
if (code_seen(axis_codes[i])) { if (code_seen(axis_codes[i])) {
if (i == E_AXIS) if (i == E_AXIS)
max_feedrate[i + target_extruder] = code_value(); planner.max_feedrate[i + target_extruder] = code_value();
else else
max_feedrate[i] = code_value(); planner.max_feedrate[i] = code_value();
} }
} }
} }
/** /**
* M204: Set Accelerations in mm/sec^2 (M204 P1200 T0 R3000 V3000) * M204: Set planner.accelerations in mm/sec^2 (M204 P1200 T0 R3000 V3000)
* *
* P = Printing moves * P = Printing moves
* T* R = Retract only (no X, Y, Z) moves * T* R = Retract only (no X, Y, Z) moves
...@@ -6500,21 +6590,21 @@ inline void gcode_M204() { ...@@ -6500,21 +6590,21 @@ inline void gcode_M204() {
if (get_target_extruder_from_command(204)) return; if (get_target_extruder_from_command(204)) return;
if (code_seen('S')) { // Kept for legacy compatibility. Should NOT BE USED for new developments. if (code_seen('S')) { // Kept for legacy compatibility. Should NOT BE USED for new developments.
acceleration = code_value(); planner.acceleration = code_value();
travel_acceleration = acceleration; planner.travel_acceleration = planner.acceleration;
ECHO_LMV(DB, "Setting Print and Travel Acceleration: ", acceleration ); ECHO_LMV(DB, "Setting Print and Travel acceleration: ", planner.acceleration );
} }
if (code_seen('P')) { if (code_seen('P')) {
acceleration = code_value(); planner.acceleration = code_value();
ECHO_LMV(DB, "Setting Print Acceleration: ", acceleration ); ECHO_LMV(DB, "Setting Print acceleration: ", planner.acceleration );
} }
if (code_seen('R')) { if (code_seen('R')) {
retract_acceleration[target_extruder] = code_value(); planner.retract_acceleration[target_extruder] = code_value();
ECHO_LMV(DB, "Setting Retract Acceleration: ", retract_acceleration[target_extruder]); ECHO_LMV(DB, "Setting Retract acceleration: ", planner.retract_acceleration[target_extruder]);
} }
if (code_seen('V')) { if (code_seen('V')) {
travel_acceleration = code_value(); planner.travel_acceleration = code_value();
ECHO_LMV(DB, "Setting Travel Acceleration: ", travel_acceleration ); ECHO_LMV(DB, "Setting Travel acceleration: ", planner.travel_acceleration );
} }
} }
...@@ -6531,12 +6621,12 @@ inline void gcode_M204() { ...@@ -6531,12 +6621,12 @@ inline void gcode_M204() {
inline void gcode_M205() { inline void gcode_M205() {
if (get_target_extruder_from_command(205)) return; if (get_target_extruder_from_command(205)) return;
if (code_seen('S')) minimumfeedrate = code_value(); if (code_seen('S')) planner.min_feedrate = code_value();
if (code_seen('V')) mintravelfeedrate = code_value(); if (code_seen('V')) planner.min_travel_feedrate = code_value();
if (code_seen('B')) minsegmenttime = code_value(); if (code_seen('B')) planner.min_segment_time = code_value();
if (code_seen('X')) max_xy_jerk = code_value(); if (code_seen('X')) planner.max_xy_jerk = code_value();
if (code_seen('Z')) max_z_jerk = code_value(); if (code_seen('Z')) planner.max_z_jerk = code_value();
if (code_seen('E')) max_e_jerk[target_extruder] = code_value(); if (code_seen('E')) planner.max_e_jerk[target_extruder] = code_value();
} }
/** /**
...@@ -7082,13 +7172,13 @@ inline void gcode_M226() { ...@@ -7082,13 +7172,13 @@ inline void gcode_M226() {
*/ */
inline void gcode_M400() { st_synchronize(); } inline void gcode_M400() { st_synchronize(); }
#if HAS(SERVO_ENDSTOPS) #if (ENABLED(AUTO_BED_LEVELING_FEATURE) && DISABLED(Z_PROBE_SLED) && HAS(SERVO_ENDSTOPS)) || MECH(DELTA)
/** /**
* M401: Engage Z Servo endstop if available * M401: Engage Z Servo endstop if available
*/ */
inline void gcode_M401() { inline void gcode_M401() {
#if ENABLED(AUTO_BED_LEVELING_FEATURE) && HASNT(Z_PROBE_SLED) #if ENABLED(HAS_SERVO_ENDSTOPS) && NOMECH(DELTA)
raise_z_for_servo(); raise_z_for_servo();
#endif #endif
deploy_z_probe(); deploy_z_probe();
...@@ -7098,7 +7188,7 @@ inline void gcode_M400() { st_synchronize(); } ...@@ -7098,7 +7188,7 @@ inline void gcode_M400() { st_synchronize(); }
* M402: Retract Z Servo endstop if enabled * M402: Retract Z Servo endstop if enabled
*/ */
inline void gcode_M402() { inline void gcode_M402() {
#if ENABLED(AUTO_BED_LEVELING_FEATURE) && HASNT(Z_PROBE_SLED) #if ENABLED(HAS_SERVO_ENDSTOPS) && NOMECH(DELTA)
raise_z_for_servo(); raise_z_for_servo();
#endif #endif
#if MECH(DELTA) #if MECH(DELTA)
...@@ -7108,7 +7198,7 @@ inline void gcode_M400() { st_synchronize(); } ...@@ -7108,7 +7198,7 @@ inline void gcode_M400() { st_synchronize(); }
#endif #endif
} }
#endif // HAS(SERVO_ENDSTOPS) #endif // (ENABLED(AUTO_BED_LEVELING_FEATURE) && DISABLED(Z_PROBE_SLED) && HAS(SERVO_ENDSTOPS)) || MECH(DELTA)
#if ENABLED(FILAMENT_SENSOR) #if ENABLED(FILAMENT_SENSOR)
...@@ -7346,15 +7436,15 @@ inline void gcode_M400() { st_synchronize(); } ...@@ -7346,15 +7436,15 @@ inline void gcode_M400() { st_synchronize(); }
ECHO_V((int) Y_MAX_POS); ECHO_V((int) Y_MAX_POS);
ECHO_M(","); ECHO_M(",");
ECHO_V((int) Z_MAX_POS); ECHO_V((int) Z_MAX_POS);
ECHO_M("],\"accelerations\":["); ECHO_M("],\"planner.accelerations\":[");
ECHO_V(max_acceleration_units_per_sq_second[X_AXIS]); ECHO_V(planner.max_planner.acceleration_units_per_sq_second[X_AXIS]);
ECHO_M(","); ECHO_M(",");
ECHO_V(max_acceleration_units_per_sq_second[Y_AXIS]); ECHO_V(planner.max_planner.acceleration_units_per_sq_second[Y_AXIS]);
ECHO_M(","); ECHO_M(",");
ECHO_V(max_acceleration_units_per_sq_second[Z_AXIS]); ECHO_V(planner.max_planner.acceleration_units_per_sq_second[Z_AXIS]);
for (uint8_t i = 0; i < EXTRUDERS; i++) { for (uint8_t i = 0; i < EXTRUDERS; i++) {
ECHO_M(","); ECHO_M(",");
ECHO_V(max_acceleration_units_per_sq_second[E_AXIS + i]); ECHO_V(planner.max_planner.acceleration_units_per_sq_second[E_AXIS + i]);
} }
ECHO_M("],"); ECHO_M("],");
...@@ -7396,14 +7486,14 @@ inline void gcode_M400() { st_synchronize(); } ...@@ -7396,14 +7486,14 @@ inline void gcode_M400() { st_synchronize(); }
ECHO_M(",0"); ECHO_M(",0");
} }
ECHO_M("],\"maxFeedrates\":["); ECHO_M("],\"maxFeedrates\":[");
ECHO_V(max_feedrate[X_AXIS]); ECHO_V(planner.max_planner.acceleration_units_per_sq_second[X_AXIS]);
ECHO_M(","); ECHO_M(",");
ECHO_V(max_feedrate[Y_AXIS]); ECHO_V(planner.max_planner.acceleration_units_per_sq_second[Y_AXIS]);
ECHO_M(","); ECHO_M(",");
ECHO_V(max_feedrate[Z_AXIS]); ECHO_V(planner.max_planner.acceleration_units_per_sq_second[Z_AXIS]);
for (uint8_t i = 0; i < EXTRUDERS; i++) { for (uint8_t i = 0; i < EXTRUDERS; i++) {
ECHO_M(","); ECHO_M(",");
ECHO_V(max_feedrate[E_AXIS + i]); ECHO_V(planner.max_planner.acceleration_units_per_sq_second[E_AXIS + i]);
} }
ECHO_M("]"); ECHO_M("]");
break; break;
...@@ -7418,13 +7508,18 @@ inline void gcode_M400() { st_synchronize(); } ...@@ -7418,13 +7508,18 @@ inline void gcode_M400() { st_synchronize(); }
* This will stop the carriages mid-move, so most likely they * This will stop the carriages mid-move, so most likely they
* will be out of sync with the stepper position after this. * will be out of sync with the stepper position after this.
*/ */
inline void gcode_M410() { quickStop(); } inline void gcode_M410() {
quickStop();
#if NOMECH(DELTA) && NOMECH(SCARA)
set_current_position_from_planner();
#endif
}
#if ENABLED(MESH_BED_LEVELING) #if ENABLED(MESH_BED_LEVELING)
/** /**
* M420: Enable/Disable Mesh Bed Leveling * M420: Enable/Disable Mesh Bed Leveling
*/ */
inline void gcode_M420() { if (code_seen('S') && code_has_value()) mbl.active = !!code_value_short(); } inline void gcode_M420() { if (code_seen('S') && code_has_value()) mbl.set_has_mesh(!!code_value_short()); }
/** /**
* M421: Set a single Mesh Bed Leveling Z coordinate * M421: Set a single Mesh Bed Leveling Z coordinate
...@@ -7625,7 +7720,7 @@ inline void gcode_M503() { ...@@ -7625,7 +7720,7 @@ inline void gcode_M503() {
*/ */
inline void gcode_M600() { inline void gcode_M600() {
if (degHotend(active_extruder) < extrude_min_temp) { if (tooColdToExtrude(active_extruder)) {
ECHO_LM(ER, MSG_TOO_COLD_FOR_FILAMENTCHANGE); ECHO_LM(ER, MSG_TOO_COLD_FOR_FILAMENTCHANGE);
return; return;
} }
...@@ -7636,25 +7731,28 @@ inline void gcode_M503() { ...@@ -7636,25 +7731,28 @@ inline void gcode_M503() {
float lastpos[NUM_AXIS]; float lastpos[NUM_AXIS];
#if MECH(DELTA)
float fr60 = feedrate / 60;
#endif
// Save current position of all axes // Save current position of all axes
for (int i = 0; i < NUM_AXIS; i++) for (int i = 0; i < NUM_AXIS; i++)
lastpos[i] = destination[i] = current_position[i]; lastpos[i] = destination[i] = current_position[i];
#if MECH(DELTA) #if MECH(DELTA)
#define RUNPLAN calculate_delta(destination); \ #define RUNPLAN calculate_delta(destination); \
plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], FILAMENT_CHANGE_XY_FEEDRATE * 60, active_extruder, active_driver); planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], fr60, active_extruder, active_driver);
#else #else
#define RUNPLAN line_to_destination(FILAMENT_CHANGE_XY_FEEDRATE * 60); #define RUNPLAN line_to_destination(FILAMENT_CHANGE_XY_FEEDRATE * 60);
#endif #endif
KEEPALIVE_STATE(IN_HANDLER); // retract by E
// Initial retract before move to filament change position
if (code_seen('E')) destination[E_AXIS] += code_value(); if (code_seen('E')) destination[E_AXIS] += code_value();
#if ENABLED(FILAMENT_CHANGE_RETRACT_LENGTH) #if ENABLED(FILAMENT_CHANGE_RETRACT_LENGTH)
else destination[E_AXIS] += FILAMENT_CHANGE_RETRACT_LENGTH; else destination[E_AXIS] += FILAMENT_CHANGE_RETRACT_LENGTH;
#endif #endif
line_to_destination(FILAMENT_CHANGE_RETRACT_FEEDRATE * 60);
RUNPLAN;
// Lift Z axis // Lift Z axis
if (code_seen('Z')) destination[Z_AXIS] += code_value(); if (code_seen('Z')) destination[Z_AXIS] += code_value();
...@@ -7689,7 +7787,8 @@ inline void gcode_M503() { ...@@ -7689,7 +7787,8 @@ inline void gcode_M503() {
#if ENABLED(FILAMENT_CHANGE_UNLOAD_LENGTH) #if ENABLED(FILAMENT_CHANGE_UNLOAD_LENGTH)
else destination[E_AXIS] += FILAMENT_CHANGE_UNLOAD_LENGTH; else destination[E_AXIS] += FILAMENT_CHANGE_UNLOAD_LENGTH;
#endif #endif
line_to_destination(FILAMENT_CHANGE_UNLOAD_FEEDRATE * 60);
RUNPLAN;
// Synchronize steppers and then disable extruders steppers for manual filament changing // Synchronize steppers and then disable extruders steppers for manual filament changing
st_synchronize(); st_synchronize();
...@@ -7781,13 +7880,13 @@ inline void gcode_M503() { ...@@ -7781,13 +7880,13 @@ inline void gcode_M503() {
// Set extruder to saved position // Set extruder to saved position
current_position[E_AXIS] = lastpos[E_AXIS]; current_position[E_AXIS] = lastpos[E_AXIS];
destination[E_AXIS] = lastpos[E_AXIS]; destination[E_AXIS] = lastpos[E_AXIS];
plan_set_e_position(current_position[E_AXIS]); planner.set_e_position_mm(current_position[E_AXIS]);
#if MECH(DELTA) #if MECH(DELTA)
// Move XYZ to starting position, then E // Move XYZ to starting position, then E
calculate_delta(lastpos); calculate_delta(lastpos);
plan_buffer_line(delta[TOWER_1], delta[TOWER_2], delta[TOWER_3], destination[E_AXIS], FILAMENT_CHANGE_XY_FEEDRATE * 60, active_extruder, active_driver); planner.buffer_line(delta[TOWER_1], delta[TOWER_2], delta[TOWER_3], destination[E_AXIS], FILAMENT_CHANGE_XY_FEEDRATE * 60, active_extruder, active_driver);
plan_buffer_line(delta[TOWER_1], delta[TOWER_2], delta[TOWER_3], lastpos[E_AXIS], FILAMENT_CHANGE_XY_FEEDRATE * 60, active_extruder, active_driver); planner.buffer_line(delta[TOWER_1], delta[TOWER_2], delta[TOWER_3], lastpos[E_AXIS], FILAMENT_CHANGE_XY_FEEDRATE * 60, active_extruder, active_driver);
#else #else
// Move XY to starting position, then Z, then E // Move XY to starting position, then Z, then E
destination[X_AXIS] = lastpos[X_AXIS]; destination[X_AXIS] = lastpos[X_AXIS];
...@@ -8113,7 +8212,7 @@ inline void gcode_T(uint8_t tmp_extruder) { ...@@ -8113,7 +8212,7 @@ inline void gcode_T(uint8_t tmp_extruder) {
#if ENABLED(XY_TRAVEL_SPEED) #if ENABLED(XY_TRAVEL_SPEED)
feedrate = XY_TRAVEL_SPEED; feedrate = XY_TRAVEL_SPEED;
#else #else
feedrate = min(max_feedrate[X_AXIS], max_feedrate[Y_AXIS]); feedrate = min(planner.max_feedrate[X_AXIS], planner.max_feedrate[Y_AXIS]);
#endif #endif
} }
...@@ -8131,12 +8230,12 @@ inline void gcode_T(uint8_t tmp_extruder) { ...@@ -8131,12 +8230,12 @@ inline void gcode_T(uint8_t tmp_extruder) {
if (dual_x_carriage_mode == DXC_AUTO_PARK_MODE && IsRunning() && if (dual_x_carriage_mode == DXC_AUTO_PARK_MODE && IsRunning() &&
(delayed_move_time != 0 || current_position[X_AXIS] != x_home_pos(active_extruder))) { (delayed_move_time != 0 || current_position[X_AXIS] != x_home_pos(active_extruder))) {
// Park old head: 1) raise 2) move to park position 3) lower // Park old head: 1) raise 2) move to park position 3) lower
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] + TOOLCHANGE_PARK_ZLIFT, planner.buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] + TOOLCHANGE_PARK_ZLIFT,
current_position[E_AXIS], max_feedrate[Z_AXIS], active_extruder, active_driver); current_position[E_AXIS], planner.max_feedrate[Z_AXIS], active_extruder, active_driver);
plan_buffer_line(x_home_pos(active_extruder), current_position[Y_AXIS], current_position[Z_AXIS] + TOOLCHANGE_PARK_ZLIFT, planner.buffer_line(x_home_pos(active_extruder), current_position[Y_AXIS], current_position[Z_AXIS] + TOOLCHANGE_PARK_ZLIFT,
current_position[E_AXIS], max_feedrate[X_AXIS], active_extruder, active_driver); current_position[E_AXIS], planner.max_feedrate[X_AXIS], active_extruder, active_driver);
plan_buffer_line(x_home_pos(active_extruder), current_position[Y_AXIS], current_position[Z_AXIS], planner.buffer_line(x_home_pos(active_extruder), current_position[Y_AXIS], current_position[Z_AXIS],
current_position[E_AXIS], max_feedrate[Z_AXIS], active_extruder, active_driver); current_position[E_AXIS], planner.max_feedrate[Z_AXIS], active_extruder, active_driver);
st_synchronize(); st_synchronize();
} }
...@@ -8493,7 +8592,7 @@ void process_next_command() { ...@@ -8493,7 +8592,7 @@ void process_next_command() {
gcode_G0_G1(codenum == 1); break; gcode_G0_G1(codenum == 1); break;
// G2, G3 // G2, G3
#if !MECH(SCARA) #if NOMECH(SCARA)
case 2: // G2 - CW ARC case 2: // G2 - CW ARC
case 3: // G3 - CCW ARC case 3: // G3 - CCW ARC
gcode_G2_G3(codenum == 2); break; gcode_G2_G3(codenum == 2); break;
...@@ -8803,7 +8902,7 @@ void process_next_command() { ...@@ -8803,7 +8902,7 @@ void process_next_command() {
#endif #endif
case 203: // M203 max feedrate mm/sec case 203: // M203 max feedrate mm/sec
gcode_M203(); break; gcode_M203(); break;
case 204: // M204 acceleration S normal moves T filament only moves case 204: // M204 planner.acceleration S normal moves T filament only moves
gcode_M204(); break; gcode_M204(); break;
case 205: //M205 advanced settings: minimum travel speed S=while printing T=travel only, B=minimum segment time X= maximum xy jerk, Z=maximum Z jerk case 205: //M205 advanced settings: minimum travel speed S=while printing T=travel only, B=minimum segment time X= maximum xy jerk, Z=maximum Z jerk
gcode_M205(); break; gcode_M205(); break;
...@@ -8905,7 +9004,7 @@ void process_next_command() { ...@@ -8905,7 +9004,7 @@ void process_next_command() {
case 400: // M400 finish all moves case 400: // M400 finish all moves
gcode_M400(); break; gcode_M400(); break;
#if HAS(SERVO_ENDSTOPS) #if (ENABLED(AUTO_BED_LEVELING_FEATURE) && DISABLED(Z_PROBE_SLED) && HAS(SERVO_ENDSTOPS)) || MECH(DELTA)
case 401: case 401:
gcode_M401(); break; gcode_M401(); break;
case 402: case 402:
...@@ -9049,7 +9148,7 @@ void ok_to_send() { ...@@ -9049,7 +9148,7 @@ void ok_to_send() {
while (NUMERIC_SIGNED(*p)) while (NUMERIC_SIGNED(*p))
ECHO_C(*p++); ECHO_C(*p++);
} }
ECHO_MV(" P", (int)(BLOCK_BUFFER_SIZE - movesplanned() - 1)); ECHO_MV(" P", (int)(BLOCK_BUFFER_SIZE - planner.movesplanned() - 1));
ECHO_MV(" B", BUFSIZE - commands_in_queue); ECHO_MV(" B", BUFSIZE - commands_in_queue);
#endif #endif
ECHO_E; ECHO_E;
...@@ -9092,8 +9191,8 @@ static void report_current_position() { ...@@ -9092,8 +9191,8 @@ static void report_current_position() {
ECHO_SMV(DB, "SCARA Cal - Theta:", delta[X_AXIS] + home_offset[X_AXIS]); ECHO_SMV(DB, "SCARA Cal - Theta:", delta[X_AXIS] + home_offset[X_AXIS]);
ECHO_EMV(" Psi+Theta (90):", delta[Y_AXIS]-delta[X_AXIS] - 90 + home_offset[Y_AXIS]); ECHO_EMV(" Psi+Theta (90):", delta[Y_AXIS]-delta[X_AXIS] - 90 + home_offset[Y_AXIS]);
ECHO_SMV(DB, "SCARA step Cal - Theta:", delta[X_AXIS] / 90 * axis_steps_per_unit[X_AXIS]); ECHO_SMV(DB, "SCARA step Cal - Theta:", delta[X_AXIS] / 90 * planner.axis_steps_per_unit[X_AXIS]);
ECHO_EMV(" Psi+Theta:", (delta[Y_AXIS]-delta[X_AXIS]) / 90 * axis_steps_per_unit[Y_AXIS]); ECHO_EMV(" Psi+Theta:", (delta[Y_AXIS]-delta[X_AXIS]) / 90 * planner.axis_steps_per_unit[Y_AXIS]);
ECHO_E; ECHO_E;
#endif #endif
} }
...@@ -9101,8 +9200,8 @@ static void report_current_position() { ...@@ -9101,8 +9200,8 @@ static void report_current_position() {
#if ENABLED(MESH_BED_LEVELING) #if ENABLED(MESH_BED_LEVELING)
// 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_buffer_line(float x, float y, float z, const float e, float feed_rate, const uint8_t& extruder, const uint8_t& driver, uint8_t x_splits = 0xff, uint8_t y_splits = 0xff) { void mesh_buffer_line(float x, float y, float z, const float e, float feed_rate, const uint8_t& extruder, const uint8_t& driver, uint8_t x_splits = 0xff, uint8_t y_splits = 0xff) {
if (!mbl.active) { if (!mbl.active()) {
plan_buffer_line(x, y, z, e, feed_rate, extruder, driver); planner.buffer_line(x, y, z, e, feed_rate, extruder, driver);
set_current_to_destination(); set_current_to_destination();
return; return;
} }
...@@ -9119,7 +9218,7 @@ static void report_current_position() { ...@@ -9119,7 +9218,7 @@ static void report_current_position() {
if (pcx == cx && pcy == cy) { if (pcx == cx && pcy == cy) {
// Start and end on same mesh square // Start and end on same mesh square
plan_buffer_line(x, y, z, e, feed_rate, extruder, driver); planner.buffer_line(x, y, z, e, feed_rate, extruder, driver);
set_current_to_destination(); set_current_to_destination();
return; return;
} }
...@@ -9160,7 +9259,7 @@ static void report_current_position() { ...@@ -9160,7 +9259,7 @@ static void report_current_position() {
} }
else { else {
// Already split on a border // Already split on a border
plan_buffer_line(x, y, z, e, feed_rate, extruder, driver); planner.buffer_line(x, y, z, e, feed_rate, extruder, driver);
set_current_to_destination(); set_current_to_destination();
return; return;
} }
...@@ -9265,7 +9364,7 @@ static void report_current_position() { ...@@ -9265,7 +9364,7 @@ static void report_current_position() {
DEBUG_POS("prepare_move_delta", delta); DEBUG_POS("prepare_move_delta", delta);
} }
plan_buffer_line(delta[TOWER_1], delta[TOWER_2], delta[TOWER_3], target[E_AXIS], _feedrate, active_extruder, active_driver); planner.buffer_line(delta[TOWER_1], delta[TOWER_2], delta[TOWER_3], target[E_AXIS], _feedrate, active_extruder, active_driver);
} }
return true; return true;
} }
...@@ -9282,8 +9381,8 @@ static void report_current_position() { ...@@ -9282,8 +9381,8 @@ static void report_current_position() {
if (active_extruder_parked) { if (active_extruder_parked) {
if (dual_x_carriage_mode == DXC_DUPLICATION_MODE && active_extruder == 0) { if (dual_x_carriage_mode == DXC_DUPLICATION_MODE && active_extruder == 0) {
// move duplicate extruder into correct duplication position. // move duplicate extruder into correct duplication position.
plan_set_position(inactive_extruder_x_pos, current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]); planner.set_position_mm(inactive_extruder_x_pos, current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
plan_buffer_line(current_position[X_AXIS] + duplicate_hotend_x_offset, current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], max_feedrate[X_AXIS], 1, active_driver); planner.buffer_line(current_position[X_AXIS] + duplicate_hotend_x_offset, current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], planner.max_planner.acceleration_units_per_sq_second[X_AXIS], 1, active_driver);
sync_plan_position(); sync_plan_position();
st_synchronize(); st_synchronize();
extruder_duplication_enabled = true; extruder_duplication_enabled = true;
...@@ -9303,9 +9402,9 @@ static void report_current_position() { ...@@ -9303,9 +9402,9 @@ static void report_current_position() {
} }
delayed_move_time = 0; delayed_move_time = 0;
// unpark extruder: 1) raise, 2) move into starting XY position, 3) lower // unpark extruder: 1) raise, 2) move into starting XY position, 3) lower
plan_buffer_line(raised_parked_position[X_AXIS], raised_parked_position[Y_AXIS], raised_parked_position[Z_AXIS], current_position[E_AXIS], max_feedrate[Z_AXIS], active_extruder, active_driver); planner.buffer_line(raised_parked_position[X_AXIS], raised_parked_position[Y_AXIS], raised_parked_position[Z_AXIS], current_position[E_AXIS], planner.max_planner.acceleration_units_per_sq_second[Z_AXIS], active_extruder, active_driver);
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], raised_parked_position[Z_AXIS], current_position[E_AXIS], min(max_feedrate[X_AXIS], max_feedrate[Y_AXIS]), active_extruder, active_driver); planner.buffer_line(current_position[X_AXIS], current_position[Y_AXIS], raised_parked_position[Z_AXIS], current_position[E_AXIS], min(planner.max_planner.acceleration_units_per_sq_second[X_AXIS], planner.max_planner.acceleration_units_per_sq_second[Y_AXIS]), active_extruder, active_driver);
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], max_feedrate[Z_AXIS], active_extruder, active_driver); planner.buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], planner.max_planner.acceleration_units_per_sq_second[Z_AXIS], active_extruder, active_driver);
active_extruder_parked = false; active_extruder_parked = false;
} }
} }
...@@ -9345,7 +9444,7 @@ static void report_current_position() { ...@@ -9345,7 +9444,7 @@ static void report_current_position() {
/** /**
* Prepare a single move and get ready for the next one * Prepare a single move and get ready for the next one
* *
* (This may call plan_buffer_line several times to put * (This may call planner.buffer_line several times to put
* smaller moves into the planner for DELTA or SCARA.) * smaller moves into the planner for DELTA or SCARA.)
*/ */
void prepare_move() { void prepare_move() {
...@@ -9486,9 +9585,9 @@ void plan_arc( ...@@ -9486,9 +9585,9 @@ void plan_arc(
#if MECH(DELTA) || MECH(SCARA) #if MECH(DELTA) || MECH(SCARA)
calculate_delta(arc_target); calculate_delta(arc_target);
adjust_delta(arc_target); adjust_delta(arc_target);
plan_buffer_line(delta[TOWER_1], delta[TOWER_2], delta[TOWER_3], arc_target[E_AXIS], feed_rate, active_extruder, active_driver); planner.buffer_line(delta[TOWER_1], delta[TOWER_2], delta[TOWER_3], arc_target[E_AXIS], feed_rate, active_extruder, active_driver);
#else #else
plan_buffer_line(arc_target[X_AXIS], arc_target[Y_AXIS], arc_target[Z_AXIS], arc_target[E_AXIS], feed_rate, active_extruder, active_driver); planner.buffer_line(arc_target[X_AXIS], arc_target[Y_AXIS], arc_target[Z_AXIS], arc_target[E_AXIS], feed_rate, active_extruder, active_driver);
#endif #endif
} }
...@@ -9496,9 +9595,9 @@ void plan_arc( ...@@ -9496,9 +9595,9 @@ void plan_arc(
#if MECH(DELTA) || MECH(SCARA) #if MECH(DELTA) || MECH(SCARA)
calculate_delta(target); calculate_delta(target);
adjust_delta(target); adjust_delta(target);
plan_buffer_line(delta[TOWER_1], delta[TOWER_2], delta[TOWER_3], target[E_AXIS], feed_rate, active_extruder, active_driver); planner.buffer_line(delta[TOWER_1], delta[TOWER_2], delta[TOWER_3], target[E_AXIS], feed_rate, active_extruder, active_driver);
#else #else
plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], feed_rate, active_extruder, active_driver); planner.buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], feed_rate, active_extruder, active_driver);
#endif #endif
// As far as the parser is concerned, the position is now == target. In reality the // As far as the parser is concerned, the position is now == target. In reality the
...@@ -9694,7 +9793,7 @@ void manage_inactivity(bool ignore_stepper_queue/*=false*/) { ...@@ -9694,7 +9793,7 @@ void manage_inactivity(bool ignore_stepper_queue/*=false*/) {
#if HAS(FILRUNOUT) #if HAS(FILRUNOUT)
if ((Printing || IS_SD_PRINTING) && (READ(FILRUNOUT_PIN) ^ FILRUNOUT_PIN_INVERTING)) if ((Printing || IS_SD_PRINTING) && (READ(FILRUNOUT_PIN) ^ FILRUNOUT_PIN_INVERTING))
filrunout(); handle_filament_runout();
#endif #endif
if (commands_in_queue < BUFSIZE - 1) get_available_commands(); if (commands_in_queue < BUFSIZE - 1) get_available_commands();
...@@ -9710,7 +9809,7 @@ void manage_inactivity(bool ignore_stepper_queue/*=false*/) { ...@@ -9710,7 +9809,7 @@ void manage_inactivity(bool ignore_stepper_queue/*=false*/) {
} }
#endif #endif
if (stepper_inactive_time && ELAPSED(ms, previous_cmd_ms + stepper_inactive_time) if (stepper_inactive_time && ELAPSED(ms, previous_cmd_ms + stepper_inactive_time)
&& !ignore_stepper_queue && !blocks_queued()) { && !ignore_stepper_queue && !planner.blocks_queued()) {
#if DISABLE_X == true #if DISABLE_X == true
disable_x(); disable_x();
#endif #endif
...@@ -9811,12 +9910,12 @@ void manage_inactivity(bool ignore_stepper_queue/*=false*/) { ...@@ -9811,12 +9910,12 @@ void manage_inactivity(bool ignore_stepper_queue/*=false*/) {
#endif #endif
} }
float oldepos = current_position[E_AXIS], oldedes = destination[E_AXIS]; float oldepos = current_position[E_AXIS], oldedes = destination[E_AXIS];
plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], planner.buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS],
destination[E_AXIS] + (EXTRUDER_RUNOUT_EXTRUDE) * (EXTRUDER_RUNOUT_ESTEPS) / axis_steps_per_unit[E_AXIS], destination[E_AXIS] + (EXTRUDER_RUNOUT_EXTRUDE) * (EXTRUDER_RUNOUT_ESTEPS) / planner.axis_steps_per_unit[E_AXIS],
(EXTRUDER_RUNOUT_SPEED) / 60. * (EXTRUDER_RUNOUT_ESTEPS) / axis_steps_per_unit[E_AXIS], active_extruder, active_driver); (EXTRUDER_RUNOUT_SPEED) / 60. * (EXTRUDER_RUNOUT_ESTEPS) / planner.axis_steps_per_unit[E_AXIS], active_extruder, active_driver);
current_position[E_AXIS] = oldepos; current_position[E_AXIS] = oldepos;
destination[E_AXIS] = oldedes; destination[E_AXIS] = oldedes;
plan_set_e_position(oldepos); planner.set_e_position_mm(oldepos);
previous_cmd_ms = ms; // refresh_cmd_timeout() previous_cmd_ms = ms; // refresh_cmd_timeout()
st_synchronize(); st_synchronize();
switch(active_extruder) { switch(active_extruder) {
...@@ -9854,7 +9953,7 @@ void manage_inactivity(bool ignore_stepper_queue/*=false*/) { ...@@ -9854,7 +9953,7 @@ void manage_inactivity(bool ignore_stepper_queue/*=false*/) {
#endif #endif
#if ENABLED(IDLE_OOZING_PREVENT) #if ENABLED(IDLE_OOZING_PREVENT)
if (blocks_queued()) axis_last_activity = millis(); if (planner.blocks_queued()) axis_last_activity = millis();
if (degHotend(active_extruder) > IDLE_OOZING_MINTEMP && !(DEBUGGING(DRYRUN)) && IDLE_OOZING_enabled) { if (degHotend(active_extruder) > IDLE_OOZING_MINTEMP && !(DEBUGGING(DRYRUN)) && IDLE_OOZING_enabled) {
#if ENABLED(FILAMENTCHANGEENABLE) #if ENABLED(FILAMENTCHANGEENABLE)
if (!filament_changing) if (!filament_changing)
...@@ -9910,7 +10009,7 @@ void manage_inactivity(bool ignore_stepper_queue/*=false*/) { ...@@ -9910,7 +10009,7 @@ void manage_inactivity(bool ignore_stepper_queue/*=false*/) {
handle_status_leds(); handle_status_leds();
#endif #endif
check_axes_activity(); planner.check_axes_activity();
} }
void kill(const char* lcd_msg) { void kill(const char* lcd_msg) {
...@@ -9954,7 +10053,7 @@ void kill(const char* lcd_msg) { ...@@ -9954,7 +10053,7 @@ void kill(const char* lcd_msg) {
} }
#if HAS(FILRUNOUT) #if HAS(FILRUNOUT)
void filrunout() { void handle_filament_runout() {
if (!filament_ran_out) { if (!filament_ran_out) {
filament_ran_out = true; filament_ran_out = true;
enqueue_and_echo_commands_P(PSTR(FILAMENT_RUNOUT_SCRIPT)); enqueue_and_echo_commands_P(PSTR(FILAMENT_RUNOUT_SCRIPT));
......
MK/module/MK_Main.h
View file @ 55a2c790
...@@ -79,8 +79,12 @@ void prepare_move(); ...@@ -79,8 +79,12 @@ void prepare_move();
void kill(const char *); void kill(const char *);
void Stop(); void Stop();
#if !MECH(DELTA) && !MECH(SCARA)
void set_current_position_from_planner();
#endif
#if ENABLED(FILAMENT_RUNOUT_SENSOR) #if ENABLED(FILAMENT_RUNOUT_SENSOR)
void filrunout(); void handle_filament_runout();
#endif #endif
/** /**
......
MK/module/conditionals.h
View file @ 55a2c790
...@@ -444,6 +444,9 @@ ...@@ -444,6 +444,9 @@
#define MIN_PROBE_Y (max(Y_MIN_POS, Y_MIN_POS + Y_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 MAX_PROBE_Y (min(Y_MAX_POS, Y_MAX_POS + Y_PROBE_OFFSET_FROM_EXTRUDER))
#define HAS_Z_ENDSTOP_SERVO (ENABLED(Z_ENDSTOP_SERVO_NR) && Z_ENDSTOP_SERVO_NR >= 0)
#define SERVO_LEVELING (ENABLED(AUTO_BED_LEVELING_FEATURE) && HAS_Z_ENDSTOP_SERVO)
// Make sure probing points are reachable // Make sure probing points are reachable
#if LEFT_PROBE_BED_POSITION < MIN_PROBE_X #if LEFT_PROBE_BED_POSITION < MIN_PROBE_X
#undef LEFT_PROBE_BED_POSITION #undef LEFT_PROBE_BED_POSITION
......
MK/module/digipot/digipot_mcp4451.cpp
View file @ 55a2c790
/**
* MK & MK4due 3D Printer Firmware
*
* Based on Marlin, Sprinter and grbl
* Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
* Copyright (C) 2013 - 2016 Alberto Cotronei @MagoKimbra
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
*/
#include "../../base.h" #include "../../base.h"
#if ENABLED(DIGIPOT_I2C) #if ENABLED(DIGIPOT_I2C)
......
MK/module/digipot/digipot_mcp4451.h
View file @ 55a2c790
/**
* MK & MK4due 3D Printer Firmware
*
* Based on Marlin, Sprinter and grbl
* Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
* Copyright (C) 2013 - 2016 Alberto Cotronei @MagoKimbra
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
*/
#ifndef DIGIPOT_MCP4451_H #ifndef DIGIPOT_MCP4451_H
#define DIGIPOT_MCP4451_H #define DIGIPOT_MCP4451_H
......
MK/module/motion/endstops.cpp → MK/module/endstop/endstops.cpp
View file @ 55a2c790
...@@ -25,7 +25,7 @@ ...@@ -25,7 +25,7 @@
*/ */
#include "../../base.h" #include "../../base.h"
#include "endstops.h" //#include "endstops.h"
// TEST_ENDSTOP: test the old and the current status of an endstop // TEST_ENDSTOP: test the old and the current status of an endstop
#define TEST_ENDSTOP(ENDSTOP) (TEST(current_endstop_bits & old_endstop_bits, ENDSTOP)) #define TEST_ENDSTOP(ENDSTOP) (TEST(current_endstop_bits & old_endstop_bits, ENDSTOP))
...@@ -189,6 +189,9 @@ void Endstops::report_state() { ...@@ -189,6 +189,9 @@ void Endstops::report_state() {
card.sdprinting = false; card.sdprinting = false;
card.closeFile(); card.closeFile();
quickStop(); quickStop();
#if NOMECH(DELTA) && NOMECH(SCARA)
set_current_position_from_planner();
#endif
disable_all_heaters(); // switch off all heaters. disable_all_heaters(); // switch off all heaters.
disable_all_coolers(); disable_all_coolers();
} }
......
MK/module/laser/laser.cpp
View file @ 55a2c790
/** /**
* MK & MK4due 3D Printer Firmware * MK & MK4due 3D Printer Firmware
* *
...@@ -24,6 +23,7 @@ ...@@ -24,6 +23,7 @@
/** /**
* laser.cpp - Laser control library for Arduino using 16 bit timers- Version 1 * laser.cpp - Laser control library for Arduino using 16 bit timers- Version 1
* Copyright (c) 2013 Timothy Schmidt. All right reserved. * Copyright (c) 2013 Timothy Schmidt. All right reserved.
* Copyright (c) 2016 Franco (nextime) Lanza
* *
* This library is free software; you can redistribute it and/or * This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public * modify it under the terms of the GNU Lesser General Public
...@@ -335,7 +335,6 @@ ...@@ -335,7 +335,6 @@
} }
} }
} }
#endif // LASER_PERIPHERALS #endif // LASER_PERIPHERALS
#endif // LASERBEAM #endif // LASERBEAM
MK/module/laser/laser.h
View file @ 55a2c790
...@@ -23,6 +23,7 @@ ...@@ -23,6 +23,7 @@
/** /**
* laser.cpp - Laser control library for Arduino using 16 bit timers- Version 1 * laser.cpp - Laser control library for Arduino using 16 bit timers- Version 1
* Copyright (c) 2013 Timothy Schmidt. All right reserved. * Copyright (c) 2013 Timothy Schmidt. All right reserved.
* Copyright (c) 2016 Franco (nextime) Lanza
* *
* This library is free software; you can redistribute it and/or * This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public * modify it under the terms of the GNU Lesser General Public
......
MK/module/lcd/ultralcd.cpp
View file @ 55a2c790
...@@ -537,9 +537,9 @@ static void lcd_status_screen() { ...@@ -537,9 +537,9 @@ static void lcd_status_screen() {
inline void line_to_current(AxisEnum axis) { inline void line_to_current(AxisEnum axis) {
#if MECH(DELTA) #if MECH(DELTA)
calculate_delta(current_position); calculate_delta(current_position);
plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS], manual_feedrate[axis]/60, active_extruder, active_driver); planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS], manual_feedrate[axis]/60, active_extruder, active_driver);
#else #else
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], manual_feedrate[axis]/60, active_extruder, active_driver); planner.buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], manual_feedrate[axis]/60, active_extruder, active_driver);
#endif #endif
} }
...@@ -557,6 +557,9 @@ inline void line_to_current(AxisEnum axis) { ...@@ -557,6 +557,9 @@ inline void line_to_current(AxisEnum axis) {
static void lcd_sdcard_stop() { static void lcd_sdcard_stop() {
quickStop(); quickStop();
#if NOMECH(DELTA) && NOMECH(SCARA)
set_current_position_from_planner();
#endif
card.sdprinting = false; card.sdprinting = false;
card.closeFile(); card.closeFile();
print_job_counter.stop(); print_job_counter.stop();
...@@ -578,12 +581,12 @@ static void lcd_main_menu() { ...@@ -578,12 +581,12 @@ static void lcd_main_menu() {
MENU_ITEM(back, MSG_WATCH); MENU_ITEM(back, MSG_WATCH);
#if ENABLED(LASERBEAM) #if ENABLED(LASERBEAM)
if (!(movesplanned() || IS_SD_PRINTING)) { if (!(planner.movesplanned() || IS_SD_PRINTING)) {
MENU_ITEM(submenu, "Laser Functions", lcd_laser_menu); MENU_ITEM(submenu, "Laser Functions", lcd_laser_menu);
} }
#endif #endif
if (movesplanned() || IS_SD_PRINTING) { if (planner.movesplanned() || IS_SD_PRINTING) {
MENU_ITEM(submenu, MSG_TUNE, lcd_tune_menu); MENU_ITEM(submenu, MSG_TUNE, lcd_tune_menu);
} }
else { else {
...@@ -857,9 +860,9 @@ static void lcd_tune_menu() { ...@@ -857,9 +860,9 @@ static void lcd_tune_menu() {
current_position[E_AXIS] += length; current_position[E_AXIS] += length;
#if MECH(DELTA) #if MECH(DELTA)
calculate_delta(current_position); calculate_delta(current_position);
plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS], feedrate, active_extruder, active_driver); planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS], feedrate, active_extruder, active_driver);
#else #else
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], feedrate, active_extruder, active_driver); planner.buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], feedrate, active_extruder, active_driver);
#endif #endif
} }
static void lcd_purge() { lcd_extrude(LCD_PURGE_LENGTH, LCD_PURGE_FEEDRATE); } static void lcd_purge() { lcd_extrude(LCD_PURGE_LENGTH, LCD_PURGE_FEEDRATE); }
...@@ -1074,7 +1077,7 @@ void lcd_cooldown() { ...@@ -1074,7 +1077,7 @@ void lcd_cooldown() {
ENCODER_DIRECTION_NORMAL(); ENCODER_DIRECTION_NORMAL();
// Encoder wheel adjusts the Z position // Encoder wheel adjusts the Z position
if (encoderPosition && movesplanned() <= 3) { if (encoderPosition && planner.movesplanned() <= 3) {
refresh_cmd_timeout(); refresh_cmd_timeout();
current_position[Z_AXIS] += float((int32_t)encoderPosition) * (MBL_Z_STEP); current_position[Z_AXIS] += float((int32_t)encoderPosition) * (MBL_Z_STEP);
NOLESS(current_position[Z_AXIS], 0); NOLESS(current_position[Z_AXIS], 0);
...@@ -1106,7 +1109,7 @@ void lcd_cooldown() { ...@@ -1106,7 +1109,7 @@ void lcd_cooldown() {
line_to_current(Z_AXIS); line_to_current(Z_AXIS);
st_synchronize(); st_synchronize();
mbl.active = true; mbl.set_has_mesh(true);
enqueue_and_echo_commands_P(PSTR("G28")); enqueue_and_echo_commands_P(PSTR("G28"));
lcd_return_to_status(); lcd_return_to_status();
//LCD_MESSAGEPGM(MSG_LEVEL_BED_DONE); //LCD_MESSAGEPGM(MSG_LEVEL_BED_DONE);
...@@ -1177,7 +1180,7 @@ void lcd_cooldown() { ...@@ -1177,7 +1180,7 @@ void lcd_cooldown() {
if (LCD_CLICKED) { if (LCD_CLICKED) {
_lcd_level_bed_position = 0; _lcd_level_bed_position = 0;
current_position[Z_AXIS] = MESH_HOME_SEARCH_Z; current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]); planner.set_position_mm(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
lcd_goto_menu(_lcd_level_goto_next_point, true); lcd_goto_menu(_lcd_level_goto_next_point, true);
} }
} }
...@@ -1353,7 +1356,7 @@ float move_menu_scale; ...@@ -1353,7 +1356,7 @@ float move_menu_scale;
static void _lcd_move(const char* name, AxisEnum axis, float min, float max) { static void _lcd_move(const char* name, AxisEnum axis, float min, float max) {
ENCODER_DIRECTION_NORMAL(); ENCODER_DIRECTION_NORMAL();
if (encoderPosition && movesplanned() <= 3) { if (encoderPosition && planner.movesplanned() <= 3) {
refresh_cmd_timeout(); refresh_cmd_timeout();
current_position[axis] += float((int32_t)encoderPosition) * move_menu_scale; current_position[axis] += float((int32_t)encoderPosition) * move_menu_scale;
if (SOFTWARE_MIN_ENDSTOPS) NOLESS(current_position[axis], min); if (SOFTWARE_MIN_ENDSTOPS) NOLESS(current_position[axis], min);
...@@ -1386,7 +1389,7 @@ static void lcd_move_e( ...@@ -1386,7 +1389,7 @@ static void lcd_move_e(
unsigned short original_active_extruder = active_extruder; unsigned short original_active_extruder = active_extruder;
active_extruder = e; active_extruder = e;
#endif #endif
if (encoderPosition && movesplanned() <= 3) { if (encoderPosition && planner.movesplanned() <= 3) {
#if ENABLED(IDLE_OOZING_PREVENT) #if ENABLED(IDLE_OOZING_PREVENT)
IDLE_OOZING_retract(false); IDLE_OOZING_retract(false);
#endif #endif
...@@ -1684,10 +1687,10 @@ static void lcd_stats_menu() { ...@@ -1684,10 +1687,10 @@ static void lcd_stats_menu() {
// Autotemp, Min, Max, Fact // Autotemp, Min, Max, Fact
// //
#if ENABLED(AUTOTEMP) && (TEMP_SENSOR_0 != 0) #if ENABLED(AUTOTEMP) && (TEMP_SENSOR_0 != 0)
MENU_ITEM_EDIT(bool, MSG_AUTOTEMP, &autotemp_enabled); MENU_ITEM_EDIT(bool, MSG_AUTOTEMP, &planner.autotemp_enabled);
MENU_ITEM_EDIT(float3, MSG_MIN, &autotemp_min, 0, HEATER_0_MAXTEMP - 15); MENU_ITEM_EDIT(float3, MSG_MIN, &planner.autotemp_min, 0, HEATER_0_MAXTEMP - 15);
MENU_ITEM_EDIT(float3, MSG_MAX, &autotemp_max, 0, HEATER_0_MAXTEMP - 15); MENU_ITEM_EDIT(float3, MSG_MAX, &planner.autotemp_max, 0, HEATER_0_MAXTEMP - 15);
MENU_ITEM_EDIT(float32, MSG_FACTOR, &autotemp_factor, 0.0, 1.0); MENU_ITEM_EDIT(float32, MSG_FACTOR, &planner.autotemp_factor, 0.0, 1.0);
#endif #endif
// //
...@@ -1837,45 +1840,49 @@ static void lcd_control_motion_menu() { ...@@ -1837,45 +1840,49 @@ static void lcd_control_motion_menu() {
#if ENABLED(MANUAL_BED_LEVELING) #if ENABLED(MANUAL_BED_LEVELING)
MENU_ITEM_EDIT(float43, MSG_BED_Z, &mbl.z_offset, -1, 1); MENU_ITEM_EDIT(float43, MSG_BED_Z, &mbl.z_offset, -1, 1);
#endif #endif
MENU_ITEM_EDIT(float5, MSG_ACC, &acceleration, 10, 99000); MENU_ITEM_EDIT(float5, MSG_ACC, &planner.acceleration, 10, 99000);
MENU_ITEM_EDIT(float3, MSG_VXY_JERK, &max_xy_jerk, 1, 990); MENU_ITEM_EDIT(float3, MSG_VXY_JERK, &planner.max_xy_jerk, 1, 990);
MENU_ITEM_EDIT(float52, MSG_VZ_JERK, &max_z_jerk, 0.1, 990); #if MECH(DELTA)
MENU_ITEM_EDIT(float3, MSG_VMAX MSG_X, &max_feedrate[X_AXIS], 1, 999); MENU_ITEM_EDIT(float3, MSG_VZ_JERK, &planner.max_z_jerk, 1, 990);
MENU_ITEM_EDIT(float3, MSG_VMAX MSG_Y, &max_feedrate[Y_AXIS], 1, 999); #else
MENU_ITEM_EDIT(float3, MSG_VMAX MSG_Z, &max_feedrate[Z_AXIS], 1, 999); MENU_ITEM_EDIT(float52, MSG_VZ_JERK, &planner.max_z_jerk, 0.1, 990);
MENU_ITEM_EDIT(float3, MSG_VMIN, &minimumfeedrate, 0, 999); #endif
MENU_ITEM_EDIT(float3, MSG_VTRAV_MIN, &mintravelfeedrate, 0, 999); MENU_ITEM_EDIT(float3, MSG_VMAX MSG_X, &planner.max_feedrate[X_AXIS], 1, 999);
MENU_ITEM_EDIT_CALLBACK(long5, MSG_AMAX MSG_X, &max_acceleration_units_per_sq_second[X_AXIS], 100, 99000, reset_acceleration_rates); MENU_ITEM_EDIT(float3, MSG_VMAX MSG_Y, &planner.max_feedrate[Y_AXIS], 1, 999);
MENU_ITEM_EDIT_CALLBACK(long5, MSG_AMAX MSG_Y, &max_acceleration_units_per_sq_second[Y_AXIS], 100, 99000, reset_acceleration_rates); MENU_ITEM_EDIT(float3, MSG_VMAX MSG_Z, &planner.max_feedrate[Z_AXIS], 1, 999);
MENU_ITEM_EDIT_CALLBACK(long5, MSG_AMAX MSG_Z, &max_acceleration_units_per_sq_second[Z_AXIS], 10, 99000, reset_acceleration_rates); MENU_ITEM_EDIT(float3, MSG_VMIN, &planner.min_feedrate, 0, 999);
MENU_ITEM_EDIT(float5, MSG_A_TRAVEL, &travel_acceleration, 100, 99000); MENU_ITEM_EDIT(float3, MSG_VTRAV_MIN, &planner.min_travel_feedrate, 0, 999);
MENU_ITEM_EDIT(float52, MSG_XSTEPS, &axis_steps_per_unit[X_AXIS], 5, 9999); MENU_ITEM_EDIT_CALLBACK(long5, MSG_AMAX MSG_X, &planner.max_acceleration_units_per_sq_second[X_AXIS], 100, 99000, planner.reset_acceleration_rates);
MENU_ITEM_EDIT(float52, MSG_YSTEPS, &axis_steps_per_unit[Y_AXIS], 5, 9999); MENU_ITEM_EDIT_CALLBACK(long5, MSG_AMAX MSG_Y, &planner.max_acceleration_units_per_sq_second[Y_AXIS], 100, 99000, planner.reset_acceleration_rates);
MENU_ITEM_EDIT(float51, MSG_ZSTEPS, &axis_steps_per_unit[Z_AXIS], 5, 9999); MENU_ITEM_EDIT_CALLBACK(long5, MSG_AMAX MSG_Z, &planner.max_acceleration_units_per_sq_second[Z_AXIS], 10, 99000, planner.reset_acceleration_rates);
MENU_ITEM_EDIT(float5, MSG_A_TRAVEL, &planner.travel_acceleration, 100, 99000);
MENU_ITEM_EDIT(float52, MSG_XSTEPS, &planner.axis_steps_per_unit[X_AXIS], 5, 9999);
MENU_ITEM_EDIT(float52, MSG_YSTEPS, &planner.axis_steps_per_unit[Y_AXIS], 5, 9999);
MENU_ITEM_EDIT(float51, MSG_ZSTEPS, &planner.axis_steps_per_unit[Z_AXIS], 5, 9999);
#if EXTRUDERS > 0 #if EXTRUDERS > 0
MENU_ITEM_EDIT(float3, MSG_VE_JERK MSG_E "0", &max_e_jerk[0], 1, 990); MENU_ITEM_EDIT(float3, MSG_VE_JERK MSG_E "0", &planner.max_e_jerk[0], 1, 990);
MENU_ITEM_EDIT(float3, MSG_VMAX MSG_E "0", &max_feedrate[E_AXIS], 1, 999); MENU_ITEM_EDIT(float3, MSG_VMAX MSG_E "0", &planner.max_feedrate[E_AXIS], 1, 999);
MENU_ITEM_EDIT(long5, MSG_AMAX MSG_E "0", &max_acceleration_units_per_sq_second[E_AXIS], 100, 99000); MENU_ITEM_EDIT(long5, MSG_AMAX MSG_E "0", &planner.max_acceleration_units_per_sq_second[E_AXIS], 100, 99000);
MENU_ITEM_EDIT(float5, MSG_A_RETRACT MSG_E "0", &retract_acceleration[0], 100, 99000); MENU_ITEM_EDIT(float5, MSG_A_RETRACT MSG_E "0", &planner.retract_acceleration[0], 100, 99000);
MENU_ITEM_EDIT(float51, MSG_E0STEPS, &axis_steps_per_unit[E_AXIS], 5, 9999); MENU_ITEM_EDIT(float51, MSG_E0STEPS, &planner.axis_steps_per_unit[E_AXIS], 5, 9999);
#if EXTRUDERS > 1 #if EXTRUDERS > 1
MENU_ITEM_EDIT(float3, MSG_VE_JERK MSG_E "1", &max_e_jerk[1], 1, 990); MENU_ITEM_EDIT(float3, MSG_VE_JERK MSG_E "1", &planner.max_e_jerk[1], 1, 990);
MENU_ITEM_EDIT(float3, MSG_VMAX MSG_E "1", &max_feedrate[E_AXIS + 1], 1, 999); MENU_ITEM_EDIT(float3, MSG_VMAX MSG_E "1", &planner.max_feedrate[E_AXIS + 1], 1, 999);
MENU_ITEM_EDIT(long5, MSG_AMAX MSG_E "1", &max_acceleration_units_per_sq_second[E_AXIS + 1], 100, 99000); MENU_ITEM_EDIT(long5, MSG_AMAX MSG_E "1", &planner.max_acceleration_units_per_sq_second[E_AXIS + 1], 100, 99000);
MENU_ITEM_EDIT(float5, MSG_A_RETRACT MSG_E "1", &retract_acceleration[1], 100, 99000); MENU_ITEM_EDIT(float5, MSG_A_RETRACT MSG_E "1", &planner.retract_acceleration[1], 100, 99000);
MENU_ITEM_EDIT(float51, MSG_E1STEPS, &axis_steps_per_unit[E_AXIS + 1], 5, 9999); MENU_ITEM_EDIT(float51, MSG_E1STEPS, &planner.axis_steps_per_unit[E_AXIS + 1], 5, 9999);
#if EXTRUDERS > 2 #if EXTRUDERS > 2
MENU_ITEM_EDIT(float3, MSG_VE_JERK MSG_E "2", &max_e_jerk[2], 1, 990); MENU_ITEM_EDIT(float3, MSG_VE_JERK MSG_E "2", &planner.max_e_jerk[2], 1, 990);
MENU_ITEM_EDIT(float3, MSG_VMAX MSG_E "2", &max_feedrate[E_AXIS + 2], 1, 999); MENU_ITEM_EDIT(float3, MSG_VMAX MSG_E "2", &planner.max_feedrate[E_AXIS + 2], 1, 999);
MENU_ITEM_EDIT(long5, MSG_AMAX MSG_E "2", &max_acceleration_units_per_sq_second[E_AXIS + 2], 100, 99000); MENU_ITEM_EDIT(long5, MSG_AMAX MSG_E "2", &planner.max_acceleration_units_per_sq_second[E_AXIS + 2], 100, 99000);
MENU_ITEM_EDIT(float5, MSG_A_RETRACT MSG_E "2", &retract_acceleration[2], 100, 99000); MENU_ITEM_EDIT(float5, MSG_A_RETRACT MSG_E "2", &planner.retract_acceleration[2], 100, 99000);
MENU_ITEM_EDIT(float51, MSG_E2STEPS, &axis_steps_per_unit[E_AXIS + 2], 5, 9999); MENU_ITEM_EDIT(float51, MSG_E2STEPS, &planner.axis_steps_per_unit[E_AXIS + 2], 5, 9999);
#if EXTRUDERS > 3 #if EXTRUDERS > 3
MENU_ITEM_EDIT(float3, MSG_VE_JERK MSG_E "3", &max_e_jerk[3], 1, 990); MENU_ITEM_EDIT(float3, MSG_VE_JERK MSG_E "3", &planner.max_e_jerk[3], 1, 990);
MENU_ITEM_EDIT(float3, MSG_VMAX MSG_E "3", &max_feedrate[E_AXIS + 3], 1, 999); MENU_ITEM_EDIT(float3, MSG_VMAX MSG_E "3", &planner.max_feedrate[E_AXIS + 3], 1, 999);
MENU_ITEM_EDIT(long5, MSG_AMAX MSG_E "3", &max_acceleration_units_per_sq_second[E_AXIS + 3], 100, 99000); MENU_ITEM_EDIT(long5, MSG_AMAX MSG_E "3", &planner.max_acceleration_units_per_sq_second[E_AXIS + 3], 100, 99000);
MENU_ITEM_EDIT(float5, MSG_A_RETRACT MSG_E "3", &retract_acceleration[3], 100, 99000); MENU_ITEM_EDIT(float5, MSG_A_RETRACT MSG_E "3", &planner.retract_acceleration[3], 100, 99000);
MENU_ITEM_EDIT(float51, MSG_E3STEPS, &axis_steps_per_unit[E_AXIS + 3], 5, 9999); MENU_ITEM_EDIT(float51, MSG_E3STEPS, &planner.axis_steps_per_unit[E_AXIS + 3], 5, 9999);
#endif // EXTRUDERS > 3 #endif // EXTRUDERS > 3
#endif // EXTRUDERS > 2 #endif // EXTRUDERS > 2
#endif // EXTRUDERS > 1 #endif // EXTRUDERS > 1
...@@ -1989,7 +1996,7 @@ static void lcd_control_motion_menu() { ...@@ -1989,7 +1996,7 @@ static void lcd_control_motion_menu() {
if (laser_peripherals_ok()) { if (laser_peripherals_ok()) {
MENU_ITEM(function, "Turn On Pumps/Fans", action_laser_acc_on); MENU_ITEM(function, "Turn On Pumps/Fans", action_laser_acc_on);
} }
else if (!(movesplanned() || IS_SD_PRINTING)) { else if (!(planner.movesplanned() || IS_SD_PRINTING)) {
MENU_ITEM(function, "Turn Off Pumps/Fans", action_laser_acc_off); MENU_ITEM(function, "Turn Off Pumps/Fans", action_laser_acc_off);
} }
#endif // LASER_PERIPHERALS #endif // LASER_PERIPHERALS
......
MK/module/mbl/mesh_bed_leveling.cpp
View file @ 55a2c790
...@@ -29,7 +29,7 @@ ...@@ -29,7 +29,7 @@
mesh_bed_leveling::mesh_bed_leveling() { reset(); } mesh_bed_leveling::mesh_bed_leveling() { reset(); }
void mesh_bed_leveling::reset() { void mesh_bed_leveling::reset() {
active = 0; status = MBL_STATUS_NONE;
z_offset = 0; z_offset = 0;
for (int8_t y = MESH_NUM_Y_POINTS; y--;) for (int8_t y = MESH_NUM_Y_POINTS; y--;)
for (int8_t x = MESH_NUM_X_POINTS; x--;) for (int8_t x = MESH_NUM_X_POINTS; x--;)
......
MK/module/mbl/mesh_bed_leveling.h
View file @ 55a2c790
...@@ -22,12 +22,14 @@ ...@@ -22,12 +22,14 @@
#if ENABLED(MESH_BED_LEVELING) #if ENABLED(MESH_BED_LEVELING)
enum MBLStatus { MBL_STATUS_NONE = 0, MBL_STATUS_HAS_MESH_BIT = 0, MBL_STATUS_ACTIVE_BIT = 1 };
#define MESH_X_DIST ((MESH_MAX_X - (MESH_MIN_X))/(MESH_NUM_X_POINTS - 1)) #define MESH_X_DIST ((MESH_MAX_X - (MESH_MIN_X))/(MESH_NUM_X_POINTS - 1))
#define MESH_Y_DIST ((MESH_MAX_Y - (MESH_MIN_Y))/(MESH_NUM_Y_POINTS - 1)) #define MESH_Y_DIST ((MESH_MAX_Y - (MESH_MIN_Y))/(MESH_NUM_Y_POINTS - 1))
class mesh_bed_leveling { class mesh_bed_leveling {
public: public:
bool active; uint8_t status; // Has Mesh and Is Active bits
float z_offset; float z_offset;
float z_values[MESH_NUM_Y_POINTS][MESH_NUM_X_POINTS]; float z_values[MESH_NUM_Y_POINTS][MESH_NUM_X_POINTS];
...@@ -39,6 +41,11 @@ ...@@ -39,6 +41,11 @@
static FORCE_INLINE float get_probe_y(int8_t i) { return MESH_MIN_Y + (MESH_Y_DIST) * i; } static FORCE_INLINE float get_probe_y(int8_t i) { return MESH_MIN_Y + (MESH_Y_DIST) * i; }
void set_z(const int8_t px, const int8_t py, const float z) { z_values[py][px] = z; } void set_z(const int8_t px, const int8_t py, const float z) { z_values[py][px] = z; }
bool active() { return TEST(status, MBL_STATUS_ACTIVE_BIT); }
void set_active(bool onOff) { if (onOff) SBI(status, MBL_STATUS_ACTIVE_BIT); else CBI(status, MBL_STATUS_ACTIVE_BIT); }
bool has_mesh() { return TEST(status, MBL_STATUS_HAS_MESH_BIT); }
void set_has_mesh(bool onOff) { if (onOff) SBI(status, MBL_STATUS_HAS_MESH_BIT); else CBI(status, MBL_STATUS_HAS_MESH_BIT); }
inline void zigzag(int8_t index, int8_t &px, int8_t &py) { inline void zigzag(int8_t index, int8_t &px, int8_t &py) {
px = index % (MESH_NUM_X_POINTS); px = index % (MESH_NUM_X_POINTS);
py = index / (MESH_NUM_X_POINTS); py = index / (MESH_NUM_X_POINTS);
......
MK/module/mechanics.h
View file @ 55a2c790
...@@ -34,5 +34,6 @@ ...@@ -34,5 +34,6 @@
#define MECH_SCARA 4 #define MECH_SCARA 4
#define MECH(mech) (MECHANISM == MECH_##mech) #define MECH(mech) (MECHANISM == MECH_##mech)
#define NOMECH(mech) (MECHANISM != MECH_##mech)
#endif #endif
\ No newline at end of file
MK/module/motion/cartesian_correction.cpp
View file @ 55a2c790
...@@ -22,7 +22,7 @@ ...@@ -22,7 +22,7 @@
/** /**
* cartesian_correction.cpp * cartesian_correction.cpp
* A class that manages hysteresis by inserting extra plan_buffer_line when necessary * A class that manages hysteresis by inserting extra planner.buffer_line when necessary
* A class that manages ZWobble * A class that manages ZWobble
* *
* Copyright (c) 2012 Neil James Martin * Copyright (c) 2012 Neil James Martin
...@@ -86,7 +86,7 @@ ...@@ -86,7 +86,7 @@
//=========================================================================== //===========================================================================
void Hysteresis::calcSteps() { void Hysteresis::calcSteps() {
for (uint8_t i = 0; i < NUM_AXIS; i++) for (uint8_t i = 0; i < NUM_AXIS; i++)
m_hysteresis_steps[i] = (long)(m_hysteresis_mm[i] * axis_steps_per_unit[i]); m_hysteresis_steps[i] = (long)(m_hysteresis_mm[i] * planner.axis_steps_per_unit[i]);
} }
//=========================================================================== //===========================================================================
...@@ -135,9 +135,9 @@ ...@@ -135,9 +135,9 @@
} }
//=========================================================================== //===========================================================================
// insert a plan_buffer_line if required to handle any hysteresis // insert a planner.buffer_line if required to handle any hysteresis
void Hysteresis::InsertCorrection(const float x, const float y, const float z, const float e) { void Hysteresis::InsertCorrection(const float x, const float y, const float z, const float e) {
long destination[NUM_AXIS] = {x * axis_steps_per_unit[X_AXIS], y * axis_steps_per_unit[Y_AXIS], z * axis_steps_per_unit[Z_AXIS], e * axis_steps_per_unit[E_AXIS + active_extruder]}; long destination[NUM_AXIS] = {x * planner.axis_steps_per_unit[X_AXIS], y * planner.axis_steps_per_unit[Y_AXIS], z * planner.axis_steps_per_unit[Z_AXIS], e * planner.axis_steps_per_unit[E_AXIS + active_extruder]};
uint8_t direction_bits = calc_direction_bits(position, destination); uint8_t direction_bits = calc_direction_bits(position, destination);
uint8_t move_bits = calc_move_bits(position, destination); uint8_t move_bits = calc_move_bits(position, destination);
...@@ -459,12 +459,12 @@ ...@@ -459,12 +459,12 @@
} }
//=========================================================================== //===========================================================================
// insert a plan_buffer_line if required to handle any hysteresis // insert a planner.buffer_line if required to handle any hysteresis
void ZWobble::InsertCorrection(const float targetZ) { void ZWobble::InsertCorrection(const float targetZ) {
if (!m_consistent) return; // don't go through consistency checks all the time; just check one bool if (!m_consistent) return; // don't go through consistency checks all the time; just check one bool
float originZ = (float)position[Z_AXIS] / axis_steps_per_unit[Z_AXIS]; float originZ = (float)position[Z_AXIS] / planner.axis_steps_per_unit[Z_AXIS];
if (originZ < ZWOBBLE_MIN_Z || targetZ < ZWOBBLE_MIN_Z) return; if (originZ < ZWOBBLE_MIN_Z || targetZ < ZWOBBLE_MIN_Z) return;
...@@ -492,7 +492,7 @@ ...@@ -492,7 +492,7 @@
ECHO_MV(" Target Rod: ", targetZRod); ECHO_MV(" Target Rod: ", targetZRod);
// difference in steps between the correct movement (originZRod->targetZRod) and the planned movement // difference in steps between the correct movement (originZRod->targetZRod) and the planned movement
long stepDiff = lround((targetZRod - originZRod) * axis_steps_per_unit[Z_AXIS]) - (lround(targetZ * axis_steps_per_unit[Z_AXIS]) - position[Z_AXIS]); long stepDiff = lround((targetZRod - originZRod) * planner.axis_steps_per_unit[Z_AXIS]) - (lround(targetZ * planner.axis_steps_per_unit[Z_AXIS]) - position[Z_AXIS]);
if (DEBUGGING(DEBUG)) if (DEBUGGING(DEBUG))
ECHO_EMV(" stepDiff: ", stepDiff); ECHO_EMV(" stepDiff: ", stepDiff);
......
MK/module/motion/cartesian_correction.h
View file @ 55a2c790
...@@ -22,7 +22,7 @@ ...@@ -22,7 +22,7 @@
/** /**
* cartesian_correction.h * cartesian_correction.h
* A class that manages hysteresis by inserting extra plan_buffer_line when necessary * A class that manages hysteresis by inserting extra planner.buffer_line when necessary
* A class that manages ZWobble * A class that manages ZWobble
* *
* Copyright (c) 2012 Neil James Martin * Copyright (c) 2012 Neil James Martin
......
MK/module/motion/qr_solve.cpp deleted 100644 → 0
View file @ 18664abd
/**
* MK & MK4due 3D Printer Firmware
*
* Based on Marlin, Sprinter and grbl
* Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
* Copyright (C) 2013 - 2016 Alberto Cotronei @MagoKimbra
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
*/
#include "../../base.h"
#if ENABLED(AUTO_BED_LEVELING_FEATURE) && ENABLED(AUTO_BED_LEVELING_GRID)
#include "qr_solve.h"
#include <stdlib.h>
#include <math.h>
//# include "r8lib.h"
int i4_min(int i1, int i2)
/******************************************************************************/
/*
Purpose:
I4_MIN returns the smaller of two I4's.
Licensing:
This code is distributed under the GNU LGPL license.
Modified:
29 August 2006
Author:
John Burkardt
Parameters:
Input, int I1, I2, two integers to be compared.
Output, int I4_MIN, the smaller of I1 and I2.
*/
{
return (i1 < i2) ? i1 : i2;
}
double r8_epsilon(void)
/******************************************************************************/
/*
Purpose:
R8_EPSILON returns the R8 round off unit.
Discussion:
R8_EPSILON is a number R which is a power of 2 with the property that,
to the precision of the computer's arithmetic,
1 < 1 + R
but
1 = ( 1 + R / 2 )
Licensing:
This code is distributed under the GNU LGPL license.
Modified:
01 September 2012
Author:
John Burkardt
Parameters:
Output, double R8_EPSILON, the R8 round-off unit.
*/
{
const double value = 2.220446049250313E-016;
return value;
}
double r8_max(double x, double y)
/******************************************************************************/
/*
Purpose:
R8_MAX returns the maximum of two R8's.
Licensing:
This code is distributed under the GNU LGPL license.
Modified:
07 May 2006
Author:
John Burkardt
Parameters:
Input, double X, Y, the quantities to compare.
Output, double R8_MAX, the maximum of X and Y.
*/
{
return (y < x) ? x : y;
}
double r8_abs(double x)
/******************************************************************************/
/*
Purpose:
R8_ABS returns the absolute value of an R8.
Licensing:
This code is distributed under the GNU LGPL license.
Modified:
07 May 2006
Author:
John Burkardt
Parameters:
Input, double X, the quantity whose absolute value is desired.
Output, double R8_ABS, the absolute value of X.
*/
{
return (x < 0.0) ? -x : x;
}
double r8_sign(double x)
/******************************************************************************/
/*
Purpose:
R8_SIGN returns the sign of an R8.
Licensing:
This code is distributed under the GNU LGPL license.
Modified:
08 May 2006
Author:
John Burkardt
Parameters:
Input, double X, the number whose sign is desired.
Output, double R8_SIGN, the sign of X.
*/
{
return (x < 0.0) ? -1.0 : 1.0;
}
double r8mat_amax(int m, int n, double a[])
/******************************************************************************/
/*
Purpose:
R8MAT_AMAX returns the maximum absolute value entry of an R8MAT.
Discussion:
An R8MAT is a doubly dimensioned array of R8 values, stored as a vector
in column-major order.
Licensing:
This code is distributed under the GNU LGPL license.
Modified:
07 September 2012
Author:
John Burkardt
Parameters:
Input, int M, the number of rows in A.
Input, int N, the number of columns in A.
Input, double A[M*N], the M by N matrix.
Output, double R8MAT_AMAX, the maximum absolute value entry of A.
*/
{
double value = r8_abs(a[0 + 0 * m]);
for (int j = 0; j < n; j++) {
for (int i = 0; i < m; i++) {
NOLESS(value, r8_abs(a[i + j * m]));
}
}
return value;
}
void r8mat_copy(double a2[], int m, int n, double a1[])
/******************************************************************************/
/*
Purpose:
R8MAT_COPY_NEW copies one R8MAT to a "new" R8MAT.
Discussion:
An R8MAT is a doubly dimensioned array of R8 values, stored as a vector
in column-major order.
Licensing:
This code is distributed under the GNU LGPL license.
Modified:
26 July 2008
Author:
John Burkardt
Parameters:
Input, int M, N, the number of rows and columns.
Input, double A1[M*N], the matrix to be copied.
Output, double R8MAT_COPY_NEW[M*N], the copy of A1.
*/
{
for (int j = 0; j < n; j++) {
for (int i = 0; i < m; i++)
a2[i + j * m] = a1[i + j * m];
}
}
/******************************************************************************/
void daxpy(int n, double da, double dx[], int incx, double dy[], int incy)
/******************************************************************************/
/*
Purpose:
DAXPY computes constant times a vector plus a vector.
Discussion:
This routine uses unrolled loops for increments equal to one.
Licensing:
This code is distributed under the GNU LGPL license.
Modified:
30 March 2007
Author:
C version by John Burkardt
Reference:
Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
LINPACK User's Guide,
SIAM, 1979.
Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
Basic Linear Algebra Subprograms for Fortran Usage,
Algorithm 539,
ACM Transactions on Mathematical Software,
Volume 5, Number 3, September 1979, pages 308-323.
Parameters:
Input, int N, the number of elements in DX and DY.
Input, double DA, the multiplier of DX.
Input, double DX[*], the first vector.
Input, int INCX, the increment between successive entries of DX.
Input/output, double DY[*], the second vector.
On output, DY[*] has been replaced by DY[*] + DA * DX[*].
Input, int INCY, the increment between successive entries of DY.
*/
{
if (n <= 0 || da == 0.0) return;
int i, ix, iy, m;
/*
Code for unequal increments or equal increments
not equal to 1.
*/
if (incx != 1 || incy != 1) {
if (0 <= incx)
ix = 0;
else
ix = (- n + 1) * incx;
if (0 <= incy)
iy = 0;
else
iy = (- n + 1) * incy;
for (i = 0; i < n; i++) {
dy[iy] = dy[iy] + da * dx[ix];
ix = ix + incx;
iy = iy + incy;
}
}
/*
Code for both increments equal to 1.
*/
else {
m = n % 4;
for (i = 0; i < m; i++)
dy[i] = dy[i] + da * dx[i];
for (i = m; i < n; i = i + 4) {
dy[i ] = dy[i ] + da * dx[i ];
dy[i + 1] = dy[i + 1] + da * dx[i + 1];
dy[i + 2] = dy[i + 2] + da * dx[i + 2];
dy[i + 3] = dy[i + 3] + da * dx[i + 3];
}
}
}
/******************************************************************************/
double ddot(int n, double dx[], int incx, double dy[], int incy)
/******************************************************************************/
/*
Purpose:
DDOT forms the dot product of two vectors.
Discussion:
This routine uses unrolled loops for increments equal to one.
Licensing:
This code is distributed under the GNU LGPL license.
Modified:
30 March 2007
Author:
C version by John Burkardt
Reference:
Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
LINPACK User's Guide,
SIAM, 1979.
Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
Basic Linear Algebra Subprograms for Fortran Usage,
Algorithm 539,
ACM Transactions on Mathematical Software,
Volume 5, Number 3, September 1979, pages 308-323.
Parameters:
Input, int N, the number of entries in the vectors.
Input, double DX[*], the first vector.
Input, int INCX, the increment between successive entries in DX.
Input, double DY[*], the second vector.
Input, int INCY, the increment between successive entries in DY.
Output, double DDOT, the sum of the product of the corresponding
entries of DX and DY.
*/
{
if (n <= 0) return 0.0;
int i, m;
double dtemp = 0.0;
/*
Code for unequal increments or equal increments
not equal to 1.
*/
if (incx != 1 || incy != 1) {
int ix = (incx >= 0) ? 0 : (-n + 1) * incx,
iy = (incy >= 0) ? 0 : (-n + 1) * incy;
for (i = 0; i < n; i++) {
dtemp += dx[ix] * dy[iy];
ix = ix + incx;
iy = iy + incy;
}
}
/*
Code for both increments equal to 1.
*/
else {
m = n % 5;
for (i = 0; i < m; i++)
dtemp += dx[i] * dy[i];
for (i = m; i < n; i = i + 5) {
dtemp += dx[i] * dy[i]
+ dx[i + 1] * dy[i + 1]
+ dx[i + 2] * dy[i + 2]
+ dx[i + 3] * dy[i + 3]
+ dx[i + 4] * dy[i + 4];
}
}
return dtemp;
}
/******************************************************************************/
double dnrm2(int n, double x[], int incx)
/******************************************************************************/
/*
Purpose:
DNRM2 returns the euclidean norm of a vector.
Discussion:
DNRM2 ( X ) = sqrt ( X' * X )
Licensing:
This code is distributed under the GNU LGPL license.
Modified:
30 March 2007
Author:
C version by John Burkardt
Reference:
Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
LINPACK User's Guide,
SIAM, 1979.
Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
Basic Linear Algebra Subprograms for Fortran Usage,
Algorithm 539,
ACM Transactions on Mathematical Software,
Volume 5, Number 3, September 1979, pages 308-323.
Parameters:
Input, int N, the number of entries in the vector.
Input, double X[*], the vector whose norm is to be computed.
Input, int INCX, the increment between successive entries of X.
Output, double DNRM2, the Euclidean norm of X.
*/
{
double norm;
if (n < 1 || incx < 1)
norm = 0.0;
else if (n == 1)
norm = r8_abs(x[0]);
else {
double scale = 0.0, ssq = 1.0;
int ix = 0;
for (int i = 0; i < n; i++) {
if (x[ix] != 0.0) {
double absxi = r8_abs(x[ix]);
if (scale < absxi) {
ssq = 1.0 + ssq * (scale / absxi) * (scale / absxi);
scale = absxi;
}
else
ssq = ssq + (absxi / scale) * (absxi / scale);
}
ix += incx;
}
norm = scale * sqrt(ssq);
}
return norm;
}
/******************************************************************************/
void dqrank(double a[], int lda, int m, int n, double tol, int* kr,
int jpvt[], double qraux[])
/******************************************************************************/
/*
Purpose:
DQRANK computes the QR factorization of a rectangular matrix.
Discussion:
This routine is used in conjunction with DQRLSS to solve
overdetermined, underdetermined and singular linear systems
in a least squares sense.
DQRANK uses the LINPACK subroutine DQRDC to compute the QR
factorization, with column pivoting, of an M by N matrix A.
The numerical rank is determined using the tolerance TOL.
Note that on output, ABS ( A(1,1) ) / ABS ( A(KR,KR) ) is an estimate
of the condition number of the matrix of independent columns,
and of R. This estimate will be <= 1/TOL.
Licensing:
This code is distributed under the GNU LGPL license.
Modified:
21 April 2012
Author:
C version by John Burkardt.
Reference:
Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
LINPACK User's Guide,
SIAM, 1979,
ISBN13: 978-0-898711-72-1,
LC: QA214.L56.
Parameters:
Input/output, double A[LDA*N]. On input, the matrix whose
decomposition is to be computed. On output, the information from DQRDC.
The triangular matrix R of the QR factorization is contained in the
upper triangle and information needed to recover the orthogonal
matrix Q is stored below the diagonal in A and in the vector QRAUX.
Input, int LDA, the leading dimension of A, which must
be at least M.
Input, int M, the number of rows of A.
Input, int N, the number of columns of A.
Input, double TOL, a relative tolerance used to determine the
numerical rank. The problem should be scaled so that all the elements
of A have roughly the same absolute accuracy, EPS. Then a reasonable
value for TOL is roughly EPS divided by the magnitude of the largest
element.
Output, int *KR, the numerical rank.
Output, int JPVT[N], the pivot information from DQRDC.
Columns JPVT(1), ..., JPVT(KR) of the original matrix are linearly
independent to within the tolerance TOL and the remaining columns
are linearly dependent.
Output, double QRAUX[N], will contain extra information defining
the QR factorization.
*/
{
double work[n];
for (int i = 0; i < n; i++)
jpvt[i] = 0;
int job = 1;
dqrdc(a, lda, m, n, qraux, jpvt, work, job);
*kr = 0;
int k = i4_min(m, n);
for (int j = 0; j < k; j++) {
if (r8_abs(a[j + j * lda]) <= tol * r8_abs(a[0 + 0 * lda]))
return;
*kr = j + 1;
}
}
/******************************************************************************/
void dqrdc(double a[], int lda, int n, int p, double qraux[], int jpvt[],
double work[], int job)
/******************************************************************************/
/*
Purpose:
DQRDC computes the QR factorization of a real rectangular matrix.
Discussion:
DQRDC uses Householder transformations.
Column pivoting based on the 2-norms of the reduced columns may be
performed at the user's option.
Licensing:
This code is distributed under the GNU LGPL license.
Modified:
07 June 2005
Author:
C version by John Burkardt.
Reference:
Jack Dongarra, Cleve Moler, Jim Bunch and Pete Stewart,
LINPACK User's Guide,
SIAM, (Society for Industrial and Applied Mathematics),
3600 University City Science Center,
Philadelphia, PA, 19104-2688.
ISBN 0-89871-172-X
Parameters:
Input/output, double A(LDA,P). On input, the N by P matrix
whose decomposition is to be computed. On output, A contains in
its upper triangle the upper triangular matrix R of the QR
factorization. Below its diagonal A contains information from
which the orthogonal part of the decomposition can be recovered.
Note that if pivoting has been requested, the decomposition is not that
of the original matrix A but that of A with its columns permuted
as described by JPVT.
Input, int LDA, the leading dimension of the array A. LDA must
be at least N.
Input, int N, the number of rows of the matrix A.
Input, int P, the number of columns of the matrix A.
Output, double QRAUX[P], contains further information required
to recover the orthogonal part of the decomposition.
Input/output, integer JPVT[P]. On input, JPVT contains integers that
control the selection of the pivot columns. The K-th column A(*,K) of A
is placed in one of three classes according to the value of JPVT(K).
> 0, then A(K) is an initial column.
= 0, then A(K) is a free column.
< 0, then A(K) is a final column.
Before the decomposition is computed, initial columns are moved to
the beginning of the array A and final columns to the end. Both
initial and final columns are frozen in place during the computation
and only free columns are moved. At the K-th stage of the
reduction, if A(*,K) is occupied by a free column it is interchanged
with the free column of largest reduced norm. JPVT is not referenced
if JOB == 0. On output, JPVT(K) contains the index of the column of the
original matrix that has been interchanged into the K-th column, if
pivoting was requested.
Workspace, double WORK[P]. WORK is not referenced if JOB == 0.
Input, int JOB, initiates column pivoting.
0, no pivoting is done.
nonzero, pivoting is done.
*/
{
int jp;
int j;
int lup;
int maxj;
double maxnrm, nrmxl, t, tt;
int pl = 1, pu = 0;
/*
If pivoting is requested, rearrange the columns.
*/
if (job != 0) {
for (j = 1; j <= p; j++) {
int swapj = (0 < jpvt[j - 1]);
jpvt[j - 1] = (jpvt[j - 1] < 0) ? -j : j;
if (swapj) {
if (j != pl)
dswap(n, a + 0 + (pl - 1)*lda, 1, a + 0 + (j - 1), 1);
jpvt[j - 1] = jpvt[pl - 1];
jpvt[pl - 1] = j;
pl++;
}
}
pu = p;
for (j = p; 1 <= j; j--) {
if (jpvt[j - 1] < 0) {
jpvt[j - 1] = -jpvt[j - 1];
if (j != pu) {
dswap(n, a + 0 + (pu - 1)*lda, 1, a + 0 + (j - 1)*lda, 1);
jp = jpvt[pu - 1];
jpvt[pu - 1] = jpvt[j - 1];
jpvt[j - 1] = jp;
}
pu = pu - 1;
}
}
}
/*
Compute the norms of the free columns.
*/
for (j = pl; j <= pu; j++)
qraux[j - 1] = dnrm2(n, a + 0 + (j - 1) * lda, 1);
for (j = pl; j <= pu; j++)
work[j - 1] = qraux[j - 1];
/*
Perform the Householder reduction of A.
*/
lup = i4_min(n, p);
for (int l = 1; l <= lup; l++) {
/*
Bring the column of largest norm into the pivot position.
*/
if (pl <= l && l < pu) {
maxnrm = 0.0;
maxj = l;
for (j = l; j <= pu; j++) {
if (maxnrm < qraux[j - 1]) {
maxnrm = qraux[j - 1];
maxj = j;
}
}
if (maxj != l) {
dswap(n, a + 0 + (l - 1)*lda, 1, a + 0 + (maxj - 1)*lda, 1);
qraux[maxj - 1] = qraux[l - 1];
work[maxj - 1] = work[l - 1];
jp = jpvt[maxj - 1];
jpvt[maxj - 1] = jpvt[l - 1];
jpvt[l - 1] = jp;
}
}
/*
Compute the Householder transformation for column L.
*/
qraux[l - 1] = 0.0;
if (l != n) {
nrmxl = dnrm2(n - l + 1, a + l - 1 + (l - 1) * lda, 1);
if (nrmxl != 0.0) {
if (a[l - 1 + (l - 1)*lda] != 0.0)
nrmxl = nrmxl * r8_sign(a[l - 1 + (l - 1) * lda]);
dscal(n - l + 1, 1.0 / nrmxl, a + l - 1 + (l - 1)*lda, 1);
a[l - 1 + (l - 1)*lda] = 1.0 + a[l - 1 + (l - 1) * lda];
/*
Apply the transformation to the remaining columns, updating the norms.
*/
for (j = l + 1; j <= p; j++) {
t = -ddot(n - l + 1, a + l - 1 + (l - 1) * lda, 1, a + l - 1 + (j - 1) * lda, 1)
/ a[l - 1 + (l - 1) * lda];
daxpy(n - l + 1, t, a + l - 1 + (l - 1)*lda, 1, a + l - 1 + (j - 1)*lda, 1);
if (pl <= j && j <= pu) {
if (qraux[j - 1] != 0.0) {
tt = 1.0 - pow(r8_abs(a[l - 1 + (j - 1) * lda]) / qraux[j - 1], 2);
tt = r8_max(tt, 0.0);
t = tt;
tt = 1.0 + 0.05 * tt * pow(qraux[j - 1] / work[j - 1], 2);
if (tt != 1.0)
qraux[j - 1] = qraux[j - 1] * sqrt(t);
else {
qraux[j - 1] = dnrm2(n - l, a + l + (j - 1) * lda, 1);
work[j - 1] = qraux[j - 1];
}
}
}
}
/*
Save the transformation.
*/
qraux[l - 1] = a[l - 1 + (l - 1) * lda];
a[l - 1 + (l - 1)*lda] = -nrmxl;
}
}
}
}
/******************************************************************************/
int dqrls(double a[], int lda, int m, int n, double tol, int* kr, double b[],
double x[], double rsd[], int jpvt[], double qraux[], int itask)
/******************************************************************************/
/*
Purpose:
DQRLS factors and solves a linear system in the least squares sense.
Discussion:
The linear system may be overdetermined, underdetermined or singular.
The solution is obtained using a QR factorization of the
coefficient matrix.
DQRLS can be efficiently used to solve several least squares
problems with the same matrix A. The first system is solved
with ITASK = 1. The subsequent systems are solved with
ITASK = 2, to avoid the recomputation of the matrix factors.
The parameters KR, JPVT, and QRAUX must not be modified
between calls to DQRLS.
DQRLS is used to solve in a least squares sense
overdetermined, underdetermined and singular linear systems.
The system is A*X approximates B where A is M by N.
B is a given M-vector, and X is the N-vector to be computed.
A solution X is found which minimimzes the sum of squares (2-norm)
of the residual, A*X - B.
The numerical rank of A is determined using the tolerance TOL.
DQRLS uses the LINPACK subroutine DQRDC to compute the QR
factorization, with column pivoting, of an M by N matrix A.
Licensing:
This code is distributed under the GNU LGPL license.
Modified:
10 September 2012
Author:
C version by John Burkardt.
Reference:
David Kahaner, Cleve Moler, Steven Nash,
Numerical Methods and Software,
Prentice Hall, 1989,
ISBN: 0-13-627258-4,
LC: TA345.K34.
Parameters:
Input/output, double A[LDA*N], an M by N matrix.
On input, the matrix whose decomposition is to be computed.
In a least squares data fitting problem, A(I,J) is the
value of the J-th basis (model) function at the I-th data point.
On output, A contains the output from DQRDC. The triangular matrix R
of the QR factorization is contained in the upper triangle and
information needed to recover the orthogonal matrix Q is stored
below the diagonal in A and in the vector QRAUX.
Input, int LDA, the leading dimension of A.
Input, int M, the number of rows of A.
Input, int N, the number of columns of A.
Input, double TOL, a relative tolerance used to determine the
numerical rank. The problem should be scaled so that all the elements
of A have roughly the same absolute accuracy EPS. Then a reasonable
value for TOL is roughly EPS divided by the magnitude of the largest
element.
Output, int *KR, the numerical rank.
Input, double B[M], the right hand side of the linear system.
Output, double X[N], a least squares solution to the linear
system.
Output, double RSD[M], the residual, B - A*X. RSD may
overwrite B.
Workspace, int JPVT[N], required if ITASK = 1.
Columns JPVT(1), ..., JPVT(KR) of the original matrix are linearly
independent to within the tolerance TOL and the remaining columns
are linearly dependent. ABS ( A(1,1) ) / ABS ( A(KR,KR) ) is an estimate
of the condition number of the matrix of independent columns,
and of R. This estimate will be <= 1/TOL.
Workspace, double QRAUX[N], required if ITASK = 1.
Input, int ITASK.
1, DQRLS factors the matrix A and solves the least squares problem.
2, DQRLS assumes that the matrix A was factored with an earlier
call to DQRLS, and only solves the least squares problem.
Output, int DQRLS, error code.
0: no error
-1: LDA < M (fatal error)
-2: N < 1 (fatal error)
-3: ITASK < 1 (fatal error)
*/
{
int ind;
if (lda < m) {
/*fprintf ( stderr, "\n" );
fprintf ( stderr, "DQRLS - Fatal error!\n" );
fprintf ( stderr, " LDA < M.\n" );*/
ind = -1;
return ind;
}
if (n <= 0) {
/*fprintf ( stderr, "\n" );
fprintf ( stderr, "DQRLS - Fatal error!\n" );
fprintf ( stderr, " N <= 0.\n" );*/
ind = -2;
return ind;
}
if (itask < 1) {
/*fprintf ( stderr, "\n" );
fprintf ( stderr, "DQRLS - Fatal error!\n" );
fprintf ( stderr, " ITASK < 1.\n" );*/
ind = -3;
return ind;
}
ind = 0;
/*
Factor the matrix.
*/
if (itask == 1)
dqrank(a, lda, m, n, tol, kr, jpvt, qraux);
/*
Solve the least-squares problem.
*/
dqrlss(a, lda, m, n, *kr, b, x, rsd, jpvt, qraux);
return ind;
}
/******************************************************************************/
void dqrlss(double a[], int lda, int m, int n, int kr, double b[], double x[],
double rsd[], int jpvt[], double qraux[])
/******************************************************************************/
/*
Purpose:
DQRLSS solves a linear system in a least squares sense.
Discussion:
DQRLSS must be preceded by a call to DQRANK.
The system is to be solved is
A * X = B
where
A is an M by N matrix with rank KR, as determined by DQRANK,
B is a given M-vector,
X is the N-vector to be computed.
A solution X, with at most KR nonzero components, is found which
minimizes the 2-norm of the residual (A*X-B).
Once the matrix A has been formed, DQRANK should be
called once to decompose it. Then, for each right hand
side B, DQRLSS should be called once to obtain the
solution and residual.
Licensing:
This code is distributed under the GNU LGPL license.
Modified:
10 September 2012
Author:
C version by John Burkardt
Parameters:
Input, double A[LDA*N], the QR factorization information
from DQRANK. The triangular matrix R of the QR factorization is
contained in the upper triangle and information needed to recover
the orthogonal matrix Q is stored below the diagonal in A and in
the vector QRAUX.
Input, int LDA, the leading dimension of A, which must
be at least M.
Input, int M, the number of rows of A.
Input, int N, the number of columns of A.
Input, int KR, the rank of the matrix, as estimated by DQRANK.
Input, double B[M], the right hand side of the linear system.
Output, double X[N], a least squares solution to the
linear system.
Output, double RSD[M], the residual, B - A*X. RSD may
overwrite B.
Input, int JPVT[N], the pivot information from DQRANK.
Columns JPVT[0], ..., JPVT[KR-1] of the original matrix are linearly
independent to within the tolerance TOL and the remaining columns
are linearly dependent.
Input, double QRAUX[N], auxiliary information from DQRANK
defining the QR factorization.
*/
{
int i;
int info;
int j;
int job;
int k;
double t;
if (kr != 0) {
job = 110;
info = dqrsl(a, lda, m, kr, qraux, b, rsd, rsd, x, rsd, rsd, job);
UNUSED(info);
}
for (i = 0; i < n; i++)
jpvt[i] = - jpvt[i];
for (i = kr; i < n; i++)
x[i] = 0.0;
for (j = 1; j <= n; j++) {
if (jpvt[j - 1] <= 0) {
k = - jpvt[j - 1];
jpvt[j - 1] = k;
while (k != j) {
t = x[j - 1];
x[j - 1] = x[k - 1];
x[k - 1] = t;
jpvt[k - 1] = -jpvt[k - 1];
k = jpvt[k - 1];
}
}
}
}
/******************************************************************************/
int dqrsl(double a[], int lda, int n, int k, double qraux[], double y[],
double qy[], double qty[], double b[], double rsd[], double ab[], int job)
/******************************************************************************/
/*
Purpose:
DQRSL computes transformations, projections, and least squares solutions.
Discussion:
DQRSL requires the output of DQRDC.
For K <= min(N,P), let AK be the matrix
AK = ( A(JPVT[0]), A(JPVT(2)), ..., A(JPVT(K)) )
formed from columns JPVT[0], ..., JPVT(K) of the original
N by P matrix A that was input to DQRDC. If no pivoting was
done, AK consists of the first K columns of A in their
original order. DQRDC produces a factored orthogonal matrix Q
and an upper triangular matrix R such that
AK = Q * (R)
(0)
This information is contained in coded form in the arrays
A and QRAUX.
The parameters QY, QTY, B, RSD, and AB are not referenced
if their computation is not requested and in this case
can be replaced by dummy variables in the calling program.
To save storage, the user may in some cases use the same
array for different parameters in the calling sequence. A
frequently occurring example is when one wishes to compute
any of B, RSD, or AB and does not need Y or QTY. In this
case one may identify Y, QTY, and one of B, RSD, or AB, while
providing separate arrays for anything else that is to be
computed.
Thus the calling sequence
dqrsl ( a, lda, n, k, qraux, y, dum, y, b, y, dum, 110, info )
will result in the computation of B and RSD, with RSD
overwriting Y. More generally, each item in the following
list contains groups of permissible identifications for
a single calling sequence.
1. (Y,QTY,B) (RSD) (AB) (QY)
2. (Y,QTY,RSD) (B) (AB) (QY)
3. (Y,QTY,AB) (B) (RSD) (QY)
4. (Y,QY) (QTY,B) (RSD) (AB)
5. (Y,QY) (QTY,RSD) (B) (AB)
6. (Y,QY) (QTY,AB) (B) (RSD)
In any group the value returned in the array allocated to
the group corresponds to the last member of the group.
Licensing:
This code is distributed under the GNU LGPL license.
Modified:
07 June 2005
Author:
C version by John Burkardt.
Reference:
Jack Dongarra, Cleve Moler, Jim Bunch and Pete Stewart,
LINPACK User's Guide,
SIAM, (Society for Industrial and Applied Mathematics),
3600 University City Science Center,
Philadelphia, PA, 19104-2688.
ISBN 0-89871-172-X
Parameters:
Input, double A[LDA*P], contains the output of DQRDC.
Input, int LDA, the leading dimension of the array A.
Input, int N, the number of rows of the matrix AK. It must
have the same value as N in DQRDC.
Input, int K, the number of columns of the matrix AK. K
must not be greater than min(N,P), where P is the same as in the
calling sequence to DQRDC.
Input, double QRAUX[P], the auxiliary output from DQRDC.
Input, double Y[N], a vector to be manipulated by DQRSL.
Output, double QY[N], contains Q * Y, if requested.
Output, double QTY[N], contains Q' * Y, if requested.
Output, double B[K], the solution of the least squares problem
minimize norm2 ( Y - AK * B),
if its computation has been requested. Note that if pivoting was
requested in DQRDC, the J-th component of B will be associated with
column JPVT(J) of the original matrix A that was input into DQRDC.
Output, double RSD[N], the least squares residual Y - AK * B,
if its computation has been requested. RSD is also the orthogonal
projection of Y onto the orthogonal complement of the column space
of AK.
Output, double AB[N], the least squares approximation Ak * B,
if its computation has been requested. AB is also the orthogonal
projection of Y onto the column space of A.
Input, integer JOB, specifies what is to be computed. JOB has
the decimal expansion ABCDE, with the following meaning:
if A != 0, compute QY.
if B != 0, compute QTY.
if C != 0, compute QTY and B.
if D != 0, compute QTY and RSD.
if E != 0, compute QTY and AB.
Note that a request to compute B, RSD, or AB automatically triggers
the computation of QTY, for which an array must be provided in the
calling sequence.
Output, int DQRSL, is zero unless the computation of B has
been requested and R is exactly singular. In this case, INFO is the
index of the first zero diagonal element of R, and B is left unaltered.
*/
{
int cab;
int cb;
int cqty;
int cqy;
int cr;
int i;
int info;
int j;
int jj;
int ju;
double t;
double temp;
/*
Set INFO flag.
*/
info = 0;
/*
Determine what is to be computed.
*/
cqy = ( job / 10000 != 0);
cqty = ((job % 10000) != 0);
cb = ((job % 1000) / 100 != 0);
cr = ((job % 100) / 10 != 0);
cab = ((job % 10) != 0);
ju = i4_min(k, n - 1);
/*
Special action when N = 1.
*/
if (ju == 0) {
if (cqy)
qy[0] = y[0];
if (cqty)
qty[0] = y[0];
if (cab)
ab[0] = y[0];
if (cb) {
if (a[0 + 0 * lda] == 0.0)
info = 1;
else
b[0] = y[0] / a[0 + 0 * lda];
}
if (cr)
rsd[0] = 0.0;
return info;
}
/*
Set up to compute QY or QTY.
*/
if (cqy) {
for (i = 1; i <= n; i++)
qy[i - 1] = y[i - 1];
}
if (cqty) {
for (i = 1; i <= n; i++)
qty[i - 1] = y[i - 1];
}
/*
Compute QY.
*/
if (cqy) {
for (jj = 1; jj <= ju; jj++) {
j = ju - jj + 1;
if (qraux[j - 1] != 0.0) {
temp = a[j - 1 + (j - 1) * lda];
a[j - 1 + (j - 1)*lda] = qraux[j - 1];
t = -ddot(n - j + 1, a + j - 1 + (j - 1) * lda, 1, qy + j - 1, 1) / a[j - 1 + (j - 1) * lda];
daxpy(n - j + 1, t, a + j - 1 + (j - 1)*lda, 1, qy + j - 1, 1);
a[j - 1 + (j - 1)*lda] = temp;
}
}
}
/*
Compute Q'*Y.
*/
if (cqty) {
for (j = 1; j <= ju; j++) {
if (qraux[j - 1] != 0.0) {
temp = a[j - 1 + (j - 1) * lda];
a[j - 1 + (j - 1)*lda] = qraux[j - 1];
t = -ddot(n - j + 1, a + j - 1 + (j - 1) * lda, 1, qty + j - 1, 1) / a[j - 1 + (j - 1) * lda];
daxpy(n - j + 1, t, a + j - 1 + (j - 1)*lda, 1, qty + j - 1, 1);
a[j - 1 + (j - 1)*lda] = temp;
}
}
}
/*
Set up to compute B, RSD, or AB.
*/
if (cb) {
for (i = 1; i <= k; i++)
b[i - 1] = qty[i - 1];
}
if (cab) {
for (i = 1; i <= k; i++)
ab[i - 1] = qty[i - 1];
}
if (cr && k < n) {
for (i = k + 1; i <= n; i++)
rsd[i - 1] = qty[i - 1];
}
if (cab && k + 1 <= n) {
for (i = k + 1; i <= n; i++)
ab[i - 1] = 0.0;
}
if (cr) {
for (i = 1; i <= k; i++)
rsd[i - 1] = 0.0;
}
/*
Compute B.
*/
if (cb) {
for (jj = 1; jj <= k; jj++) {
j = k - jj + 1;
if (a[j - 1 + (j - 1)*lda] == 0.0) {
info = j;
break;
}
b[j - 1] = b[j - 1] / a[j - 1 + (j - 1) * lda];
if (j != 1) {
t = -b[j - 1];
daxpy(j - 1, t, a + 0 + (j - 1)*lda, 1, b, 1);
}
}
}
/*
Compute RSD or AB as required.
*/
if (cr || cab) {
for (jj = 1; jj <= ju; jj++) {
j = ju - jj + 1;
if (qraux[j - 1] != 0.0) {
temp = a[j - 1 + (j - 1) * lda];
a[j - 1 + (j - 1)*lda] = qraux[j - 1];
if (cr) {
t = -ddot(n - j + 1, a + j - 1 + (j - 1) * lda, 1, rsd + j - 1, 1)
/ a[j - 1 + (j - 1) * lda];
daxpy(n - j + 1, t, a + j - 1 + (j - 1)*lda, 1, rsd + j - 1, 1);
}
if (cab) {
t = -ddot(n - j + 1, a + j - 1 + (j - 1) * lda, 1, ab + j - 1, 1)
/ a[j - 1 + (j - 1) * lda];
daxpy(n - j + 1, t, a + j - 1 + (j - 1)*lda, 1, ab + j - 1, 1);
}
a[j - 1 + (j - 1)*lda] = temp;
}
}
}
return info;
}
/******************************************************************************/
/******************************************************************************/
void dscal(int n, double sa, double x[], int incx)
/******************************************************************************/
/*
Purpose:
DSCAL scales a vector by a constant.
Licensing:
This code is distributed under the GNU LGPL license.
Modified:
30 March 2007
Author:
C version by John Burkardt
Reference:
Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
LINPACK User's Guide,
SIAM, 1979.
Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
Basic Linear Algebra Subprograms for Fortran Usage,
Algorithm 539,
ACM Transactions on Mathematical Software,
Volume 5, Number 3, September 1979, pages 308-323.
Parameters:
Input, int N, the number of entries in the vector.
Input, double SA, the multiplier.
Input/output, double X[*], the vector to be scaled.
Input, int INCX, the increment between successive entries of X.
*/
{
int i;
int ix;
int m;
if (n <= 0) return;
if (incx == 1) {
m = n % 5;
for (i = 0; i < m; i++)
x[i] = sa * x[i];
for (i = m; i < n; i = i + 5) {
x[i] = sa * x[i];
x[i + 1] = sa * x[i + 1];
x[i + 2] = sa * x[i + 2];
x[i + 3] = sa * x[i + 3];
x[i + 4] = sa * x[i + 4];
}
}
else {
if (0 <= incx)
ix = 0;
else
ix = (- n + 1) * incx;
for (i = 0; i < n; i++) {
x[ix] = sa * x[ix];
ix = ix + incx;
}
}
}
/******************************************************************************/
void dswap(int n, double x[], int incx, double y[], int incy)
/******************************************************************************/
/*
Purpose:
DSWAP interchanges two vectors.
Licensing:
This code is distributed under the GNU LGPL license.
Modified:
30 March 2007
Author:
C version by John Burkardt
Reference:
Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
LINPACK User's Guide,
SIAM, 1979.
Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
Basic Linear Algebra Subprograms for Fortran Usage,
Algorithm 539,
ACM Transactions on Mathematical Software,
Volume 5, Number 3, September 1979, pages 308-323.
Parameters:
Input, int N, the number of entries in the vectors.
Input/output, double X[*], one of the vectors to swap.
Input, int INCX, the increment between successive entries of X.
Input/output, double Y[*], one of the vectors to swap.
Input, int INCY, the increment between successive elements of Y.
*/
{
if (n <= 0) return;
int i, ix, iy, m;
double temp;
if (incx == 1 && incy == 1) {
m = n % 3;
for (i = 0; i < m; i++) {
temp = x[i];
x[i] = y[i];
y[i] = temp;
}
for (i = m; i < n; i = i + 3) {
temp = x[i];
x[i] = y[i];
y[i] = temp;
temp = x[i + 1];
x[i + 1] = y[i + 1];
y[i + 1] = temp;
temp = x[i + 2];
x[i + 2] = y[i + 2];
y[i + 2] = temp;
}
}
else {
ix = (incx >= 0) ? 0 : (-n + 1) * incx;
iy = (incy >= 0) ? 0 : (-n + 1) * incy;
for (i = 0; i < n; i++) {
temp = x[ix];
x[ix] = y[iy];
y[iy] = temp;
ix = ix + incx;
iy = iy + incy;
}
}
}
/******************************************************************************/
/******************************************************************************/
void qr_solve(double x[], int m, int n, double a[], double b[])
/******************************************************************************/
/*
Purpose:
QR_SOLVE solves a linear system in the least squares sense.
Discussion:
If the matrix A has full column rank, then the solution X should be the
unique vector that minimizes the Euclidean norm of the residual.
If the matrix A does not have full column rank, then the solution is
not unique; the vector X will minimize the residual norm, but so will
various other vectors.
Licensing:
This code is distributed under the GNU LGPL license.
Modified:
11 September 2012
Author:
John Burkardt
Reference:
David Kahaner, Cleve Moler, Steven Nash,
Numerical Methods and Software,
Prentice Hall, 1989,
ISBN: 0-13-627258-4,
LC: TA345.K34.
Parameters:
Input, int M, the number of rows of A.
Input, int N, the number of columns of A.
Input, double A[M*N], the matrix.
Input, double B[M], the right hand side.
Output, double QR_SOLVE[N], the least squares solution.
*/
{
double a_qr[n * m], qraux[n], r[m], tol;
int ind, itask, jpvt[n], kr, lda;
r8mat_copy(a_qr, m, n, a);
lda = m;
tol = r8_epsilon() / r8mat_amax(m, n, a_qr);
itask = 1;
ind = dqrls ( a_qr, lda, m, n, tol, &kr, b, x, r, jpvt, qraux, itask ); UNUSED(ind);
}
/******************************************************************************/
#endif
MK/module/motion/stepper.cpp
View file @ 55a2c790
...@@ -417,7 +417,7 @@ ISR(TIMER1_COMPA_vect) { ...@@ -417,7 +417,7 @@ ISR(TIMER1_COMPA_vect) {
if (cleaning_buffer_counter) { if (cleaning_buffer_counter) {
current_block = NULL; current_block = NULL;
plan_discard_current_block(); planner.discard_current_block();
#if ENABLED(SD_FINISHED_RELEASECOMMAND) #if ENABLED(SD_FINISHED_RELEASECOMMAND)
if ((cleaning_buffer_counter == 1) && (SD_FINISHED_STEPPERRELEASE)) enqueue_and_echo_commands_P(PSTR(SD_FINISHED_RELEASECOMMAND)); if ((cleaning_buffer_counter == 1) && (SD_FINISHED_STEPPERRELEASE)) enqueue_and_echo_commands_P(PSTR(SD_FINISHED_RELEASECOMMAND));
#endif #endif
...@@ -438,7 +438,7 @@ ISR(TIMER1_COMPA_vect) { ...@@ -438,7 +438,7 @@ ISR(TIMER1_COMPA_vect) {
// If there is no current block, attempt to pop one from the buffer // If there is no current block, attempt to pop one from the buffer
if (!current_block) { if (!current_block) {
// Anything in the buffer? // Anything in the buffer?
current_block = plan_get_current_block(); current_block = planner.get_current_block();
if (current_block) { if (current_block) {
current_block->busy = true; current_block->busy = true;
trapezoid_generator_reset(); trapezoid_generator_reset();
...@@ -651,12 +651,6 @@ ISR(TIMER1_COMPA_vect) { ...@@ -651,12 +651,6 @@ ISR(TIMER1_COMPA_vect) {
#endif #endif
#endif // LASERBEAM #endif // LASERBEAM
// safe check for erroneous calculated events count
if(current_block->step_event_count >= MAX_EVENTS_COUNT) {
kill_current_block();
break;
}
step_events_completed++; step_events_completed++;
if (step_events_completed >= current_block->step_event_count) break; if (step_events_completed >= current_block->step_event_count) break;
} }
...@@ -710,7 +704,6 @@ ISR(TIMER1_COMPA_vect) { ...@@ -710,7 +704,6 @@ ISR(TIMER1_COMPA_vect) {
if (step_rate <= acc_step_rate) { if (step_rate <= acc_step_rate) {
step_rate = acc_step_rate - step_rate; // Decelerate from acceleration end point. step_rate = acc_step_rate - step_rate; // Decelerate from acceleration end point.
// lower limit
NOLESS(step_rate, current_block->final_rate); NOLESS(step_rate, current_block->final_rate);
} }
else { else {
...@@ -763,7 +756,7 @@ ISR(TIMER1_COMPA_vect) { ...@@ -763,7 +756,7 @@ ISR(TIMER1_COMPA_vect) {
// If current block is finished, reset pointer // If current block is finished, reset pointer
if (step_events_completed >= current_block->step_event_count) { if (step_events_completed >= current_block->step_event_count) {
current_block = NULL; current_block = NULL;
plan_discard_current_block(); planner.discard_current_block();
#if ENABLED(LASERBEAM) && ENABLED(LASER_PULSE_METHOD) #if ENABLED(LASERBEAM) && ENABLED(LASER_PULSE_METHOD)
if (current_block->laser_mode == CONTINUOUS && current_block->laser_status == LASER_ON) if (current_block->laser_mode == CONTINUOUS && current_block->laser_status == LASER_ON)
laser_extinguish(); laser_extinguish();
...@@ -1043,7 +1036,7 @@ void st_init() { ...@@ -1043,7 +1036,7 @@ void st_init() {
/** /**
* Block until all buffered steps are executed * Block until all buffered steps are executed
*/ */
void st_synchronize() { while (blocks_queued()) idle(); } void st_synchronize() { while (planner.blocks_queued()) idle(); }
/** /**
* Set the stepper positions directly in steps * Set the stepper positions directly in steps
...@@ -1119,7 +1112,7 @@ float st_get_axis_position_mm(AxisEnum axis) { ...@@ -1119,7 +1112,7 @@ float st_get_axis_position_mm(AxisEnum axis) {
axis_pos = st_get_position(axis); axis_pos = st_get_position(axis);
#endif #endif
return axis_pos / axis_steps_per_unit[axis]; return axis_pos / planner.axis_steps_per_unit[axis];
} }
void enable_all_steppers() { void enable_all_steppers() {
...@@ -1150,7 +1143,7 @@ void finishAndDisableSteppers() { ...@@ -1150,7 +1143,7 @@ void finishAndDisableSteppers() {
void quickStop() { void quickStop() {
cleaning_buffer_counter = 5000; cleaning_buffer_counter = 5000;
DISABLE_STEPPER_DRIVER_INTERRUPT(); DISABLE_STEPPER_DRIVER_INTERRUPT();
while (blocks_queued()) plan_discard_current_block(); while (planner.blocks_queued()) planner.discard_current_block();
current_block = NULL; current_block = NULL;
ENABLE_STEPPER_DRIVER_INTERRUPT(); ENABLE_STEPPER_DRIVER_INTERRUPT();
} }
...@@ -1217,7 +1210,7 @@ void kill_current_block() { ...@@ -1217,7 +1210,7 @@ void kill_current_block() {
} }
float triggered_position_mm(AxisEnum axis) { float triggered_position_mm(AxisEnum axis) {
return endstops_trigsteps[axis] / axis_steps_per_unit[axis]; return endstops_trigsteps[axis] / planner.axis_steps_per_unit[axis];
} }
bool motor_direction(AxisEnum axis) { return TEST(last_direction_bits, axis); } bool motor_direction(AxisEnum axis) { return TEST(last_direction_bits, axis); }
...@@ -1456,7 +1449,7 @@ void microstep_readings() { ...@@ -1456,7 +1449,7 @@ void microstep_readings() {
} }
#if ENABLED(Z_DUAL_ENDSTOPS) #if ENABLED(Z_DUAL_ENDSTOPS)
void In_Homing_Process(bool state) { performing_homing = state; } void set_homing_flag(bool state) { performing_homing = state; }
void Lock_z_motor(bool state) { locked_z_motor = state; } void set_z_lock(bool state) { locked_z_motor = state; }
void Lock_z2_motor(bool state) { locked_z2_motor = state; } void set_z2_lock(bool state) { locked_z2_motor = state; }
#endif #endif
MK/module/motion/stepper.h
View file @ 55a2c790
...@@ -22,6 +22,22 @@ ...@@ -22,6 +22,22 @@
/** /**
* stepper.h - stepper motor driver: executes motion plans of planner.c using the stepper motors * stepper.h - stepper motor driver: executes motion plans of planner.c using the stepper motors
* Part of Grbl
*
* Copyright (c) 2009-2011 Simen Svale Skogsrud
*
* Grbl is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Grbl is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Grbl. If not, see <http://www.gnu.org/licenses/>.
*/ */
#ifndef STEPPER_H #ifndef STEPPER_H
...@@ -107,9 +123,9 @@ ...@@ -107,9 +123,9 @@
void kill_current_block(); void kill_current_block();
#if ENABLED(Z_DUAL_ENDSTOPS) #if ENABLED(Z_DUAL_ENDSTOPS)
void In_Homing_Process(bool state); void set_homing_flag(bool state);
void Lock_z_motor(bool state); void set_z_lock(bool state);
void Lock_z2_motor(bool state); void set_z2_lock(bool state);
#endif #endif
#if ENABLED(BABYSTEPPING) #if ENABLED(BABYSTEPPING)
......
MK/module/motion/stepper_indirection.h
View file @ 55a2c790
...@@ -45,17 +45,19 @@ ...@@ -45,17 +45,19 @@
#define X_ENABLE_READ READ(X_ENABLE_PIN) #define X_ENABLE_READ READ(X_ENABLE_PIN)
// X2 motor // X2 motor
#define X2_STEP_INIT SET_OUTPUT(X2_STEP_PIN) #if ENABLED(DUAL_X_CARRIAGE)
#define X2_STEP_WRITE(STATE) WRITE(X2_STEP_PIN,STATE) #define X2_STEP_INIT SET_OUTPUT(X2_STEP_PIN)
#define X2_STEP_READ READ(X2_STEP_PIN) #define X2_STEP_WRITE(STATE) WRITE(X2_STEP_PIN,STATE)
#define X2_STEP_READ READ(X2_STEP_PIN)
#define X2_DIR_INIT SET_OUTPUT(X2_DIR_PIN) #define X2_DIR_INIT SET_OUTPUT(X2_DIR_PIN)
#define X2_DIR_WRITE(STATE) WRITE(X2_DIR_PIN,STATE) #define X2_DIR_WRITE(STATE) WRITE(X2_DIR_PIN,STATE)
#define X2_DIR_READ READ(X_DIR_PIN) #define X2_DIR_READ READ(X_DIR_PIN)
#define X2_ENABLE_INIT SET_OUTPUT(X2_ENABLE_PIN) #define X2_ENABLE_INIT SET_OUTPUT(X2_ENABLE_PIN)
#define X2_ENABLE_WRITE(STATE) WRITE(X2_ENABLE_PIN,STATE) #define X2_ENABLE_WRITE(STATE) WRITE(X2_ENABLE_PIN,STATE)
#define X2_ENABLE_READ READ(X_ENABLE_PIN) #define X2_ENABLE_READ READ(X_ENABLE_PIN)
#endif // DUAL_X_CARRIAGE
// Y motor // Y motor
#define Y_STEP_INIT SET_OUTPUT(Y_STEP_PIN) #define Y_STEP_INIT SET_OUTPUT(Y_STEP_PIN)
...@@ -71,17 +73,19 @@ ...@@ -71,17 +73,19 @@
#define Y_ENABLE_READ READ(Y_ENABLE_PIN) #define Y_ENABLE_READ READ(Y_ENABLE_PIN)
// Y2 motor // Y2 motor
#define Y2_STEP_INIT SET_OUTPUT(Y2_STEP_PIN) #if ENABLED(Y_DUAL_STEPPER_DRIVERS)
#define Y2_STEP_WRITE(STATE) WRITE(Y2_STEP_PIN,STATE) #define Y2_STEP_INIT SET_OUTPUT(Y2_STEP_PIN)
#define Y2_STEP_READ READ(Y2_STEP_PIN) #define Y2_STEP_WRITE(STATE) WRITE(Y2_STEP_PIN,STATE)
#define Y2_STEP_READ READ(Y2_STEP_PIN)
#define Y2_DIR_INIT SET_OUTPUT(Y2_DIR_PIN) #define Y2_DIR_INIT SET_OUTPUT(Y2_DIR_PIN)
#define Y2_DIR_WRITE(STATE) WRITE(Y2_DIR_PIN,STATE) #define Y2_DIR_WRITE(STATE) WRITE(Y2_DIR_PIN,STATE)
#define Y2_DIR_READ READ(Y2_DIR_PIN) #define Y2_DIR_READ READ(Y2_DIR_PIN)
#define Y2_ENABLE_INIT SET_OUTPUT(Y2_ENABLE_PIN) #define Y2_ENABLE_INIT SET_OUTPUT(Y2_ENABLE_PIN)
#define Y2_ENABLE_WRITE(STATE) WRITE(Y2_ENABLE_PIN,STATE) #define Y2_ENABLE_WRITE(STATE) WRITE(Y2_ENABLE_PIN,STATE)
#define Y2_ENABLE_READ READ(Y2_ENABLE_PIN) #define Y2_ENABLE_READ READ(Y2_ENABLE_PIN)
#endif // Y_DUAL_STEPPER_DRIVERS
// Z motor // Z motor
#define Z_STEP_INIT SET_OUTPUT(Z_STEP_PIN) #define Z_STEP_INIT SET_OUTPUT(Z_STEP_PIN)
...@@ -97,17 +101,19 @@ ...@@ -97,17 +101,19 @@
#define Z_ENABLE_READ READ(Z_ENABLE_PIN) #define Z_ENABLE_READ READ(Z_ENABLE_PIN)
// Z2 motor // Z2 motor
#define Z2_STEP_INIT SET_OUTPUT(Z2_STEP_PIN) #if ENABLED(Z_DUAL_STEPPER_DRIVERS)
#define Z2_STEP_WRITE(STATE) WRITE(Z2_STEP_PIN,STATE) #define Z2_STEP_INIT SET_OUTPUT(Z2_STEP_PIN)
#define Z2_STEP_READ READ(Z2_STEP_PIN) #define Z2_STEP_WRITE(STATE) WRITE(Z2_STEP_PIN,STATE)
#define Z2_STEP_READ READ(Z2_STEP_PIN)
#define Z2_DIR_INIT SET_OUTPUT(Z2_DIR_PIN) #define Z2_DIR_INIT SET_OUTPUT(Z2_DIR_PIN)
#define Z2_DIR_WRITE(STATE) WRITE(Z2_DIR_PIN,STATE) #define Z2_DIR_WRITE(STATE) WRITE(Z2_DIR_PIN,STATE)
#define Z2_DIR_READ READ(Z2_DIR_PIN) #define Z2_DIR_READ READ(Z2_DIR_PIN)
#define Z2_ENABLE_INIT SET_OUTPUT(Z2_ENABLE_PIN) #define Z2_ENABLE_INIT SET_OUTPUT(Z2_ENABLE_PIN)
#define Z2_ENABLE_WRITE(STATE) WRITE(Z2_ENABLE_PIN,STATE) #define Z2_ENABLE_WRITE(STATE) WRITE(Z2_ENABLE_PIN,STATE)
#define Z2_ENABLE_READ READ(Z2_ENABLE_PIN) #define Z2_ENABLE_READ READ(Z2_ENABLE_PIN)
#endif // Z_DUAL_STEPPER_DRIVERS
// E0 motor // E0 motor
#define E0_STEP_INIT SET_OUTPUT(E0_STEP_PIN) #define E0_STEP_INIT SET_OUTPUT(E0_STEP_PIN)
...@@ -374,11 +380,11 @@ ...@@ -374,11 +380,11 @@
// TMC26X drivers have step and dir on normal pins, but everything else via SPI // TMC26X drivers have step and dir on normal pins, but everything else via SPI
////////////////////////////////// //////////////////////////////////
#if ENABLED(HAVE_TMCDRIVER) #if ENABLED(HAVE_TMCDRIVER)
#include <SPI.h> #include <SPI.h>
#include <TMC26XStepper.h> #include <TMC26XStepper.h>
void tmc_init(); void tmc_init();
#if ENABLED(X_IS_TMC) #if ENABLED(X_IS_TMC)
extern TMC26XStepper stepperX; extern TMC26XStepper stepperX;
#undef X_ENABLE_INIT #undef X_ENABLE_INIT
#define X_ENABLE_INIT ((void)0) #define X_ENABLE_INIT ((void)0)
...@@ -389,8 +395,8 @@ ...@@ -389,8 +395,8 @@
#undef X_ENABLE_READ #undef X_ENABLE_READ
#define X_ENABLE_READ stepperX.isEnabled() #define X_ENABLE_READ stepperX.isEnabled()
#endif #endif
#if ENABLED(X2_IS_TMC) #if ENABLED(X2_IS_TMC)
extern TMC26XStepper stepperX2; extern TMC26XStepper stepperX2;
#undef X2_ENABLE_INIT #undef X2_ENABLE_INIT
#define X2_ENABLE_INIT ((void)0) #define X2_ENABLE_INIT ((void)0)
...@@ -400,8 +406,8 @@ ...@@ -400,8 +406,8 @@
#undef X2_ENABLE_READ #undef X2_ENABLE_READ
#define X2_ENABLE_READ stepperX2.isEnabled() #define X2_ENABLE_READ stepperX2.isEnabled()
#endif #endif
#if ENABLED(Y_IS_TMC) #if ENABLED(Y_IS_TMC)
extern TMC26XStepper stepperY; extern TMC26XStepper stepperY;
#undef Y_ENABLE_INIT #undef Y_ENABLE_INIT
#define Y_ENABLE_INIT ((void)0) #define Y_ENABLE_INIT ((void)0)
...@@ -411,8 +417,8 @@ ...@@ -411,8 +417,8 @@
#undef Y_ENABLE_READ #undef Y_ENABLE_READ
#define Y_ENABLE_READ stepperY.isEnabled() #define Y_ENABLE_READ stepperY.isEnabled()
#endif #endif
#if ENABLED(Y2_IS_TMC) #if ENABLED(Y2_IS_TMC)
extern TMC26XStepper stepperY2; extern TMC26XStepper stepperY2;
#undef Y2_ENABLE_INIT #undef Y2_ENABLE_INIT
#define Y2_ENABLE_INIT ((void)0) #define Y2_ENABLE_INIT ((void)0)
...@@ -422,8 +428,8 @@ ...@@ -422,8 +428,8 @@
#undef Y2_ENABLE_READ #undef Y2_ENABLE_READ
#define Y2_ENABLE_READ stepperY2.isEnabled() #define Y2_ENABLE_READ stepperY2.isEnabled()
#endif #endif
#if ENABLED(Z_IS_TMC) #if ENABLED(Z_IS_TMC)
extern TMC26XStepper stepperZ; extern TMC26XStepper stepperZ;
#undef Z_ENABLE_INIT #undef Z_ENABLE_INIT
#define Z_ENABLE_INIT ((void)0) #define Z_ENABLE_INIT ((void)0)
...@@ -433,8 +439,8 @@ ...@@ -433,8 +439,8 @@
#undef Z_ENABLE_READ #undef Z_ENABLE_READ
#define Z_ENABLE_READ stepperZ.isEnabled() #define Z_ENABLE_READ stepperZ.isEnabled()
#endif #endif
#if ENABLED(Z2_IS_TMC) #if ENABLED(Z2_IS_TMC)
extern TMC26XStepper stepperZ2; extern TMC26XStepper stepperZ2;
#undef Z2_ENABLE_INIT #undef Z2_ENABLE_INIT
#define Z2_ENABLE_INIT ((void)0) #define Z2_ENABLE_INIT ((void)0)
...@@ -444,8 +450,8 @@ ...@@ -444,8 +450,8 @@
#undef Z2_ENABLE_READ #undef Z2_ENABLE_READ
#define Z2_ENABLE_READ stepperZ2.isEnabled() #define Z2_ENABLE_READ stepperZ2.isEnabled()
#endif #endif
#if ENABLED(E0_IS_TMC) #if ENABLED(E0_IS_TMC)
extern TMC26XStepper stepperE0; extern TMC26XStepper stepperE0;
#undef E0_ENABLE_INIT #undef E0_ENABLE_INIT
#define E0_ENABLE_INIT ((void)0) #define E0_ENABLE_INIT ((void)0)
...@@ -455,8 +461,8 @@ ...@@ -455,8 +461,8 @@
#undef E0_ENABLE_READ #undef E0_ENABLE_READ
#define E0_ENABLE_READ stepperE0.isEnabled() #define E0_ENABLE_READ stepperE0.isEnabled()
#endif #endif
#if ENABLED(E1_IS_TMC) #if ENABLED(E1_IS_TMC)
extern TMC26XStepper stepperE1; extern TMC26XStepper stepperE1;
#undef E1_ENABLE_INIT #undef E1_ENABLE_INIT
#define E1_ENABLE_INIT ((void)0) #define E1_ENABLE_INIT ((void)0)
...@@ -466,8 +472,8 @@ ...@@ -466,8 +472,8 @@
#undef E1_ENABLE_READ #undef E1_ENABLE_READ
#define E1_ENABLE_READ stepperE1.isEnabled() #define E1_ENABLE_READ stepperE1.isEnabled()
#endif #endif
#if ENABLED(E2_IS_TMC) #if ENABLED(E2_IS_TMC)
extern TMC26XStepper stepperE2; extern TMC26XStepper stepperE2;
#undef E2_ENABLE_INIT #undef E2_ENABLE_INIT
#define E2_ENABLE_INIT ((void)0) #define E2_ENABLE_INIT ((void)0)
...@@ -477,8 +483,8 @@ ...@@ -477,8 +483,8 @@
#undef E2_ENABLE_READ #undef E2_ENABLE_READ
#define E2_ENABLE_READ stepperE2.isEnabled() #define E2_ENABLE_READ stepperE2.isEnabled()
#endif #endif
#if ENABLED(E3_IS_TMC) #if ENABLED(E3_IS_TMC)
extern TMC26XStepper stepperE3; extern TMC26XStepper stepperE3;
#undef E3_ENABLE_INIT #undef E3_ENABLE_INIT
#define E3_ENABLE_INIT ((void)0) #define E3_ENABLE_INIT ((void)0)
...@@ -488,7 +494,7 @@ ...@@ -488,7 +494,7 @@
#undef E3_ENABLE_READ #undef E3_ENABLE_READ
#define E3_ENABLE_READ stepperE3.isEnabled() #define E3_ENABLE_READ stepperE3.isEnabled()
#endif #endif
#endif // HAVE_TMCDRIVER #endif // HAVE_TMCDRIVER
...@@ -498,11 +504,11 @@ ...@@ -498,11 +504,11 @@
////////////////////////////////// //////////////////////////////////
#if ENABLED(HAVE_L6470DRIVER) #if ENABLED(HAVE_L6470DRIVER)
#include <SPI.h> #include <SPI.h>
#include <L6470.h> #include <L6470.h>
void L6470_init(); void L6470_init();
#if ENABLED(X_IS_L6470) #if ENABLED(X_IS_L6470)
extern L6470 stepperX; extern L6470 stepperX;
#undef X_ENABLE_INIT #undef X_ENABLE_INIT
#define X_ENABLE_INIT ((void)0) #define X_ENABLE_INIT ((void)0)
...@@ -522,8 +528,8 @@ ...@@ -522,8 +528,8 @@
#undef X_DIR_READ #undef X_DIR_READ
#define X_DIR_READ (stepperX.getStatus() & STATUS_DIR) #define X_DIR_READ (stepperX.getStatus() & STATUS_DIR)
#endif #endif
#if ENABLED(X2_IS_L6470) #if ENABLED(X2_IS_L6470)
extern L6470 stepperX2; extern L6470 stepperX2;
#undef X2_ENABLE_INIT #undef X2_ENABLE_INIT
#define X2_ENABLE_INIT ((void)0) #define X2_ENABLE_INIT ((void)0)
...@@ -542,8 +548,8 @@ ...@@ -542,8 +548,8 @@
#undef X2_DIR_READ #undef X2_DIR_READ
#define X2_DIR_READ (stepperX2.getStatus() & STATUS_DIR) #define X2_DIR_READ (stepperX2.getStatus() & STATUS_DIR)
#endif #endif
#if ENABLED(Y_IS_L6470) #if ENABLED(Y_IS_L6470)
extern L6470 stepperY; extern L6470 stepperY;
#undef Y_ENABLE_INIT #undef Y_ENABLE_INIT
#define Y_ENABLE_INIT ((void)0) #define Y_ENABLE_INIT ((void)0)
...@@ -562,8 +568,8 @@ ...@@ -562,8 +568,8 @@
#undef Y_DIR_READ #undef Y_DIR_READ
#define Y_DIR_READ (stepperY.getStatus() & STATUS_DIR) #define Y_DIR_READ (stepperY.getStatus() & STATUS_DIR)
#endif #endif
#if ENABLED(Y2_IS_L6470) #if ENABLED(Y2_IS_L6470)
extern L6470 stepperY2; extern L6470 stepperY2;
#undef Y2_ENABLE_INIT #undef Y2_ENABLE_INIT
#define Y2_ENABLE_INIT ((void)0) #define Y2_ENABLE_INIT ((void)0)
...@@ -582,8 +588,8 @@ ...@@ -582,8 +588,8 @@
#undef Y2_DIR_READ #undef Y2_DIR_READ
#define Y2_DIR_READ (stepperY2.getStatus() & STATUS_DIR) #define Y2_DIR_READ (stepperY2.getStatus() & STATUS_DIR)
#endif #endif
#if ENABLED(Z_IS_L6470) #if ENABLED(Z_IS_L6470)
extern L6470 stepperZ; extern L6470 stepperZ;
#undef Z_ENABLE_INIT #undef Z_ENABLE_INIT
#define Z_ENABLE_INIT ((void)0) #define Z_ENABLE_INIT ((void)0)
...@@ -602,8 +608,8 @@ ...@@ -602,8 +608,8 @@
#undef Y_DIR_READ #undef Y_DIR_READ
#define Y_DIR_READ (stepperZ.getStatus() & STATUS_DIR) #define Y_DIR_READ (stepperZ.getStatus() & STATUS_DIR)
#endif #endif
#if ENABLED(Z2_IS_L6470) #if ENABLED(Z2_IS_L6470)
extern L6470 stepperZ2; extern L6470 stepperZ2;
#undef Z2_ENABLE_INIT #undef Z2_ENABLE_INIT
#define Z2_ENABLE_INIT ((void)0) #define Z2_ENABLE_INIT ((void)0)
...@@ -622,8 +628,8 @@ ...@@ -622,8 +628,8 @@
#undef Y2_DIR_READ #undef Y2_DIR_READ
#define Y2_DIR_READ (stepperZ2.getStatus() & STATUS_DIR) #define Y2_DIR_READ (stepperZ2.getStatus() & STATUS_DIR)
#endif #endif
#if ENABLED(E0_IS_L6470) #if ENABLED(E0_IS_L6470)
extern L6470 stepperE0; extern L6470 stepperE0;
#undef E0_ENABLE_INIT #undef E0_ENABLE_INIT
#define E0_ENABLE_INIT ((void)0) #define E0_ENABLE_INIT ((void)0)
...@@ -642,8 +648,8 @@ ...@@ -642,8 +648,8 @@
#undef E0_DIR_READ #undef E0_DIR_READ
#define E0_DIR_READ (stepperE0.getStatus() & STATUS_DIR) #define E0_DIR_READ (stepperE0.getStatus() & STATUS_DIR)
#endif #endif
#if ENABLED(E1_IS_L6470) #if ENABLED(E1_IS_L6470)
extern L6470 stepperE1; extern L6470 stepperE1;
#undef E1_ENABLE_INIT #undef E1_ENABLE_INIT
#define E1_ENABLE_INIT ((void)0) #define E1_ENABLE_INIT ((void)0)
...@@ -662,8 +668,8 @@ ...@@ -662,8 +668,8 @@
#undef E1_DIR_READ #undef E1_DIR_READ
#define E1_DIR_READ (stepperE1.getStatus() & STATUS_DIR) #define E1_DIR_READ (stepperE1.getStatus() & STATUS_DIR)
#endif #endif
#if ENABLED(E2_IS_L6470) #if ENABLED(E2_IS_L6470)
extern L6470 stepperE2; extern L6470 stepperE2;
#undef E2_ENABLE_INIT #undef E2_ENABLE_INIT
#define E2_ENABLE_INIT ((void)0) #define E2_ENABLE_INIT ((void)0)
...@@ -682,8 +688,8 @@ ...@@ -682,8 +688,8 @@
#undef E2_DIR_READ #undef E2_DIR_READ
#define E2_DIR_READ (stepperE2.getStatus() & STATUS_DIR) #define E2_DIR_READ (stepperE2.getStatus() & STATUS_DIR)
#endif #endif
#if ENABLED(E3_IS_L6470) #if ENABLED(E3_IS_L6470)
extern L6470 stepperE3; extern L6470 stepperE3;
#undef E3_ENABLE_INIT #undef E3_ENABLE_INIT
#define E3_ENABLE_INIT ((void)0) #define E3_ENABLE_INIT ((void)0)
...@@ -702,7 +708,7 @@ ...@@ -702,7 +708,7 @@
#undef E3_DIR_READ #undef E3_DIR_READ
#define E3_DIR_READ (stepperE3.getStatus() & STATUS_DIR) #define E3_DIR_READ (stepperE3.getStatus() & STATUS_DIR)
#endif #endif
#endif //HAVE_L6470DRIVER #endif //HAVE_L6470DRIVER
......
MK/module/motion/vector_3.cpp deleted 100644 → 0
View file @ 18664abd
/**
* MK & MK4due 3D Printer Firmware
*
* Based on Marlin, Sprinter and grbl
* Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
* Copyright (C) 2013 - 2016 Alberto Cotronei @MagoKimbra
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
*/
/**
* vector_3.cpp - Vector library for bed leveling
* Copyright (c) 2012 Lars Brubaker. All right reserved.
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/
#include "../../base.h"
#if ENABLED(AUTO_BED_LEVELING_FEATURE)
#include <math.h>
#include "vector_3.h"
vector_3::vector_3() : x(0), y(0), z(0) { }
vector_3::vector_3(float x_, float y_, float z_) : x(x_), y(y_), z(z_) { }
vector_3 vector_3::cross(vector_3 left, vector_3 right) {
return vector_3(left.y * right.z - left.z * right.y,
left.z * right.x - left.x * right.z,
left.x * right.y - left.y * right.x);
}
vector_3 vector_3::operator+(vector_3 v) { return vector_3((x + v.x), (y + v.y), (z + v.z)); }
vector_3 vector_3::operator-(vector_3 v) { return vector_3((x - v.x), (y - v.y), (z - v.z)); }
vector_3 vector_3::get_normal() {
vector_3 normalized = vector_3(x, y, z);
normalized.normalize();
return normalized;
}
float vector_3::get_length() { return sqrt((x * x) + (y * y) + (z * z)); }
void vector_3::normalize() {
float length = get_length();
x /= length;
y /= length;
z /= length;
}
void vector_3::apply_rotation(matrix_3x3 matrix) {
float resultX = x * matrix.matrix[0][0] + y * matrix.matrix[1][0] + z * matrix.matrix[2][0];
float resultY = x * matrix.matrix[0][1] + y * matrix.matrix[1][1] + z * matrix.matrix[2][1];
float resultZ = x * matrix.matrix[0][2] + y * matrix.matrix[1][2] + z * matrix.matrix[2][2];
x = resultX;
y = resultY;
z = resultZ;
}
void vector_3::debug(const char title[]) {
ECHO_ST(DB, title);
ECHO_MV(" x: ", x, 6);
ECHO_MV(" y: ", y, 6);
ECHO_EMV(" z: ", z, 6);
}
void apply_rotation_xyz(matrix_3x3 matrix, float& x, float& y, float& z) {
vector_3 vector = vector_3(x, y, z);
vector.apply_rotation(matrix);
x = vector.x;
y = vector.y;
z = vector.z;
}
matrix_3x3 matrix_3x3::create_from_rows(vector_3 row_0, vector_3 row_1, vector_3 row_2) {
//row_0.debug("row_0");
//row_1.debug("row_1");
//row_2.debug("row_2");
matrix_3x3 new_matrix;
new_matrix.matrix[0][0] = row_0.x; new_matrix.matrix[0][1] = row_0.y; new_matrix.matrix[0][2] = row_0.z;
new_matrix.matrix[1][0] = row_1.x; new_matrix.matrix[1][1] = row_1.y; new_matrix.matrix[1][2] = row_1.z;
new_matrix.matrix[2][0] = row_2.x; new_matrix.matrix[2][1] = row_2.y; new_matrix.matrix[2][2] = row_2.z;
//new_matrix.debug("new_matrix");
return new_matrix;
}
void matrix_3x3::set_to_identity() {
matrix[0][0] = 1; matrix[0][1] = 0; matrix[0][2] = 0;
matrix[1][0] = 0; matrix[1][1] = 1; matrix[1][2] = 0;
matrix[2][0] = 0; matrix[2][1] = 0; matrix[2][2] = 1;
}
matrix_3x3 matrix_3x3::create_look_at(vector_3 target) {
vector_3 z_row = target.get_normal();
vector_3 x_row = vector_3(1, 0, -target.x / target.z).get_normal();
vector_3 y_row = vector_3::cross(z_row, x_row).get_normal();
// x_row.debug("x_row");
// y_row.debug("y_row");
// z_row.debug("z_row");
// create the matrix already correctly transposed
matrix_3x3 rot = matrix_3x3::create_from_rows(x_row, y_row, z_row);
// rot.debug("rot");
return rot;
}
matrix_3x3 matrix_3x3::transpose(matrix_3x3 original) {
matrix_3x3 new_matrix;
new_matrix.matrix[0][0] = original.matrix[0][0]; new_matrix.matrix[0][1] = original.matrix[1][0]; new_matrix.matrix[0][2] = original.matrix[2][0];
new_matrix.matrix[1][0] = original.matrix[0][1]; new_matrix.matrix[1][1] = original.matrix[1][1]; new_matrix.matrix[1][2] = original.matrix[2][1];
new_matrix.matrix[2][0] = original.matrix[0][2]; new_matrix.matrix[2][1] = original.matrix[1][2]; new_matrix.matrix[2][2] = original.matrix[2][2];
return new_matrix;
}
void matrix_3x3::debug(const char title[]) {
ECHO_LT(DB, title);
for (uint8_t i = 0; i < 3; i++) {
ECHO_S(DB);
for (uint8_t j = 0; j < 3; j++) {
if (matrix[i][j] >= 0.0) ECHO_C('+');
ECHO_V(matrix[i][j], 6);
ECHO_C(' ');
}
ECHO_E;
}
}
#endif // AUTO_BED_LEVELING_FEATURE
MK/module/motion/planner.cpp → MK/module/planner/planner.cpp
View file @ 55a2c790
...@@ -21,24 +21,12 @@ ...@@ -21,24 +21,12 @@
*/ */
/** /**
* planner.cpp - Buffer movement commands and manage the acceleration profile plan * planner.cpp
* Part of Grbl
* *
* Copyright (c) 2009-2011 Simen Svale Skogsrud * Buffer movement commands and manage the acceleration profile plan
*
* Grbl is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Grbl is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Grbl. If not, see <http://www.gnu.org/licenses/>.
* *
* Derived from Grbl
* Copyright (c) 2009-2011 Simen Svale Skogsrud
* *
* The ring buffer implementation gleaned from the wiring_serial library by David A. Mellis. * The ring buffer implementation gleaned from the wiring_serial library by David A. Mellis.
* *
...@@ -71,110 +59,88 @@ ...@@ -71,110 +59,88 @@
*/ */
#include "../../base.h" #include "../../base.h"
#include "planner.h"
Planner planner;
//===========================================================================
//============================= public variables ============================ // public:
//===========================================================================
/**
millis_t minsegmenttime; * A ring buffer of moves described in steps
float max_feedrate[3 + EXTRUDERS]; // Max speeds in mm per minute */
float axis_steps_per_unit[3 + EXTRUDERS]; block_t Planner::block_buffer[BLOCK_BUFFER_SIZE];
unsigned long max_acceleration_units_per_sq_second[3 + EXTRUDERS]; // Use M201 to override by software volatile uint8_t Planner::block_buffer_head = 0; // Index of the next block to be pushed
float minimumfeedrate; volatile uint8_t Planner::block_buffer_tail = 0;
float acceleration; // Normal acceleration mm/s^2 DEFAULT ACCELERATION for all printing moves. M204 SXXXX
float retract_acceleration[EXTRUDERS]; // mm/s^2 filament pull-back and push-forward while standing still in the other axes M204 TXXXX float Planner::max_feedrate[3 + EXTRUDERS]; // Max speeds in mm per minute
float travel_acceleration; // Travel acceleration mm/s^2 DEFAULT ACCELERATION for all NON printing moves. M204 MXXXX float Planner::axis_steps_per_unit[3 + EXTRUDERS];
float max_xy_jerk; // The largest speed change requiring no acceleration unsigned long Planner::axis_steps_per_sqr_second[3 + EXTRUDERS];
float max_z_jerk; unsigned long Planner::max_acceleration_units_per_sq_second[3 + EXTRUDERS]; // Use M201 to override by software
float max_e_jerk[EXTRUDERS]; // mm/s - initial speed for extruder retract moves
float mintravelfeedrate; millis_t Planner::min_segment_time;
unsigned long axis_steps_per_sqr_second[3 + EXTRUDERS]; float Planner::min_feedrate;
uint8_t last_extruder; float Planner::acceleration; // Normal acceleration mm/s^2 DEFAULT ACCELERATION for all printing moves. M204 SXXXX
float Planner::retract_acceleration[EXTRUDERS]; // Retract acceleration mm/s^2 filament pull-back and push-forward while standing still in the other axes M204 TXXXX
float Planner::travel_acceleration; // Travel acceleration mm/s^2 DEFAULT ACCELERATION for all NON printing moves. M204 MXXXX
float Planner::max_xy_jerk; // The largest speed change requiring no acceleration
float Planner::max_z_jerk;
float Planner::max_e_jerk[EXTRUDERS];
float Planner::min_travel_feedrate;
#if ENABLED(AUTO_BED_LEVELING_FEATURE) #if ENABLED(AUTO_BED_LEVELING_FEATURE)
// Transform required to compensate for bed level matrix_3x3 Planner::bed_level_matrix; // Transform to compensate for bed level
matrix_3x3 plan_bed_level_matrix = { #endif
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0
};
#endif // AUTO_BED_LEVELING_FEATURE
#if ENABLED(AUTOTEMP) #if ENABLED(AUTOTEMP)
float autotemp_max = 250; float Planner::autotemp_max = 250;
float autotemp_min = 210; float Planner::autotemp_min = 210;
float autotemp_factor = 0.1; float Planner::autotemp_factor = 0.1;
bool autotemp_enabled = false; bool Planner::autotemp_enabled = false;
#endif #endif
//=========================================================================== // private:
//============ semi-private variables, used in inline functions =============
//===========================================================================
block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instfructions long Planner::position[NUM_AXIS] = { 0 };
volatile unsigned char block_buffer_head; // Index of the next block to be pushed
volatile unsigned char block_buffer_tail; // Index of the block to process now
//=========================================================================== float Planner::previous_speed[NUM_AXIS];
//============================ private variables ============================
//===========================================================================
// The current position of the tool in absolute steps float Planner::previous_nominal_speed;
long position[NUM_AXIS]; // Rescaled from extern when axis_steps_per_unit are changed by gcode
static float previous_speed[NUM_AXIS]; // Speed of previous path line segment
static float previous_nominal_speed; // Nominal speed of previous path line segment
uint8_t g_uc_extruder_last_move[EXTRUDERS] = { 0 }; uint8_t Planner::last_extruder;
#if ENABLED(DISABLE_INACTIVE_EXTRUDER)
uint8_t Planner::g_uc_extruder_last_move[EXTRUDERS] = { 0 };
#endif // DISABLE_INACTIVE_EXTRUDER
#if ENABLED(XY_FREQUENCY_LIMIT) #if ENABLED(XY_FREQUENCY_LIMIT)
// Used for the frequency limit
#define MAX_FREQ_TIME (1000000.0/XY_FREQUENCY_LIMIT)
// Old direction bits. Used for speed calculations // Old direction bits. Used for speed calculations
static unsigned char old_direction_bits = 0; unsigned char Planner::old_direction_bits = 0;
// Segment times (in µs). Used for speed calculations // Segment times (in µs). Used for speed calculations
static long axis_segment_time[2][3] = { {MAX_FREQ_TIME + 1, 0, 0}, {MAX_FREQ_TIME + 1, 0, 0} }; long Planner::axis_segment_time[2][3] = { {MAX_FREQ_TIME + 1, 0, 0}, {MAX_FREQ_TIME + 1, 0, 0} };
#endif
#if ENABLED(FILAMENT_SENSOR)
static char meas_sample; // temporary variable to hold filament measurement sample
#endif
#if ENABLED(DUAL_X_CARRIAGE)
extern bool extruder_duplication_enabled;
#endif #endif
//=========================================================================== /**
//================================ functions ================================ * Class and Instance Methods
//=========================================================================== */
// Get the next / previous index of the next block in the ring buffer
// NOTE: Using & here (not %) because BLOCK_BUFFER_SIZE is always a power of 2
FORCE_INLINE int8_t next_block_index(int8_t block_index) { return BLOCK_MOD(block_index + 1); }
FORCE_INLINE int8_t prev_block_index(int8_t block_index) { return BLOCK_MOD(block_index - 1); }
// Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the Planner::Planner() {
// given acceleration: #if ENABLED(AUTO_BED_LEVELING_FEATURE)
FORCE_INLINE float estimate_acceleration_distance(float initial_rate, float target_rate, float acceleration) { bed_level_matrix.set_to_identity();
if (acceleration == 0) return 0; // acceleration was 0, set acceleration distance to 0 #endif
return (target_rate * target_rate - initial_rate * initial_rate) / (acceleration * 2); init();
} }
// This function gives you the point at which you must start braking (at the rate of -acceleration) if void Planner::init() {
// you started at speed initial_rate and accelerated until this point and want to end at the final_rate after block_buffer_head = block_buffer_tail = 0;
// a total travel of distance. This can be used to compute the intersection point between acceleration and memset(position, 0, sizeof(position)); // clear position
// deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed) for (int i = 0; i < NUM_AXIS; i++) previous_speed[i] = 0.0;
previous_nominal_speed = 0.0;
FORCE_INLINE float intersection_distance(float initial_rate, float final_rate, float acceleration, float distance) {
if (acceleration == 0) return 0; // acceleration was 0, set intersection distance to 0
return (acceleration * 2 * distance - initial_rate * initial_rate + final_rate * final_rate) / (acceleration * 4);
} }
/** /**
* Calculate trapezoid parameters, multiplying the entry- and exit-speeds * Calculate trapezoid parameters, multiplying the entry- and exit-speeds
* by the provided factors. * by the provided factors.
*/ */
void calculate_trapezoid_for_block(block_t* block, float entry_factor, float exit_factor) { void Planner::calculate_trapezoid_for_block(block_t* block, float entry_factor, float exit_factor) {
unsigned long initial_rate = ceil(block->nominal_rate * entry_factor), unsigned long initial_rate = ceil(block->nominal_rate * entry_factor),
final_rate = ceil(block->nominal_rate * exit_factor); // (steps per second) final_rate = ceil(block->nominal_rate * exit_factor); // (steps per second)
...@@ -220,25 +186,8 @@ void calculate_trapezoid_for_block(block_t* block, float entry_factor, float exi ...@@ -220,25 +186,8 @@ void calculate_trapezoid_for_block(block_t* block, float entry_factor, float exi
CRITICAL_SECTION_END; CRITICAL_SECTION_END;
} }
/** // The kernel called by recalculate() when scanning the plan from last to first entry.
* Calculate the maximum allowable speed at this point, in order void Planner::reverse_pass_kernel(block_t* previous, block_t* current, block_t* next) {
* to reach 'target_velocity' using 'acceleration' within a given
* 'distance'.
*/
FORCE_INLINE float max_allowable_speed(float acceleration, float target_velocity, float distance) {
return sqrt(target_velocity * target_velocity - 2 * acceleration * distance);
}
// "Junction jerk" in this context is the immediate change in speed at the junction of two blocks.
// This method will calculate the junction jerk as the euclidean distance between the nominal
// velocities of the respective blocks.
// inline float junction_jerk(block_t *before, block_t *after) {
// return sqrt(
// pow((before->speed_x-after->speed_x), 2)+pow((before->speed_y-after->speed_y), 2));
//}
// The kernel called by planner_recalculate() when scanning the plan from last to first entry.
void planner_reverse_pass_kernel(block_t* previous, block_t* current, block_t* next) {
if (!current) return; if (!current) return;
UNUSED(previous); UNUSED(previous);
...@@ -268,13 +217,13 @@ void planner_reverse_pass_kernel(block_t* previous, block_t* current, block_t* n ...@@ -268,13 +217,13 @@ void planner_reverse_pass_kernel(block_t* previous, block_t* current, block_t* n
* recalculate() needs to go over the current plan twice. * recalculate() needs to go over the current plan twice.
* Once in reverse and once forward. This implements the reverse pass. * Once in reverse and once forward. This implements the reverse pass.
*/ */
void planner_reverse_pass() { void Planner::reverse_pass() {
if (movesplanned() > 3) { if (movesplanned() > 3) {
block_t* block[3] = { NULL, NULL, NULL }; block_t* block[3] = { NULL, NULL, NULL };
//Make a local copy of block_buffer_tail, because the interrupt can alter it // Make a local copy of block_buffer_tail, because the interrupt can alter it
CRITICAL_SECTION_START; CRITICAL_SECTION_START;
uint8_t tail = block_buffer_tail; uint8_t tail = block_buffer_tail;
CRITICAL_SECTION_END CRITICAL_SECTION_END
...@@ -285,13 +234,13 @@ void planner_reverse_pass() { ...@@ -285,13 +234,13 @@ void planner_reverse_pass() {
block[2] = block[1]; block[2] = block[1];
block[1] = block[0]; block[1] = block[0];
block[0] = &block_buffer[b]; block[0] = &block_buffer[b];
planner_reverse_pass_kernel(block[0], block[1], block[2]); reverse_pass_kernel(block[0], block[1], block[2]);
} }
} }
} }
// The kernel called by planner_recalculate() when scanning the plan from first to last entry. // The kernel called by recalculate() when scanning the plan from first to last entry.
void planner_forward_pass_kernel(block_t* previous, block_t* current, block_t* next) { void Planner::forward_pass_kernel(block_t* previous, block_t* current, block_t* next) {
if (!previous) return; if (!previous) return;
UNUSED(next); UNUSED(next);
...@@ -316,16 +265,16 @@ void planner_forward_pass_kernel(block_t* previous, block_t* current, block_t* n ...@@ -316,16 +265,16 @@ void planner_forward_pass_kernel(block_t* previous, block_t* current, block_t* n
* recalculate() needs to go over the current plan twice. * recalculate() needs to go over the current plan twice.
* Once in reverse and once forward. This implements the forward pass. * Once in reverse and once forward. This implements the forward pass.
*/ */
void planner_forward_pass() { void Planner::forward_pass() {
block_t* block[3] = { NULL, NULL, NULL }; block_t* block[3] = { NULL, NULL, NULL };
for (uint8_t b = block_buffer_tail; b != block_buffer_head; b = next_block_index(b)) { for (uint8_t b = block_buffer_tail; b != block_buffer_head; b = next_block_index(b)) {
block[0] = block[1]; block[0] = block[1];
block[1] = block[2]; block[1] = block[2];
block[2] = &block_buffer[b]; block[2] = &block_buffer[b];
planner_forward_pass_kernel(block[0], block[1], block[2]); forward_pass_kernel(block[0], block[1], block[2]);
} }
planner_forward_pass_kernel(block[1], block[2], NULL); forward_pass_kernel(block[1], block[2], NULL);
} }
/** /**
...@@ -333,7 +282,7 @@ void planner_forward_pass() { ...@@ -333,7 +282,7 @@ void planner_forward_pass() {
* according to the entry_factor for each junction. Must be called by * according to the entry_factor for each junction. Must be called by
* recalculate() after updating the blocks. * recalculate() after updating the blocks.
*/ */
void planner_recalculate_trapezoids() { void Planner::recalculate_trapezoids() {
int8_t block_index = block_buffer_tail; int8_t block_index = block_buffer_tail;
block_t* current; block_t* current;
block_t* next = NULL; block_t* next = NULL;
...@@ -360,53 +309,52 @@ void planner_recalculate_trapezoids() { ...@@ -360,53 +309,52 @@ void planner_recalculate_trapezoids() {
} }
} }
// Recalculates the motion plan according to the following algorithm: /**
// * Recalculate the motion plan according to the following algorithm:
// 1. Go over every block in reverse order and calculate a junction speed reduction (i.e. block_t.entry_factor) *
// so that: * 1. Go over every block in reverse order...
// a. The junction jerk is within the set limit *
// b. No speed reduction within one block requires faster deceleration than the one, true constant * Calculate a junction speed reduction (block_t.entry_factor) so:
// acceleration. *
// 2. Go over every block in chronological order and dial down junction speed reduction values if * a. The junction jerk is within the set limit, and
// a. The speed increase within one block would require faster acceleration than the one, true *
// constant acceleration. * b. No speed reduction within one block requires faster
// * deceleration than the one, true constant acceleration.
// When these stages are complete all blocks have an entry_factor that will allow all speed changes to *
// be performed using only the one, true constant acceleration, and where no junction jerk is jerkier than * 2. Go over every block in chronological order...
// the set limit. Finally it will: *
// * Dial down junction speed reduction values if:
// 3. Recalculate trapezoids for all blocks. * a. The speed increase within one block would require faster
* acceleration than the one, true constant acceleration.
void planner_recalculate() { *
planner_reverse_pass(); * After that, all blocks will have an entry_factor allowing all speed changes to
planner_forward_pass(); * be performed using only the one, true constant acceleration, and where no junction
planner_recalculate_trapezoids(); * jerk is jerkier than the set limit, Jerky. Finally it will:
*
* 3. Recalculate "trapezoids" for all blocks.
*/
void Planner::recalculate() {
reverse_pass();
forward_pass();
recalculate_trapezoids();
} }
void plan_init() {
block_buffer_head = block_buffer_tail = 0;
memset(position, 0, sizeof(position)); // clear position
for (int i = 0; i < NUM_AXIS; i++) previous_speed[i] = 0.0;
previous_nominal_speed = 0.0;
}
#if ENABLED(AUTOTEMP) #if ENABLED(AUTOTEMP)
void getHighESpeed() {
void Planner::getHighESpeed() {
static float oldt = 0; static float oldt = 0;
if (!autotemp_enabled) return; if (!autotemp_enabled) return;
if (degTargetHotend0() + 2 < autotemp_min) return; // probably temperature set to zero. if (degTargetHotend(0) + 2 < autotemp_min) return; // probably temperature set to zero.
float high = 0.0; float high = 0.0;
uint8_t block_index = block_buffer_tail; for (uint8_t b = block_buffer_tail; b != block_buffer_head; b = next_block_index(b)) {
block_t* block = &block_buffer[b];
while (block_index != block_buffer_head) {
block_t* block = &block_buffer[block_index];
if (block->steps[X_AXIS] || block->steps[Y_AXIS] || block->steps[Z_AXIS]) { if (block->steps[X_AXIS] || block->steps[Y_AXIS] || block->steps[Z_AXIS]) {
float se = (float)block->steps[E_AXIS] / block->step_event_count * block->nominal_speed; // mm/sec; float se = (float)block->steps[E_AXIS] / block->step_event_count * block->nominal_speed; // mm/sec;
NOLESS(high, se); NOLESS(high, se);
} }
block_index = next_block_index(block_index);
} }
float t = autotemp_min + high * autotemp_factor; float t = autotemp_min + high * autotemp_factor;
...@@ -416,14 +364,14 @@ void plan_init() { ...@@ -416,14 +364,14 @@ void plan_init() {
t += (AUTOTEMP_OLDWEIGHT) * oldt; t += (AUTOTEMP_OLDWEIGHT) * oldt;
} }
oldt = t; oldt = t;
setTargetHotend0(t); setTargetHotend(t, 0);
} }
#endif //AUTOTEMP #endif //AUTOTEMP
/** /**
* Maintain fans, paste extruder pressure, * Maintain fans, paste extruder pressure,
*/ */
void check_axes_activity() { void Planner::check_axes_activity() {
unsigned char axis_active[NUM_AXIS] = { 0 }, unsigned char axis_active[NUM_AXIS] = { 0 },
tail_fan_speed = fanSpeed; tail_fan_speed = fanSpeed;
...@@ -523,9 +471,9 @@ void check_axes_activity() { ...@@ -523,9 +471,9 @@ void check_axes_activity() {
*/ */
#if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING) #if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
void plan_buffer_line(float x, float y, float z, const float& e, float feed_rate, const uint8_t extruder, const uint8_t driver) void Planner::buffer_line(float x, float y, float z, const float& e, float feed_rate, const uint8_t extruder, const uint8_t driver)
#else #else
void plan_buffer_line(const float& x, const float& y, const float& z, const float& e, float feed_rate, const uint8_t extruder, const uint8_t driver) void Planner::buffer_line(const float& x, const float& y, const float& z, const float& e, float feed_rate, const uint8_t extruder, const uint8_t driver)
#endif // AUTO_BED_LEVELING_FEATURE #endif // AUTO_BED_LEVELING_FEATURE
{ {
...@@ -546,7 +494,8 @@ void check_axes_activity() { ...@@ -546,7 +494,8 @@ void check_axes_activity() {
while (block_buffer_tail == next_buffer_head) idle(); while (block_buffer_tail == next_buffer_head) idle();
#if ENABLED(MESH_BED_LEVELING) #if ENABLED(MESH_BED_LEVELING)
if (mbl.active) z += mbl.get_z(x - home_offset[X_AXIS], y - home_offset[Y_AXIS]); if (mbl.active())
z += mbl.get_z(x - home_offset[X_AXIS], y - home_offset[Y_AXIS]);
#elif ENABLED(AUTO_BED_LEVELING_FEATURE) #elif ENABLED(AUTO_BED_LEVELING_FEATURE)
apply_rotation_xyz(bed_level_matrix, x, y, z); apply_rotation_xyz(bed_level_matrix, x, y, z);
#endif #endif
...@@ -850,9 +799,9 @@ void check_axes_activity() { ...@@ -850,9 +799,9 @@ void check_axes_activity() {
} }
if (block->steps[E_AXIS]) if (block->steps[E_AXIS])
NOLESS(feed_rate, minimumfeedrate); NOLESS(feed_rate, min_feedrate);
else else
NOLESS(feed_rate, mintravelfeedrate); NOLESS(feed_rate, min_travel_feedrate);
/** /**
* This part of the code calculates the total length of the movement. * This part of the code calculates the total length of the movement.
...@@ -909,35 +858,28 @@ void check_axes_activity() { ...@@ -909,35 +858,28 @@ void check_axes_activity() {
// Calculate steps between laser firings (steps_l) and consider that when determining largest // Calculate steps between laser firings (steps_l) and consider that when determining largest
// interval between steps for X, Y, Z, E, L to feed to the motion control code. // interval between steps for X, Y, Z, E, L to feed to the motion control code.
if (laser.mode == RASTER || laser.mode == PULSED) { if (laser.mode == RASTER || laser.mode == PULSED) {
#if ENABLED(LASER_PULSE_METHOD) block->steps_l = labs(1000 * block->millimeters * laser.ppm);
// Optimizing. Move calculations here rather than in stepper isr
static const float Factor = F_CPU/(LASER_PWM*2*100.0*255.0);
block->laser_raster_intensity_factor = laser.intensity * Factor;
#endif
block->steps_l = (unsigned long)labs(block->millimeters*laser.ppm);
if (laser.mode == RASTER) {
for (int i = 0; i < LASER_MAX_RASTER_LINE; i++) { for (int i = 0; i < LASER_MAX_RASTER_LINE; i++) {
#if (!ENABLED(LASER_PULSE_METHOD)) // Scale the image intensity based on the raster power.
float OldRange, NewRange, NewValue; // 100% power on a pixel basis is 255, convert back to 255 = 100.
int OldRange, NewRange;
float NewValue;
OldRange = (255.0 - 0.0); OldRange = (255.0 - 0.0);
NewRange = (laser.rasterlaserpower - LASER_REMAP_INTENSITY); NewRange = (laser.rasterlaserpower * 255.0 / 100.0 - LASER_REMAP_INTENSITY);
NewValue = (float)(((((float)laser.raster_data[i] - 0) * NewRange) / OldRange) + LASER_REMAP_INTENSITY); NewValue = (float)(((((float)laser.raster_data[i] - 0) * NewRange) / OldRange) + LASER_REMAP_INTENSITY);
//If less than 7%, turn off the laser tube. // If less than 7%, turn off the laser tube.
if(NewValue == LASER_REMAP_INTENSITY) if (NewValue <= LASER_REMAP_INTENSITY)
NewValue = 0; NewValue = 0;
block->laser_raster_data[i] = NewValue; block->laser_raster_data[i] = NewValue;
#else
block->laser_raster_data[i] = laser.raster_data[i];
#endif
}
} }
} }
else else
block->steps_l = 0; block->steps_l = 0;
block->step_event_count = max(block->steps[X_AXIS], max(block->steps[Y_AXIS], max(block->steps[Z_AXIS], max(block->steps[E_AXIS], block->steps_l)))); block->step_event_count = max(block->step_event_count, block->steps_l / 1000);
if (laser.diagnostics && block->laser_status == LASER_ON) if (laser.diagnostics && block->laser_status == LASER_ON)
ECHO_LM(INFO, "Laser firing enabled"); ECHO_LM(INFO, "Laser firing enabled");
...@@ -946,7 +888,7 @@ void check_axes_activity() { ...@@ -946,7 +888,7 @@ void check_axes_activity() {
float inverse_millimeters = 1.0 / block->millimeters; // Inverse millimeters to remove multiple divides float inverse_millimeters = 1.0 / block->millimeters; // Inverse millimeters to remove multiple divides
// Calculate speed in mm/second for each axis. No divide by zero due to previous checks. // Calculate moves/second for this move. No divide by zero due to previous checks.
float inverse_second = feed_rate * inverse_millimeters; float inverse_second = feed_rate * inverse_millimeters;
int moves_queued = movesplanned(); int moves_queued = movesplanned();
...@@ -961,9 +903,9 @@ void check_axes_activity() { ...@@ -961,9 +903,9 @@ void check_axes_activity() {
// segment time im micro seconds // segment time im micro seconds
unsigned long segment_time = lround(1000000.0 / inverse_second); unsigned long segment_time = lround(1000000.0 / inverse_second);
if (mq) { if (mq) {
if (segment_time < minsegmenttime) { if (segment_time < min_segment_time) {
// buffer is draining, add extra time. The amount of time added increases if the buffer is still emptied more. // buffer is draining, add extra time. The amount of time added increases if the buffer is still emptied more.
inverse_second = 1000000.0 / (segment_time + lround(2 * (minsegmenttime - segment_time) / moves_queued)); inverse_second = 1000000.0 / (segment_time + lround(2 * (min_segment_time - segment_time) / moves_queued));
#if ENABLED(XY_FREQUENCY_LIMIT) #if ENABLED(XY_FREQUENCY_LIMIT)
segment_time = lround(1000000.0 / inverse_second); segment_time = lround(1000000.0 / inverse_second);
#endif #endif
...@@ -976,11 +918,12 @@ void check_axes_activity() { ...@@ -976,11 +918,12 @@ void check_axes_activity() {
block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0 block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0
#if ENABLED(FILAMENT_SENSOR) #if ENABLED(FILAMENT_SENSOR)
//FMM update ring buffer used for delay with filament measurements static float filwidth_e_count = 0, filwidth_delay_dist = 0;
if (extruder == FILAMENT_SENSOR_EXTRUDER_NUM && delay_index2 > -1) { //only for extruder with filament sensor and if ring buffer is initialized //FMM update ring buffer used for delay with filament measurements
if (extruder == FILAMENT_SENSOR_EXTRUDER_NUM && filwidth_delay_index2 > -1) { //only for extruder with filament sensor and if ring buffer is initialized
const int MMD = MAX_MEASUREMENT_DELAY + 1, MMD10 = MMD * 10; const int MMD_CM = MAX_MEASUREMENT_DELAY + 1, MMD_MM = MMD_CM * 10;
delay_dist += delta_mm[E_AXIS]; // increment counter with next move in e axis delay_dist += delta_mm[E_AXIS]; // increment counter with next move in e axis
while (delay_dist >= MMD10) delay_dist -= MMD10; // loop around the buffer while (delay_dist >= MMD10) delay_dist -= MMD10; // loop around the buffer
...@@ -1175,7 +1118,7 @@ void check_axes_activity() { ...@@ -1175,7 +1118,7 @@ void check_axes_activity() {
block->recalculate_flag = true; // Always calculate trapezoid for new block block->recalculate_flag = true; // Always calculate trapezoid for new block
// Update previous path unit_vector and nominal speed // Update previous path unit_vector and nominal speed
memcpy(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[] for (int i = 0; i < NUM_AXIS; i++) previous_speed[i] = current_speed[i];
previous_nominal_speed = block->nominal_speed; previous_nominal_speed = block->nominal_speed;
#if ENABLED(ADVANCE) #if ENABLED(ADVANCE)
...@@ -1215,33 +1158,34 @@ void check_axes_activity() { ...@@ -1215,33 +1158,34 @@ void check_axes_activity() {
block_buffer_head = next_buffer_head; block_buffer_head = next_buffer_head;
// Update position // Update position
memcpy(position, target, sizeof(target)); // position[] = target[] for (int i = 0; i < NUM_AXIS; i++) position[i] = target[i];
planner_recalculate(); recalculate();
st_wake_up(); st_wake_up();
} // plan_buffer_line() } // buffer_line()
#if ENABLED(AUTO_BED_LEVELING_FEATURE) #if ENABLED(AUTO_BED_LEVELING_FEATURE)
/** /**
* Get the XYZ position of the steppers as a vector_3. * Get the XYZ position of the steppers as a vector_3.
* *
* On CORE machines XYZ is derived from ABC. * On CORE machines XYZ is derived from ABC.
*/ */
vector_3 plan_adjusted_position() { vector_3 Planner::adjusted_position() {
vector_3 position = vector_3(st_get_axis_position_mm(X_AXIS), st_get_axis_position_mm(Y_AXIS), st_get_axis_position_mm(Z_AXIS)); vector_3 pos = vector_3(st_get_axis_position_mm(X_AXIS), st_get_axis_position_mm(Y_AXIS), st_get_axis_position_mm(Z_AXIS));
//position.debug("in plan_get position"); //pos.debug("in Planner::adjusted_position");
//plan_bed_level_matrix.debug("in plan_adjusted_position"); //bed_level_matrix.debug("in Planner::adjusted_position");
matrix_3x3 inverse = matrix_3x3::transpose(plan_bed_level_matrix); matrix_3x3 inverse = matrix_3x3::transpose(bed_level_matrix);
//inverse.debug("in plan_get inverse"); //inverse.debug("in Planner::inverse");
position.apply_rotation(inverse); pos.apply_rotation(inverse);
//position.debug("after rotation"); //pos.debug("after rotation");
return position; return pos;
} }
#endif // AUTO_BED_LEVELING_FEATURE #endif // AUTO_BED_LEVELING_FEATURE
...@@ -1251,15 +1195,16 @@ void check_axes_activity() { ...@@ -1251,15 +1195,16 @@ void check_axes_activity() {
* On CORE machines stepper ABC will be translated from the given XYZ. * On CORE machines stepper ABC will be translated from the given XYZ.
*/ */
#if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING) #if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
void plan_set_position(float x, float y, float z, const float& e) void Planner::set_position_mm(float x, float y, float z, const float& e)
#else #else
void plan_set_position(const float& x, const float& y, const float& z, const float& e) void Planner::set_position_mm(const float& x, const float& y, const float& z, const float& e)
#endif // AUTO_BED_LEVELING_FEATURE || MESH_BED_LEVELING #endif // AUTO_BED_LEVELING_FEATURE || MESH_BED_LEVELING
{ {
#if ENABLED(MESH_BED_LEVELING) #if ENABLED(MESH_BED_LEVELING)
if (mbl.active) z += mbl.get_z(x - home_offset[X_AXIS], y - home_offset[Y_AXIS]); if (mbl.active())
z += mbl.get_z(x - home_offset[X_AXIS], y - home_offset[Y_AXIS]);
#elif ENABLED(AUTO_BED_LEVELING_FEATURE) #elif ENABLED(AUTO_BED_LEVELING_FEATURE)
apply_rotation_xyz(plan_bed_level_matrix, x, y, z); apply_rotation_xyz(bed_level_matrix, x, y, z);
#endif #endif
long nx = position[X_AXIS] = lround(x * axis_steps_per_unit[X_AXIS]), long nx = position[X_AXIS] = lround(x * axis_steps_per_unit[X_AXIS]),
...@@ -1270,17 +1215,31 @@ void check_axes_activity() { ...@@ -1270,17 +1215,31 @@ void check_axes_activity() {
st_set_position(nx, ny, nz, ne); st_set_position(nx, ny, nz, ne);
previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest. previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
for (uint8_t i = 0; i < NUM_AXIS; i++) previous_speed[i] = 0.0; for (int i = 0; i < NUM_AXIS; i++) previous_speed[i] = 0.0;
} }
void plan_set_e_position(const float& e) { /**
* Directly set the planner E position (hence the stepper E position).
*/
void Planner::set_e_position_mm(const float& e) {
position[E_AXIS] = lround(e * axis_steps_per_unit[E_AXIS + active_extruder]); position[E_AXIS] = lround(e * axis_steps_per_unit[E_AXIS + active_extruder]);
last_extruder = active_extruder; last_extruder = active_extruder;
st_set_e_position(position[E_AXIS]); st_set_e_position(position[E_AXIS]);
} }
// Calculate the steps/s^2 acceleration rates, based on the mm/s^s // Recalculate the steps/s^2 acceleration rates, based on the mm/s^2
void reset_acceleration_rates() { void Planner::reset_acceleration_rates() {
for (uint8_t i = 0; i < 3 + EXTRUDERS; i++) for (int i = 0; i < 3 + EXTRUDERS; i++)
axis_steps_per_sqr_second[i] = max_acceleration_units_per_sq_second[i] * axis_steps_per_unit[i]; axis_steps_per_sqr_second[i] = max_acceleration_units_per_sq_second[i] * axis_steps_per_unit[i];
} }
#if ENABLED(AUTOTEMP)
void Planner::autotemp_M109() {
autotemp_enabled = code_seen('F');
if (autotemp_enabled) autotemp_factor = code_value();
if (code_seen('S')) autotemp_min = code_value();
if (code_seen('B')) autotemp_max = code_value();
}
#endif
\ No newline at end of file
MK/module/motion/planner.h → MK/module/planner/planner.h
View file @ 55a2c790
...@@ -21,39 +21,39 @@ ...@@ -21,39 +21,39 @@
*/ */
/** /**
* planner.h - Buffer movement commands and manages the acceleration profile plan * planner.h
* Part of Grbl
* *
* Copyright (c) 2009-2011 Simen Svale Skogsrud * Buffer movement commands and manage the acceleration profile plan
*
* Grbl is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Grbl is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
* *
* You should have received a copy of the GNU General Public License * Derived from Grbl
* along with Grbl. If not, see <http://www.gnu.org/licenses/>. * Copyright (c) 2009-2011 Simen Svale Skogsrud
*/ */
// This module is to be considered a sub-module of stepper.c. Please don't include
// this file from any other module.
#ifndef PLANNER_H #ifndef PLANNER_H
#define PLANNER_H #define PLANNER_H
// This struct is used when buffering the setup for each linear movement "nominal" values are as specified in #if ENABLED(AUTO_BED_LEVELING_FEATURE)
// the source g-code and may never actually be reached if acceleration management is active. #include "vector_3.h"
#endif
class Planner;
extern Planner planner;
/**
* struct block_t
*
* A single entry in the planner buffer.
* Tracks linear movement over multiple axes.
*
* The "nominal" values are as-specified by gcode, and
* may never actually be reached due to acceleration limits.
*/
typedef struct { typedef struct {
unsigned char active_driver; // Selects the active driver unsigned char active_driver; // Selects the active driver
// Fields used by the bresenham algorithm for tracing the line // Fields used by the bresenham algorithm for tracing the line
unsigned long steps[NUM_AXIS]; // Step count along each axis long steps[NUM_AXIS]; // Step count along each axis
unsigned long step_event_count; // The number of step events required to complete this block unsigned long step_event_count; // The number of step events required to complete this block
#if ENABLED(COLOR_MIXING_EXTRUDER) #if ENABLED(COLOR_MIXING_EXTRUDER)
...@@ -66,7 +66,6 @@ typedef struct { ...@@ -66,7 +66,6 @@ typedef struct {
unsigned char direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) unsigned char direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h)
// Advance extrusion
#if ENABLED(ADVANCE) #if ENABLED(ADVANCE)
long advance_rate; long advance_rate;
volatile long initial_advance; volatile long initial_advance;
...@@ -78,7 +77,6 @@ typedef struct { ...@@ -78,7 +77,6 @@ typedef struct {
#endif #endif
// Fields used by the motion planner to manage acceleration // Fields used by the motion planner to manage acceleration
// float speed_x, speed_y, speed_z, speed_e; // Nominal mm/sec for each axis
float nominal_speed; // The nominal speed for this block in mm/sec float nominal_speed; // The nominal speed for this block in mm/sec
float entry_speed; // Entry speed at previous-current junction in mm/sec float entry_speed; // Entry speed at previous-current junction in mm/sec
float max_entry_speed; // Maximum allowable junction entry speed in mm/sec float max_entry_speed; // Maximum allowable junction entry speed in mm/sec
...@@ -108,44 +106,115 @@ typedef struct { ...@@ -108,44 +106,115 @@ typedef struct {
float laser_intensity; // Laser firing instensity in clock cycles for the PWM timer float laser_intensity; // Laser firing instensity in clock cycles for the PWM timer
#if ENABLED(LASER_RASTER) #if ENABLED(LASER_RASTER)
unsigned char laser_raster_data[LASER_MAX_RASTER_LINE]; unsigned char laser_raster_data[LASER_MAX_RASTER_LINE];
float laser_raster_intensity_factor;
#endif #endif
#endif #endif
volatile char busy; volatile char busy;
} block_t; } block_t;
#define BLOCK_MOD(n) ((n)&(BLOCK_BUFFER_SIZE-1)) #define BLOCK_MOD(n) ((n)&(BLOCK_BUFFER_SIZE-1))
#define MAX_EVENTS_COUNT 2147483648 // max for a signed 32 bit number class Planner {
// Initialize the motion plan subsystem public:
void plan_init();
void check_axes_activity(); /**
* A ring buffer of moves described in steps
*/
static block_t block_buffer[BLOCK_BUFFER_SIZE];
static volatile uint8_t block_buffer_head; // Index of the next block to be pushed
static volatile uint8_t block_buffer_tail;
static float max_feedrate[3 + EXTRUDERS]; // Max speeds in mm per minute
static float axis_steps_per_unit[3 + EXTRUDERS];
static unsigned long axis_steps_per_sqr_second[3 + EXTRUDERS];
static unsigned long max_acceleration_units_per_sq_second[3 + EXTRUDERS]; // Use M201 to override by software
static millis_t min_segment_time;
static float min_feedrate;
static float acceleration; // Normal acceleration mm/s^2 DEFAULT ACCELERATION for all printing moves. M204 SXXXX
static float retract_acceleration[EXTRUDERS]; // Retract acceleration mm/s^2 filament pull-back and push-forward while standing still in the other axes M204 TXXXX
static float travel_acceleration; // Travel acceleration mm/s^2 DEFAULT ACCELERATION for all NON printing moves. M204 MXXXX
static float max_xy_jerk; // The largest speed change requiring no acceleration
static float max_z_jerk;
static float max_e_jerk[EXTRUDERS];
static float min_travel_feedrate;
// Get the number of buffered moves #if ENABLED(AUTO_BED_LEVELING_FEATURE)
extern volatile unsigned char block_buffer_head; static matrix_3x3 bed_level_matrix; // Transform to compensate for bed level
extern volatile unsigned char block_buffer_tail; #endif
/** private:
* Number of moves currently in the planner
/**
* The current position of the tool in absolute steps
* Reclculated if any axis_steps_per_unit are changed by gcode
*/ */
FORCE_INLINE uint8_t movesplanned() { return BLOCK_MOD(block_buffer_head - block_buffer_tail + BLOCK_BUFFER_SIZE); } static long position[NUM_AXIS];
#if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING) /**
* Speed of previous path line segment
*/
static float previous_speed[NUM_AXIS];
#if ENABLED(AUTO_BED_LEVELING_FEATURE) /**
* Nominal speed of previous path line segment
*/
static float previous_nominal_speed;
#include "vector_3.h" #if ENABLED(DISABLE_INACTIVE_EXTRUDER)
/**
* Counters to manage disabling inactive extruders
*/
static uint8_t g_uc_extruder_last_move[EXTRUDERS];
#endif // DISABLE_INACTIVE_EXTRUDER
#if ENABLED(XY_FREQUENCY_LIMIT)
// Used for the frequency limit
#define MAX_FREQ_TIME long(1000000.0/XY_FREQUENCY_LIMIT)
// Old direction bits. Used for speed calculations
static unsigned char old_direction_bits;
// Segment times (in µs). Used for speed calculations
static long axis_segment_time[2][3];
#endif
/**
* Last extruder used
*/
static uint8_t last_extruder;
// Transform to compensate for bed level public:
extern matrix_3x3 plan_bed_level_matrix;
/**
* Instance Methods
*/
Planner();
void init();
/**
* Static (class) Methods
*/
static void reset_acceleration_rates();
// Manage fans, paste pressure, etc.
static void check_axes_activity();
/**
* Number of moves currently in the planner
*/
static uint8_t movesplanned() { return BLOCK_MOD(block_buffer_head - block_buffer_tail + BLOCK_BUFFER_SIZE); }
#if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
#if ENABLED(AUTO_BED_LEVELING_FEATURE)
/** /**
* The corrected position, applying the bed level matrix * The corrected position, applying the bed level matrix
*/ */
vector_3 plan_adjusted_position(); static vector_3 adjusted_position();
#endif #endif
/** /**
...@@ -155,80 +224,50 @@ FORCE_INLINE uint8_t movesplanned() { return BLOCK_MOD(block_buffer_head - block ...@@ -155,80 +224,50 @@ FORCE_INLINE uint8_t movesplanned() { return BLOCK_MOD(block_buffer_head - block
* feed_rate - (target) speed of the move * feed_rate - (target) speed of the move
* extruder - target extruder * extruder - target extruder
*/ */
void plan_buffer_line(float x, float y, float z, const float& e, float feed_rate, const uint8_t extruder, const uint8_t driver); static void buffer_line(float x, float y, float z, const float& e, float feed_rate, const uint8_t extruder, const uint8_t driver);
/** /**
* Set the planner.position and individual stepper positions. * Set the planner.position and individual stepper positions.
* Used by G92, G28, G29, and other procedures. * Used by G92, G28, G29, and other procedures.
* *
* Multiplies by axis_steps_per_unit[] and does necessary conversion * Multiplies by axis_steps_per_unit[] and does necessary conversion
* for COREXY / COREXZ to set the corresponding stepper positions. * for COREXY / COREXZ / COREYZ to set the corresponding stepper positions.
* *
* Clears previous speed values. * Clears previous speed values.
*/ */
void plan_set_position(float x, float y, float z, const float& e); static void set_position_mm(float x, float y, float z, const float& e);
#else #else
void plan_buffer_line(const float& x, const float& y, const float& z, const float& e, float feed_rate, const uint8_t extruder, const uint8_t driver); static void buffer_line(const float& x, const float& y, const float& z, const float& e, float feed_rate, const uint8_t extruder, const uint8_t driver);
void plan_set_position(const float& x, const float& y, const float& z, const float& e); static void set_position_mm(const float& x, const float& y, const float& z, const float& e);
#endif // AUTO_BED_LEVELING_FEATURE || MESH_BED_LEVELING #endif // AUTO_BED_LEVELING_FEATURE || MESH_BED_LEVELING
/** /**
* Set the E position (mm) of the planner (and the E stepper) * Set the E position (mm) of the planner (and the E stepper)
*/ */
void plan_set_e_position(const float& e); static void set_e_position_mm(const float& e);
//===========================================================================
//============================= public variables ============================
//===========================================================================
extern millis_t minsegmenttime;
extern float max_feedrate[3 + EXTRUDERS]; // Max speeds in mm per minute
extern float axis_steps_per_unit[3 + EXTRUDERS];
extern unsigned long max_acceleration_units_per_sq_second[3 + EXTRUDERS]; // Use M201 to override by software
extern float minimumfeedrate;
extern float acceleration; // Normal acceleration mm/s^2 DEFAULT ACCELERATION for all moves. M204 SXXXX
extern float retract_acceleration[EXTRUDERS]; // Retract acceleration mm/s^2 filament pull-back and push-forward while standing still in the other axis M204 TXXXX
extern float travel_acceleration; // Travel acceleration mm/s^2 DEFAULT ACCELERATION for all NON printing moves. M204 MXXXX
extern float max_xy_jerk; // The largest speed change requiring no acceleration
extern float max_z_jerk;
extern float max_e_jerk[EXTRUDERS];
extern float mintravelfeedrate;
extern unsigned long axis_steps_per_sqr_second[3 + EXTRUDERS];
extern long position[NUM_AXIS];
#if ENABLED(AUTOTEMP)
extern bool autotemp_enabled;
extern float autotemp_max;
extern float autotemp_min;
extern float autotemp_factor;
#endif
extern block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instructions
extern volatile unsigned char block_buffer_head; // Index of the next block to be pushed
extern volatile unsigned char block_buffer_tail;
/** /**
* Does the buffer have any blocks queued? * Does the buffer have any blocks queued?
*/ */
FORCE_INLINE bool blocks_queued() { return (block_buffer_head != block_buffer_tail); } static bool blocks_queued() { return (block_buffer_head != block_buffer_tail); }
/** /**
* "Discards" the block and "releases" the memory. * "Discards" the block and "releases" the memory.
* Called when the current block is no longer needed. * Called when the current block is no longer needed.
*/ */
FORCE_INLINE void plan_discard_current_block() { static void discard_current_block() {
if (blocks_queued()) if (blocks_queued())
block_buffer_tail = BLOCK_MOD(block_buffer_tail + 1); block_buffer_tail = BLOCK_MOD(block_buffer_tail + 1);
} }
/** /**
* The current block. NULL if the buffer is empty. * The current block. NULL if the buffer is empty.
* This also marks the block as busy. * This also marks the block as busy.
*/ */
FORCE_INLINE block_t* plan_get_current_block() { static block_t* get_current_block() {
if (blocks_queued()) { if (blocks_queued()) {
block_t* block = &block_buffer[block_buffer_tail]; block_t* block = &block_buffer[block_buffer_tail];
block->busy = true; block->busy = true;
...@@ -236,8 +275,68 @@ FORCE_INLINE block_t* plan_get_current_block() { ...@@ -236,8 +275,68 @@ FORCE_INLINE block_t* plan_get_current_block() {
} }
else else
return NULL; return NULL;
} }
#if ENABLED(AUTOTEMP)
static float autotemp_max;
static float autotemp_min;
static float autotemp_factor;
static bool autotemp_enabled;
static void getHighESpeed();
static void autotemp_M109();
#endif
private:
/**
* Get the index of the next / previous block in the ring buffer
*/
static int8_t next_block_index(int8_t block_index) { return BLOCK_MOD(block_index + 1); }
static int8_t prev_block_index(int8_t block_index) { return BLOCK_MOD(block_index - 1); }
/**
* Calculate the distance (not time) it takes to accelerate
* from initial_rate to target_rate using the given acceleration:
*/
static float estimate_acceleration_distance(float initial_rate, float target_rate, float acceleration) {
if (acceleration == 0) return 0; // acceleration was 0, set acceleration distance to 0
return (target_rate * target_rate - initial_rate * initial_rate) / (acceleration * 2);
}
/**
* Return the point at which you must start braking (at the rate of -'acceleration') if
* you start at 'initial_rate', accelerate (until reaching the point), and want to end at
* 'final_rate' after traveling 'distance'.
*
* This is used to compute the intersection point between acceleration and deceleration
* in cases where the "trapezoid" has no plateau (i.e., never reaches maximum speed)
*/
static float intersection_distance(float initial_rate, float final_rate, float acceleration, float distance) {
if (acceleration == 0) return 0; // acceleration was 0, set intersection distance to 0
return (acceleration * 2 * distance - initial_rate * initial_rate + final_rate * final_rate) / (acceleration * 4);
}
/**
* Calculate the maximum allowable speed at this point, in order
* to reach 'target_velocity' using 'acceleration' within a given
* 'distance'.
*/
static float max_allowable_speed(float acceleration, float target_velocity, float distance) {
return sqrt(target_velocity * target_velocity - 2 * acceleration * distance);
}
static void calculate_trapezoid_for_block(block_t* block, float entry_factor, float exit_factor);
static void reverse_pass_kernel(block_t* previous, block_t* current, block_t* next);
static void forward_pass_kernel(block_t* previous, block_t* current, block_t* next);
static void reverse_pass();
static void forward_pass();
static void recalculate_trapezoids();
static void recalculate();
void reset_acceleration_rates(); };
#endif // PLANNER_H #endif // PLANNER_H
MK/module/planner/qr_solve.cpp 0 → 100644
View file @ 55a2c790
/**
* MK & MK4due 3D Printer Firmware
*
* Based on Marlin, Sprinter and grbl
* Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
* Copyright (C) 2013 - 2016 Alberto Cotronei @MagoKimbra
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
*/
#include "../../base.h"
#if ENABLED(AUTO_BED_LEVELING_FEATURE) && ENABLED(AUTO_BED_LEVELING_GRID)
#include "qr_solve.h"
#include <stdlib.h>
#include <math.h>
//# include "r8lib.h"
int i4_min(int i1, int i2)
/******************************************************************************/
/**
Purpose:
I4_MIN returns the smaller of two I4's.
Licensing:
This code is distributed under the GNU LGPL license.
Modified:
29 August 2006
Author:
John Burkardt
Parameters:
Input, int I1, I2, two integers to be compared.
Output, int I4_MIN, the smaller of I1 and I2.
*/
{
return (i1 < i2) ? i1 : i2;
}
double r8_epsilon(void)
/******************************************************************************/
/**
Purpose:
R8_EPSILON returns the R8 round off unit.
Discussion:
R8_EPSILON is a number R which is a power of 2 with the property that,
to the precision of the computer's arithmetic,
1 < 1 + R
but
1 = ( 1 + R / 2 )
Licensing:
This code is distributed under the GNU LGPL license.
Modified:
01 September 2012
Author:
John Burkardt
Parameters:
Output, double R8_EPSILON, the R8 round-off unit.
*/
{
const double value = 2.220446049250313E-016;
return value;
}
double r8_max(double x, double y)
/******************************************************************************/
/**
Purpose:
R8_MAX returns the maximum of two R8's.
Licensing:
This code is distributed under the GNU LGPL license.
Modified:
07 May 2006
Author:
John Burkardt
Parameters:
Input, double X, Y, the quantities to compare.
Output, double R8_MAX, the maximum of X and Y.
*/
{
return (y < x) ? x : y;
}
double r8_abs(double x)
/******************************************************************************/
/**
Purpose:
R8_ABS returns the absolute value of an R8.
Licensing:
This code is distributed under the GNU LGPL license.
Modified:
07 May 2006
Author:
John Burkardt
Parameters:
Input, double X, the quantity whose absolute value is desired.
Output, double R8_ABS, the absolute value of X.
*/
{
return (x < 0.0) ? -x : x;
}
double r8_sign(double x)
/******************************************************************************/
/**
Purpose:
R8_SIGN returns the sign of an R8.
Licensing:
This code is distributed under the GNU LGPL license.
Modified:
08 May 2006
Author:
John Burkardt
Parameters:
Input, double X, the number whose sign is desired.
Output, double R8_SIGN, the sign of X.
*/
{
return (x < 0.0) ? -1.0 : 1.0;
}
double r8mat_amax(int m, int n, double a[])
/******************************************************************************/
/**
Purpose:
R8MAT_AMAX returns the maximum absolute value entry of an R8MAT.
Discussion:
An R8MAT is a doubly dimensioned array of R8 values, stored as a vector
in column-major order.
Licensing:
This code is distributed under the GNU LGPL license.
Modified:
07 September 2012
Author:
John Burkardt
Parameters:
Input, int M, the number of rows in A.
Input, int N, the number of columns in A.
Input, double A[M*N], the M by N matrix.
Output, double R8MAT_AMAX, the maximum absolute value entry of A.
*/
{
double value = r8_abs(a[0 + 0 * m]);
for (int j = 0; j < n; j++) {
for (int i = 0; i < m; i++) {
NOLESS(value, r8_abs(a[i + j * m]));
}
}
return value;
}
void r8mat_copy(double a2[], int m, int n, double a1[])
/******************************************************************************/
/**
Purpose:
R8MAT_COPY_NEW copies one R8MAT to a "new" R8MAT.
Discussion:
An R8MAT is a doubly dimensioned array of R8 values, stored as a vector
in column-major order.
Licensing:
This code is distributed under the GNU LGPL license.
Modified:
26 July 2008
Author:
John Burkardt
Parameters:
Input, int M, N, the number of rows and columns.
Input, double A1[M*N], the matrix to be copied.
Output, double R8MAT_COPY_NEW[M*N], the copy of A1.
*/
{
for (int j = 0; j < n; j++) {
for (int i = 0; i < m; i++)
a2[i + j * m] = a1[i + j * m];
}
}
/******************************************************************************/
void daxpy(int n, double da, double dx[], int incx, double dy[], int incy)
/******************************************************************************/
/**
Purpose:
DAXPY computes constant times a vector plus a vector.
Discussion:
This routine uses unrolled loops for increments equal to one.
Licensing:
This code is distributed under the GNU LGPL license.
Modified:
30 March 2007
Author:
C version by John Burkardt
Reference:
Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
LINPACK User's Guide,
SIAM, 1979.
Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
Basic Linear Algebra Subprograms for Fortran Usage,
Algorithm 539,
ACM Transactions on Mathematical Software,
Volume 5, Number 3, September 1979, pages 308-323.
Parameters:
Input, int N, the number of elements in DX and DY.
Input, double DA, the multiplier of DX.
Input, double DX[*], the first vector.
Input, int INCX, the increment between successive entries of DX.
Input/output, double DY[*], the second vector.
On output, DY[*] has been replaced by DY[*] + DA * DX[*].
Input, int INCY, the increment between successive entries of DY.
*/
{
if (n <= 0 || da == 0.0) return;
int i, ix, iy, m;
/**
Code for unequal increments or equal increments
not equal to 1.
*/
if (incx != 1 || incy != 1) {
if (0 <= incx)
ix = 0;
else
ix = (- n + 1) * incx;
if (0 <= incy)
iy = 0;
else
iy = (- n + 1) * incy;
for (i = 0; i < n; i++) {
dy[iy] = dy[iy] + da * dx[ix];
ix = ix + incx;
iy = iy + incy;
}
}
/**
Code for both increments equal to 1.
*/
else {
m = n % 4;
for (i = 0; i < m; i++)
dy[i] = dy[i] + da * dx[i];
for (i = m; i < n; i = i + 4) {
dy[i ] = dy[i ] + da * dx[i ];
dy[i + 1] = dy[i + 1] + da * dx[i + 1];
dy[i + 2] = dy[i + 2] + da * dx[i + 2];
dy[i + 3] = dy[i + 3] + da * dx[i + 3];
}
}
}
/******************************************************************************/
double ddot(int n, double dx[], int incx, double dy[], int incy)
/******************************************************************************/
/**
Purpose:
DDOT forms the dot product of two vectors.
Discussion:
This routine uses unrolled loops for increments equal to one.
Licensing:
This code is distributed under the GNU LGPL license.
Modified:
30 March 2007
Author:
C version by John Burkardt
Reference:
Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
LINPACK User's Guide,
SIAM, 1979.
Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
Basic Linear Algebra Subprograms for Fortran Usage,
Algorithm 539,
ACM Transactions on Mathematical Software,
Volume 5, Number 3, September 1979, pages 308-323.
Parameters:
Input, int N, the number of entries in the vectors.
Input, double DX[*], the first vector.
Input, int INCX, the increment between successive entries in DX.
Input, double DY[*], the second vector.
Input, int INCY, the increment between successive entries in DY.
Output, double DDOT, the sum of the product of the corresponding
entries of DX and DY.
*/
{
if (n <= 0) return 0.0;
int i, m;
double dtemp = 0.0;
/**
Code for unequal increments or equal increments
not equal to 1.
*/
if (incx != 1 || incy != 1) {
int ix = (incx >= 0) ? 0 : (-n + 1) * incx,
iy = (incy >= 0) ? 0 : (-n + 1) * incy;
for (i = 0; i < n; i++) {
dtemp += dx[ix] * dy[iy];
ix = ix + incx;
iy = iy + incy;
}
}
/**
Code for both increments equal to 1.
*/
else {
m = n % 5;
for (i = 0; i < m; i++)
dtemp += dx[i] * dy[i];
for (i = m; i < n; i = i + 5) {
dtemp += dx[i] * dy[i]
+ dx[i + 1] * dy[i + 1]
+ dx[i + 2] * dy[i + 2]
+ dx[i + 3] * dy[i + 3]
+ dx[i + 4] * dy[i + 4];
}
}
return dtemp;
}
/******************************************************************************/
double dnrm2(int n, double x[], int incx)
/******************************************************************************/
/**
Purpose:
DNRM2 returns the euclidean norm of a vector.
Discussion:
DNRM2 ( X ) = sqrt ( X' * X )
Licensing:
This code is distributed under the GNU LGPL license.
Modified:
30 March 2007
Author:
C version by John Burkardt
Reference:
Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
LINPACK User's Guide,
SIAM, 1979.
Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
Basic Linear Algebra Subprograms for Fortran Usage,
Algorithm 539,
ACM Transactions on Mathematical Software,
Volume 5, Number 3, September 1979, pages 308-323.
Parameters:
Input, int N, the number of entries in the vector.
Input, double X[*], the vector whose norm is to be computed.
Input, int INCX, the increment between successive entries of X.
Output, double DNRM2, the Euclidean norm of X.
*/
{
double norm;
if (n < 1 || incx < 1)
norm = 0.0;
else if (n == 1)
norm = r8_abs(x[0]);
else {
double scale = 0.0, ssq = 1.0;
int ix = 0;
for (int i = 0; i < n; i++) {
if (x[ix] != 0.0) {
double absxi = r8_abs(x[ix]);
if (scale < absxi) {
ssq = 1.0 + ssq * (scale / absxi) * (scale / absxi);
scale = absxi;
}
else
ssq = ssq + (absxi / scale) * (absxi / scale);
}
ix += incx;
}
norm = scale * sqrt(ssq);
}
return norm;
}
/******************************************************************************/
void dqrank(double a[], int lda, int m, int n, double tol, int* kr,
int jpvt[], double qraux[])
/******************************************************************************/
/**
Purpose:
DQRANK computes the QR factorization of a rectangular matrix.
Discussion:
This routine is used in conjunction with DQRLSS to solve
overdetermined, underdetermined and singular linear systems
in a least squares sense.
DQRANK uses the LINPACK subroutine DQRDC to compute the QR
factorization, with column pivoting, of an M by N matrix A.
The numerical rank is determined using the tolerance TOL.
Note that on output, ABS ( A(1,1) ) / ABS ( A(KR,KR) ) is an estimate
of the condition number of the matrix of independent columns,
and of R. This estimate will be <= 1/TOL.
Licensing:
This code is distributed under the GNU LGPL license.
Modified:
21 April 2012
Author:
C version by John Burkardt.
Reference:
Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
LINPACK User's Guide,
SIAM, 1979,
ISBN13: 978-0-898711-72-1,
LC: QA214.L56.
Parameters:
Input/output, double A[LDA*N]. On input, the matrix whose
decomposition is to be computed. On output, the information from DQRDC.
The triangular matrix R of the QR factorization is contained in the
upper triangle and information needed to recover the orthogonal
matrix Q is stored below the diagonal in A and in the vector QRAUX.
Input, int LDA, the leading dimension of A, which must
be at least M.
Input, int M, the number of rows of A.
Input, int N, the number of columns of A.
Input, double TOL, a relative tolerance used to determine the
numerical rank. The problem should be scaled so that all the elements
of A have roughly the same absolute accuracy, EPS. Then a reasonable
value for TOL is roughly EPS divided by the magnitude of the largest
element.
Output, int *KR, the numerical rank.
Output, int JPVT[N], the pivot information from DQRDC.
Columns JPVT(1), ..., JPVT(KR) of the original matrix are linearly
independent to within the tolerance TOL and the remaining columns
are linearly dependent.
Output, double QRAUX[N], will contain extra information defining
the QR factorization.
*/
{
double work[n];
for (int i = 0; i < n; i++)
jpvt[i] = 0;
int job = 1;
dqrdc(a, lda, m, n, qraux, jpvt, work, job);
*kr = 0;
int k = i4_min(m, n);
for (int j = 0; j < k; j++) {
if (r8_abs(a[j + j * lda]) <= tol * r8_abs(a[0 + 0 * lda]))
return;
*kr = j + 1;
}
}
/******************************************************************************/
void dqrdc(double a[], int lda, int n, int p, double qraux[], int jpvt[],
double work[], int job)
/******************************************************************************/
/**
Purpose:
DQRDC computes the QR factorization of a real rectangular matrix.
Discussion:
DQRDC uses Householder transformations.
Column pivoting based on the 2-norms of the reduced columns may be
performed at the user's option.
Licensing:
This code is distributed under the GNU LGPL license.
Modified:
07 June 2005
Author:
C version by John Burkardt.
Reference:
Jack Dongarra, Cleve Moler, Jim Bunch and Pete Stewart,
LINPACK User's Guide,
SIAM, (Society for Industrial and Applied Mathematics),
3600 University City Science Center,
Philadelphia, PA, 19104-2688.
ISBN 0-89871-172-X
Parameters:
Input/output, double A(LDA,P). On input, the N by P matrix
whose decomposition is to be computed. On output, A contains in
its upper triangle the upper triangular matrix R of the QR
factorization. Below its diagonal A contains information from
which the orthogonal part of the decomposition can be recovered.
Note that if pivoting has been requested, the decomposition is not that
of the original matrix A but that of A with its columns permuted
as described by JPVT.
Input, int LDA, the leading dimension of the array A. LDA must
be at least N.
Input, int N, the number of rows of the matrix A.
Input, int P, the number of columns of the matrix A.
Output, double QRAUX[P], contains further information required
to recover the orthogonal part of the decomposition.
Input/output, integer JPVT[P]. On input, JPVT contains integers that
control the selection of the pivot columns. The K-th column A(*,K) of A
is placed in one of three classes according to the value of JPVT(K).
> 0, then A(K) is an initial column.
= 0, then A(K) is a free column.
< 0, then A(K) is a final column.
Before the decomposition is computed, initial columns are moved to
the beginning of the array A and final columns to the end. Both
initial and final columns are frozen in place during the computation
and only free columns are moved. At the K-th stage of the
reduction, if A(*,K) is occupied by a free column it is interchanged
with the free column of largest reduced norm. JPVT is not referenced
if JOB == 0. On output, JPVT(K) contains the index of the column of the
original matrix that has been interchanged into the K-th column, if
pivoting was requested.
Workspace, double WORK[P]. WORK is not referenced if JOB == 0.
Input, int JOB, initiates column pivoting.
0, no pivoting is done.
nonzero, pivoting is done.
*/
{
int jp;
int j;
int lup;
int maxj;
double maxnrm, nrmxl, t, tt;
int pl = 1, pu = 0;
/**
If pivoting is requested, rearrange the columns.
*/
if (job != 0) {
for (j = 1; j <= p; j++) {
int swapj = (0 < jpvt[j - 1]);
jpvt[j - 1] = (jpvt[j - 1] < 0) ? -j : j;
if (swapj) {
if (j != pl)
dswap(n, a + 0 + (pl - 1)*lda, 1, a + 0 + (j - 1), 1);
jpvt[j - 1] = jpvt[pl - 1];
jpvt[pl - 1] = j;
pl++;
}
}
pu = p;
for (j = p; 1 <= j; j--) {
if (jpvt[j - 1] < 0) {
jpvt[j - 1] = -jpvt[j - 1];
if (j != pu) {
dswap(n, a + 0 + (pu - 1)*lda, 1, a + 0 + (j - 1)*lda, 1);
jp = jpvt[pu - 1];
jpvt[pu - 1] = jpvt[j - 1];
jpvt[j - 1] = jp;
}
pu = pu - 1;
}
}
}
/**
Compute the norms of the free columns.
*/
for (j = pl; j <= pu; j++)
qraux[j - 1] = dnrm2(n, a + 0 + (j - 1) * lda, 1);
for (j = pl; j <= pu; j++)
work[j - 1] = qraux[j - 1];
/**
Perform the Householder reduction of A.
*/
lup = i4_min(n, p);
for (int l = 1; l <= lup; l++) {
/**
Bring the column of largest norm into the pivot position.
*/
if (pl <= l && l < pu) {
maxnrm = 0.0;
maxj = l;
for (j = l; j <= pu; j++) {
if (maxnrm < qraux[j - 1]) {
maxnrm = qraux[j - 1];
maxj = j;
}
}
if (maxj != l) {
dswap(n, a + 0 + (l - 1)*lda, 1, a + 0 + (maxj - 1)*lda, 1);
qraux[maxj - 1] = qraux[l - 1];
work[maxj - 1] = work[l - 1];
jp = jpvt[maxj - 1];
jpvt[maxj - 1] = jpvt[l - 1];
jpvt[l - 1] = jp;
}
}
/**
Compute the Householder transformation for column L.
*/
qraux[l - 1] = 0.0;
if (l != n) {
nrmxl = dnrm2(n - l + 1, a + l - 1 + (l - 1) * lda, 1);
if (nrmxl != 0.0) {
if (a[l - 1 + (l - 1)*lda] != 0.0)
nrmxl = nrmxl * r8_sign(a[l - 1 + (l - 1) * lda]);
dscal(n - l + 1, 1.0 / nrmxl, a + l - 1 + (l - 1)*lda, 1);
a[l - 1 + (l - 1)*lda] = 1.0 + a[l - 1 + (l - 1) * lda];
/**
Apply the transformation to the remaining columns, updating the norms.
*/
for (j = l + 1; j <= p; j++) {
t = -ddot(n - l + 1, a + l - 1 + (l - 1) * lda, 1, a + l - 1 + (j - 1) * lda, 1)
/ a[l - 1 + (l - 1) * lda];
daxpy(n - l + 1, t, a + l - 1 + (l - 1)*lda, 1, a + l - 1 + (j - 1)*lda, 1);
if (pl <= j && j <= pu) {
if (qraux[j - 1] != 0.0) {
tt = 1.0 - pow(r8_abs(a[l - 1 + (j - 1) * lda]) / qraux[j - 1], 2);
tt = r8_max(tt, 0.0);
t = tt;
tt = 1.0 + 0.05 * tt * pow(qraux[j - 1] / work[j - 1], 2);
if (tt != 1.0)
qraux[j - 1] = qraux[j - 1] * sqrt(t);
else {
qraux[j - 1] = dnrm2(n - l, a + l + (j - 1) * lda, 1);
work[j - 1] = qraux[j - 1];
}
}
}
}
/**
Save the transformation.
*/
qraux[l - 1] = a[l - 1 + (l - 1) * lda];
a[l - 1 + (l - 1)*lda] = -nrmxl;
}
}
}
}
/******************************************************************************/
int dqrls(double a[], int lda, int m, int n, double tol, int* kr, double b[],
double x[], double rsd[], int jpvt[], double qraux[], int itask)
/******************************************************************************/
/**
Purpose:
DQRLS factors and solves a linear system in the least squares sense.
Discussion:
The linear system may be overdetermined, underdetermined or singular.
The solution is obtained using a QR factorization of the
coefficient matrix.
DQRLS can be efficiently used to solve several least squares
problems with the same matrix A. The first system is solved
with ITASK = 1. The subsequent systems are solved with
ITASK = 2, to avoid the recomputation of the matrix factors.
The parameters KR, JPVT, and QRAUX must not be modified
between calls to DQRLS.
DQRLS is used to solve in a least squares sense
overdetermined, underdetermined and singular linear systems.
The system is A*X approximates B where A is M by N.
B is a given M-vector, and X is the N-vector to be computed.
A solution X is found which minimimzes the sum of squares (2-norm)
of the residual, A*X - B.
The numerical rank of A is determined using the tolerance TOL.
DQRLS uses the LINPACK subroutine DQRDC to compute the QR
factorization, with column pivoting, of an M by N matrix A.
Licensing:
This code is distributed under the GNU LGPL license.
Modified:
10 September 2012
Author:
C version by John Burkardt.
Reference:
David Kahaner, Cleve Moler, Steven Nash,
Numerical Methods and Software,
Prentice Hall, 1989,
ISBN: 0-13-627258-4,
LC: TA345.K34.
Parameters:
Input/output, double A[LDA*N], an M by N matrix.
On input, the matrix whose decomposition is to be computed.
In a least squares data fitting problem, A(I,J) is the
value of the J-th basis (model) function at the I-th data point.
On output, A contains the output from DQRDC. The triangular matrix R
of the QR factorization is contained in the upper triangle and
information needed to recover the orthogonal matrix Q is stored
below the diagonal in A and in the vector QRAUX.
Input, int LDA, the leading dimension of A.
Input, int M, the number of rows of A.
Input, int N, the number of columns of A.
Input, double TOL, a relative tolerance used to determine the
numerical rank. The problem should be scaled so that all the elements
of A have roughly the same absolute accuracy EPS. Then a reasonable
value for TOL is roughly EPS divided by the magnitude of the largest
element.
Output, int *KR, the numerical rank.
Input, double B[M], the right hand side of the linear system.
Output, double X[N], a least squares solution to the linear
system.
Output, double RSD[M], the residual, B - A*X. RSD may
overwrite B.
Workspace, int JPVT[N], required if ITASK = 1.
Columns JPVT(1), ..., JPVT(KR) of the original matrix are linearly
independent to within the tolerance TOL and the remaining columns
are linearly dependent. ABS ( A(1,1) ) / ABS ( A(KR,KR) ) is an estimate
of the condition number of the matrix of independent columns,
and of R. This estimate will be <= 1/TOL.
Workspace, double QRAUX[N], required if ITASK = 1.
Input, int ITASK.
1, DQRLS factors the matrix A and solves the least squares problem.
2, DQRLS assumes that the matrix A was factored with an earlier
call to DQRLS, and only solves the least squares problem.
Output, int DQRLS, error code.
0: no error
-1: LDA < M (fatal error)
-2: N < 1 (fatal error)
-3: ITASK < 1 (fatal error)
*/
{
int ind;
if (lda < m) {
/*fprintf ( stderr, "\n" );
fprintf ( stderr, "DQRLS - Fatal error!\n" );
fprintf ( stderr, " LDA < M.\n" );*/
ind = -1;
return ind;
}
if (n <= 0) {
/*fprintf ( stderr, "\n" );
fprintf ( stderr, "DQRLS - Fatal error!\n" );
fprintf ( stderr, " N <= 0.\n" );*/
ind = -2;
return ind;
}
if (itask < 1) {
/*fprintf ( stderr, "\n" );
fprintf ( stderr, "DQRLS - Fatal error!\n" );
fprintf ( stderr, " ITASK < 1.\n" );*/
ind = -3;
return ind;
}
ind = 0;
/**
Factor the matrix.
*/
if (itask == 1)
dqrank(a, lda, m, n, tol, kr, jpvt, qraux);
/**
Solve the least-squares problem.
*/
dqrlss(a, lda, m, n, *kr, b, x, rsd, jpvt, qraux);
return ind;
}
/******************************************************************************/
void dqrlss(double a[], int lda, int m, int n, int kr, double b[], double x[],
double rsd[], int jpvt[], double qraux[])
/******************************************************************************/
/**
Purpose:
DQRLSS solves a linear system in a least squares sense.
Discussion:
DQRLSS must be preceded by a call to DQRANK.
The system is to be solved is
A * X = B
where
A is an M by N matrix with rank KR, as determined by DQRANK,
B is a given M-vector,
X is the N-vector to be computed.
A solution X, with at most KR nonzero components, is found which
minimizes the 2-norm of the residual (A*X-B).
Once the matrix A has been formed, DQRANK should be
called once to decompose it. Then, for each right hand
side B, DQRLSS should be called once to obtain the
solution and residual.
Licensing:
This code is distributed under the GNU LGPL license.
Modified:
10 September 2012
Author:
C version by John Burkardt
Parameters:
Input, double A[LDA*N], the QR factorization information
from DQRANK. The triangular matrix R of the QR factorization is
contained in the upper triangle and information needed to recover
the orthogonal matrix Q is stored below the diagonal in A and in
the vector QRAUX.
Input, int LDA, the leading dimension of A, which must
be at least M.
Input, int M, the number of rows of A.
Input, int N, the number of columns of A.
Input, int KR, the rank of the matrix, as estimated by DQRANK.
Input, double B[M], the right hand side of the linear system.
Output, double X[N], a least squares solution to the
linear system.
Output, double RSD[M], the residual, B - A*X. RSD may
overwrite B.
Input, int JPVT[N], the pivot information from DQRANK.
Columns JPVT[0], ..., JPVT[KR-1] of the original matrix are linearly
independent to within the tolerance TOL and the remaining columns
are linearly dependent.
Input, double QRAUX[N], auxiliary information from DQRANK
defining the QR factorization.
*/
{
int i;
int info;
int j;
int job;
int k;
double t;
if (kr != 0) {
job = 110;
info = dqrsl(a, lda, m, kr, qraux, b, rsd, rsd, x, rsd, rsd, job); UNUSED(info);
}
for (i = 0; i < n; i++)
jpvt[i] = - jpvt[i];
for (i = kr; i < n; i++)
x[i] = 0.0;
for (j = 1; j <= n; j++) {
if (jpvt[j - 1] <= 0) {
k = - jpvt[j - 1];
jpvt[j - 1] = k;
while (k != j) {
t = x[j - 1];
x[j - 1] = x[k - 1];
x[k - 1] = t;
jpvt[k - 1] = -jpvt[k - 1];
k = jpvt[k - 1];
}
}
}
}
/******************************************************************************/
int dqrsl(double a[], int lda, int n, int k, double qraux[], double y[],
double qy[], double qty[], double b[], double rsd[], double ab[], int job)
/******************************************************************************/
/**
Purpose:
DQRSL computes transformations, projections, and least squares solutions.
Discussion:
DQRSL requires the output of DQRDC.
For K <= min(N,P), let AK be the matrix
AK = ( A(JPVT[0]), A(JPVT(2)), ..., A(JPVT(K)) )
formed from columns JPVT[0], ..., JPVT(K) of the original
N by P matrix A that was input to DQRDC. If no pivoting was
done, AK consists of the first K columns of A in their
original order. DQRDC produces a factored orthogonal matrix Q
and an upper triangular matrix R such that
AK = Q * (R)
(0)
This information is contained in coded form in the arrays
A and QRAUX.
The parameters QY, QTY, B, RSD, and AB are not referenced
if their computation is not requested and in this case
can be replaced by dummy variables in the calling program.
To save storage, the user may in some cases use the same
array for different parameters in the calling sequence. A
frequently occurring example is when one wishes to compute
any of B, RSD, or AB and does not need Y or QTY. In this
case one may identify Y, QTY, and one of B, RSD, or AB, while
providing separate arrays for anything else that is to be
computed.
Thus the calling sequence
dqrsl ( a, lda, n, k, qraux, y, dum, y, b, y, dum, 110, info )
will result in the computation of B and RSD, with RSD
overwriting Y. More generally, each item in the following
list contains groups of permissible identifications for
a single calling sequence.
1. (Y,QTY,B) (RSD) (AB) (QY)
2. (Y,QTY,RSD) (B) (AB) (QY)
3. (Y,QTY,AB) (B) (RSD) (QY)
4. (Y,QY) (QTY,B) (RSD) (AB)
5. (Y,QY) (QTY,RSD) (B) (AB)
6. (Y,QY) (QTY,AB) (B) (RSD)
In any group the value returned in the array allocated to
the group corresponds to the last member of the group.
Licensing:
This code is distributed under the GNU LGPL license.
Modified:
07 June 2005
Author:
C version by John Burkardt.
Reference:
Jack Dongarra, Cleve Moler, Jim Bunch and Pete Stewart,
LINPACK User's Guide,
SIAM, (Society for Industrial and Applied Mathematics),
3600 University City Science Center,
Philadelphia, PA, 19104-2688.
ISBN 0-89871-172-X
Parameters:
Input, double A[LDA*P], contains the output of DQRDC.
Input, int LDA, the leading dimension of the array A.
Input, int N, the number of rows of the matrix AK. It must
have the same value as N in DQRDC.
Input, int K, the number of columns of the matrix AK. K
must not be greater than min(N,P), where P is the same as in the
calling sequence to DQRDC.
Input, double QRAUX[P], the auxiliary output from DQRDC.
Input, double Y[N], a vector to be manipulated by DQRSL.
Output, double QY[N], contains Q * Y, if requested.
Output, double QTY[N], contains Q' * Y, if requested.
Output, double B[K], the solution of the least squares problem
minimize norm2 ( Y - AK * B),
if its computation has been requested. Note that if pivoting was
requested in DQRDC, the J-th component of B will be associated with
column JPVT(J) of the original matrix A that was input into DQRDC.
Output, double RSD[N], the least squares residual Y - AK * B,
if its computation has been requested. RSD is also the orthogonal
projection of Y onto the orthogonal complement of the column space
of AK.
Output, double AB[N], the least squares approximation Ak * B,
if its computation has been requested. AB is also the orthogonal
projection of Y onto the column space of A.
Input, integer JOB, specifies what is to be computed. JOB has
the decimal expansion ABCDE, with the following meaning:
if A != 0, compute QY.
if B != 0, compute QTY.
if C != 0, compute QTY and B.
if D != 0, compute QTY and RSD.
if E != 0, compute QTY and AB.
Note that a request to compute B, RSD, or AB automatically triggers
the computation of QTY, for which an array must be provided in the
calling sequence.
Output, int DQRSL, is zero unless the computation of B has
been requested and R is exactly singular. In this case, INFO is the
index of the first zero diagonal element of R, and B is left unaltered.
*/
{
int cab;
int cb;
int cqty;
int cqy;
int cr;
int i;
int info;
int j;
int jj;
int ju;
double t;
double temp;
/**
Set INFO flag.
*/
info = 0;
/**
Determine what is to be computed.
*/
cqy = ( job / 10000 != 0);
cqty = ((job % 10000) != 0);
cb = ((job % 1000) / 100 != 0);
cr = ((job % 100) / 10 != 0);
cab = ((job % 10) != 0);
ju = i4_min(k, n - 1);
/**
Special action when N = 1.
*/
if (ju == 0) {
if (cqy)
qy[0] = y[0];
if (cqty)
qty[0] = y[0];
if (cab)
ab[0] = y[0];
if (cb) {
if (a[0 + 0 * lda] == 0.0)
info = 1;
else
b[0] = y[0] / a[0 + 0 * lda];
}
if (cr)
rsd[0] = 0.0;
return info;
}
/**
Set up to compute QY or QTY.
*/
if (cqy) {
for (i = 1; i <= n; i++)
qy[i - 1] = y[i - 1];
}
if (cqty) {
for (i = 1; i <= n; i++)
qty[i - 1] = y[i - 1];
}
/**
Compute QY.
*/
if (cqy) {
for (jj = 1; jj <= ju; jj++) {
j = ju - jj + 1;
if (qraux[j - 1] != 0.0) {
temp = a[j - 1 + (j - 1) * lda];
a[j - 1 + (j - 1)*lda] = qraux[j - 1];
t = -ddot(n - j + 1, a + j - 1 + (j - 1) * lda, 1, qy + j - 1, 1) / a[j - 1 + (j - 1) * lda];
daxpy(n - j + 1, t, a + j - 1 + (j - 1)*lda, 1, qy + j - 1, 1);
a[j - 1 + (j - 1)*lda] = temp;
}
}
}
/**
Compute Q'*Y.
*/
if (cqty) {
for (j = 1; j <= ju; j++) {
if (qraux[j - 1] != 0.0) {
temp = a[j - 1 + (j - 1) * lda];
a[j - 1 + (j - 1)*lda] = qraux[j - 1];
t = -ddot(n - j + 1, a + j - 1 + (j - 1) * lda, 1, qty + j - 1, 1) / a[j - 1 + (j - 1) * lda];
daxpy(n - j + 1, t, a + j - 1 + (j - 1)*lda, 1, qty + j - 1, 1);
a[j - 1 + (j - 1)*lda] = temp;
}
}
}
/**
Set up to compute B, RSD, or AB.
*/
if (cb) {
for (i = 1; i <= k; i++)
b[i - 1] = qty[i - 1];
}
if (cab) {
for (i = 1; i <= k; i++)
ab[i - 1] = qty[i - 1];
}
if (cr && k < n) {
for (i = k + 1; i <= n; i++)
rsd[i - 1] = qty[i - 1];
}
if (cab && k + 1 <= n) {
for (i = k + 1; i <= n; i++)
ab[i - 1] = 0.0;
}
if (cr) {
for (i = 1; i <= k; i++)
rsd[i - 1] = 0.0;
}
/**
Compute B.
*/
if (cb) {
for (jj = 1; jj <= k; jj++) {
j = k - jj + 1;
if (a[j - 1 + (j - 1)*lda] == 0.0) {
info = j;
break;
}
b[j - 1] = b[j - 1] / a[j - 1 + (j - 1) * lda];
if (j != 1) {
t = -b[j - 1];
daxpy(j - 1, t, a + 0 + (j - 1)*lda, 1, b, 1);
}
}
}
/**
Compute RSD or AB as required.
*/
if (cr || cab) {
for (jj = 1; jj <= ju; jj++) {
j = ju - jj + 1;
if (qraux[j - 1] != 0.0) {
temp = a[j - 1 + (j - 1) * lda];
a[j - 1 + (j - 1)*lda] = qraux[j - 1];
if (cr) {
t = -ddot(n - j + 1, a + j - 1 + (j - 1) * lda, 1, rsd + j - 1, 1)
/ a[j - 1 + (j - 1) * lda];
daxpy(n - j + 1, t, a + j - 1 + (j - 1)*lda, 1, rsd + j - 1, 1);
}
if (cab) {
t = -ddot(n - j + 1, a + j - 1 + (j - 1) * lda, 1, ab + j - 1, 1)
/ a[j - 1 + (j - 1) * lda];
daxpy(n - j + 1, t, a + j - 1 + (j - 1)*lda, 1, ab + j - 1, 1);
}
a[j - 1 + (j - 1)*lda] = temp;
}
}
}
return info;
}
/******************************************************************************/
/******************************************************************************/
void dscal(int n, double sa, double x[], int incx)
/******************************************************************************/
/**
Purpose:
DSCAL scales a vector by a constant.
Licensing:
This code is distributed under the GNU LGPL license.
Modified:
30 March 2007
Author:
C version by John Burkardt
Reference:
Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
LINPACK User's Guide,
SIAM, 1979.
Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
Basic Linear Algebra Subprograms for Fortran Usage,
Algorithm 539,
ACM Transactions on Mathematical Software,
Volume 5, Number 3, September 1979, pages 308-323.
Parameters:
Input, int N, the number of entries in the vector.
Input, double SA, the multiplier.
Input/output, double X[*], the vector to be scaled.
Input, int INCX, the increment between successive entries of X.
*/
{
int i;
int ix;
int m;
if (n <= 0) return;
if (incx == 1) {
m = n % 5;
for (i = 0; i < m; i++)
x[i] = sa * x[i];
for (i = m; i < n; i = i + 5) {
x[i] = sa * x[i];
x[i + 1] = sa * x[i + 1];
x[i + 2] = sa * x[i + 2];
x[i + 3] = sa * x[i + 3];
x[i + 4] = sa * x[i + 4];
}
}
else {
if (0 <= incx)
ix = 0;
else
ix = (- n + 1) * incx;
for (i = 0; i < n; i++) {
x[ix] = sa * x[ix];
ix = ix + incx;
}
}
}
/******************************************************************************/
void dswap(int n, double x[], int incx, double y[], int incy)
/******************************************************************************/
/**
Purpose:
DSWAP interchanges two vectors.
Licensing:
This code is distributed under the GNU LGPL license.
Modified:
30 March 2007
Author:
C version by John Burkardt
Reference:
Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
LINPACK User's Guide,
SIAM, 1979.
Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
Basic Linear Algebra Subprograms for Fortran Usage,
Algorithm 539,
ACM Transactions on Mathematical Software,
Volume 5, Number 3, September 1979, pages 308-323.
Parameters:
Input, int N, the number of entries in the vectors.
Input/output, double X[*], one of the vectors to swap.
Input, int INCX, the increment between successive entries of X.
Input/output, double Y[*], one of the vectors to swap.
Input, int INCY, the increment between successive elements of Y.
*/
{
if (n <= 0) return;
int i, ix, iy, m;
double temp;
if (incx == 1 && incy == 1) {
m = n % 3;
for (i = 0; i < m; i++) {
temp = x[i];
x[i] = y[i];
y[i] = temp;
}
for (i = m; i < n; i = i + 3) {
temp = x[i];
x[i] = y[i];
y[i] = temp;
temp = x[i + 1];
x[i + 1] = y[i + 1];
y[i + 1] = temp;
temp = x[i + 2];
x[i + 2] = y[i + 2];
y[i + 2] = temp;
}
}
else {
ix = (incx >= 0) ? 0 : (-n + 1) * incx;
iy = (incy >= 0) ? 0 : (-n + 1) * incy;
for (i = 0; i < n; i++) {
temp = x[ix];
x[ix] = y[iy];
y[iy] = temp;
ix = ix + incx;
iy = iy + incy;
}
}
}
/******************************************************************************/
/******************************************************************************/
void qr_solve(double x[], int m, int n, double a[], double b[])
/******************************************************************************/
/**
Purpose:
QR_SOLVE solves a linear system in the least squares sense.
Discussion:
If the matrix A has full column rank, then the solution X should be the
unique vector that minimizes the Euclidean norm of the residual.
If the matrix A does not have full column rank, then the solution is
not unique; the vector X will minimize the residual norm, but so will
various other vectors.
Licensing:
This code is distributed under the GNU LGPL license.
Modified:
11 September 2012
Author:
John Burkardt
Reference:
David Kahaner, Cleve Moler, Steven Nash,
Numerical Methods and Software,
Prentice Hall, 1989,
ISBN: 0-13-627258-4,
LC: TA345.K34.
Parameters:
Input, int M, the number of rows of A.
Input, int N, the number of columns of A.
Input, double A[M*N], the matrix.
Input, double B[M], the right hand side.
Output, double QR_SOLVE[N], the least squares solution.
*/
{
double a_qr[n * m], qraux[n], r[m], tol;
int ind, itask, jpvt[n], kr, lda;
r8mat_copy(a_qr, m, n, a);
lda = m;
tol = r8_epsilon() / r8mat_amax(m, n, a_qr);
itask = 1;
ind = dqrls(a_qr, lda, m, n, tol, &kr, b, x, r, jpvt, qraux, itask); UNUSED(ind);
}
/******************************************************************************/
#endif
MK/module/motion/qr_solve.h → MK/module/planner/qr_solve.h
View file @ 55a2c790
...@@ -20,23 +20,31 @@ ...@@ -20,23 +20,31 @@
* *
*/ */
#include "../../base.h"
#ifndef QR_SOLVE_H
#define QR_SOLVE_H
#if ENABLED(AUTO_BED_LEVELING_GRID) #if ENABLED(AUTO_BED_LEVELING_GRID)
void daxpy(int n, double da, double dx[], int incx, double dy[], int incy); void daxpy(int n, double da, double dx[], int incx, double dy[], int incy);
double ddot(int n, double dx[], int incx, double dy[], int incy); double ddot(int n, double dx[], int incx, double dy[], int incy);
double dnrm2(int n, double x[], int incx); double dnrm2(int n, double x[], int incx);
void dqrank(double a[], int lda, int m, int n, double tol, int* kr, void dqrank(double a[], int lda, int m, int n, double tol, int* kr,
int jpvt[], double qraux[]); int jpvt[], double qraux[]);
void dqrdc(double a[], int lda, int n, int p, double qraux[], int jpvt[], void dqrdc(double a[], int lda, int n, int p, double qraux[], int jpvt[],
double work[], int job); double work[], int job);
int dqrls(double a[], int lda, int m, int n, double tol, int* kr, double b[], int dqrls(double a[], int lda, int m, int n, double tol, int* kr, double b[],
double x[], double rsd[], int jpvt[], double qraux[], int itask); double x[], double rsd[], int jpvt[], double qraux[], int itask);
void dqrlss(double a[], int lda, int m, int n, int kr, double b[], double x[], void dqrlss(double a[], int lda, int m, int n, int kr, double b[], double x[],
double rsd[], int jpvt[], double qraux[]); double rsd[], int jpvt[], double qraux[]);
int dqrsl(double a[], int lda, int n, int k, double qraux[], double y[], int dqrsl(double a[], int lda, int n, int k, double qraux[], double y[],
double qy[], double qty[], double b[], double rsd[], double ab[], int job); double qy[], double qty[], double b[], double rsd[], double ab[], int job);
void dscal(int n, double sa, double x[], int incx); void dscal(int n, double sa, double x[], int incx);
void dswap(int n, double x[], int incx, double y[], int incy); void dswap(int n, double x[], int incx, double y[], int incy);
void qr_solve(double x[], int m, int n, double a[], double b[]); void qr_solve(double x[], int m, int n, double a[], double b[]);
#endif // ENABLED(AUTO_BED_LEVELING_GRID)
#endif // QR_SOLVE_H
#endif
MK/module/planner/vector_3.cpp 0 → 100644
View file @ 55a2c790
/**
* MK & MK4due 3D Printer Firmware
*
* Based on Marlin, Sprinter and grbl
* Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
* Copyright (C) 2013 - 2016 Alberto Cotronei @MagoKimbra
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
*/
/**
* vector_3.cpp - Vector library for bed leveling
* Copyright (c) 2012 Lars Brubaker. All right reserved.
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/
#include "../../base.h"
#if ENABLED(AUTO_BED_LEVELING_FEATURE)
#include <math.h>
#include "vector_3.h"
vector_3::vector_3() : x(0), y(0), z(0) { }
vector_3::vector_3(float x_, float y_, float z_) : x(x_), y(y_), z(z_) { }
vector_3 vector_3::cross(vector_3 left, vector_3 right) {
return vector_3(left.y * right.z - left.z * right.y,
left.z * right.x - left.x * right.z,
left.x * right.y - left.y * right.x);
}
vector_3 vector_3::operator+(vector_3 v) { return vector_3((x + v.x), (y + v.y), (z + v.z)); }
vector_3 vector_3::operator-(vector_3 v) { return vector_3((x - v.x), (y - v.y), (z - v.z)); }
vector_3 vector_3::get_normal() {
vector_3 normalized = vector_3(x, y, z);
normalized.normalize();
return normalized;
}
float vector_3::get_length() { return sqrt((x * x) + (y * y) + (z * z)); }
void vector_3::normalize() {
float length = get_length();
x /= length;
y /= length;
z /= length;
}
void vector_3::apply_rotation(matrix_3x3 matrix) {
float resultX = x * matrix.matrix[3 * 0 + 0] + y * matrix.matrix[3 * 1 + 0] + z * matrix.matrix[3 * 2 + 0];
float resultY = x * matrix.matrix[3 * 0 + 1] + y * matrix.matrix[3 * 1 + 1] + z * matrix.matrix[3 * 2 + 1];
float resultZ = x * matrix.matrix[3 * 0 + 2] + y * matrix.matrix[3 * 1 + 2] + z * matrix.matrix[3 * 2 + 2];
x = resultX;
y = resultY;
z = resultZ;
}
void vector_3::debug(const char title[]) {
ECHO_ST(DB, title);
ECHO_MV(" x: ", x, 6);
ECHO_MV(" y: ", y, 6);
ECHO_EMV(" z: ", z, 6);
}
void apply_rotation_xyz(matrix_3x3 matrix, float& x, float& y, float& z) {
vector_3 vector = vector_3(x, y, z);
vector.apply_rotation(matrix);
x = vector.x;
y = vector.y;
z = vector.z;
}
matrix_3x3 matrix_3x3::create_from_rows(vector_3 row_0, vector_3 row_1, vector_3 row_2) {
//row_0.debug("row_0");
//row_1.debug("row_1");
//row_2.debug("row_2");
matrix_3x3 new_matrix;
new_matrix.matrix[0] = row_0.x; new_matrix.matrix[1] = row_0.y; new_matrix.matrix[2] = row_0.z;
new_matrix.matrix[3] = row_1.x; new_matrix.matrix[4] = row_1.y; new_matrix.matrix[5] = row_1.z;
new_matrix.matrix[6] = row_2.x; new_matrix.matrix[7] = row_2.y; new_matrix.matrix[8] = row_2.z;
//new_matrix.debug("new_matrix");
return new_matrix;
}
void matrix_3x3::set_to_identity() {
matrix[0] = 1; matrix[1] = 0; matrix[2] = 0;
matrix[3] = 0; matrix[4] = 1; matrix[5] = 0;
matrix[6] = 0; matrix[7] = 0; matrix[8] = 1;
}
matrix_3x3 matrix_3x3::create_look_at(vector_3 target) {
vector_3 z_row = target.get_normal();
vector_3 x_row = vector_3(1, 0, -target.x / target.z).get_normal();
vector_3 y_row = vector_3::cross(z_row, x_row).get_normal();
// x_row.debug("x_row");
// y_row.debug("y_row");
// z_row.debug("z_row");
// create the matrix already correctly transposed
matrix_3x3 rot = matrix_3x3::create_from_rows(x_row, y_row, z_row);
// rot.debug("rot");
return rot;
}
matrix_3x3 matrix_3x3::transpose(matrix_3x3 original) {
matrix_3x3 new_matrix;
new_matrix.matrix[0] = original.matrix[0]; new_matrix.matrix[1] = original.matrix[3]; new_matrix.matrix[2] = original.matrix[6];
new_matrix.matrix[3] = original.matrix[1]; new_matrix.matrix[4] = original.matrix[4]; new_matrix.matrix[5] = original.matrix[7];
new_matrix.matrix[6] = original.matrix[2]; new_matrix.matrix[7] = original.matrix[5]; new_matrix.matrix[8] = original.matrix[8];
return new_matrix;
}
void matrix_3x3::debug(const char title[]) {
ECHO_LT(DB, title);
int count = 0;
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
if (matrix[count] >= 0.0) ECHO_C('+');
ECHO_V(matrix[count], 6);
ECHO_C(' ');
count++;
}
ECHO_E;
}
}
#endif // AUTO_BED_LEVELING_FEATURE
MK/module/motion/vector_3.h → MK/module/planner/vector_3.h
View file @ 55a2c790
...@@ -40,12 +40,12 @@ ...@@ -40,12 +40,12 @@
*/ */
#ifndef VECTOR_3_H #ifndef VECTOR_3_H
#define VECTOR_3_H #define VECTOR_3_H
#if ENABLED(AUTO_BED_LEVELING_FEATURE) #if ENABLED(AUTO_BED_LEVELING_FEATURE)
class matrix_3x3; class matrix_3x3;
struct vector_3 { struct vector_3 {
float x, y, z; float x, y, z;
vector_3(); vector_3();
...@@ -62,20 +62,22 @@ ...@@ -62,20 +62,22 @@
void debug(const char title[]); void debug(const char title[]);
void apply_rotation(matrix_3x3 matrix); void apply_rotation(matrix_3x3 matrix);
}; };
struct matrix_3x3 { struct matrix_3x3 {
float matrix[3][3]; float matrix[9];
static matrix_3x3 create_from_rows(vector_3 row_0, vector_3 row_1, vector_3 row_2); static matrix_3x3 create_from_rows(vector_3 row_0, vector_3 row_1, vector_3 row_2);
static matrix_3x3 create_look_at(vector_3 target); static matrix_3x3 create_look_at(vector_3 target);
static matrix_3x3 transpose(matrix_3x3 original); static matrix_3x3 transpose(matrix_3x3 original);
void set_to_identity(); void set_to_identity();
void debug(const char title[]); void debug(const char title[]);
}; };
void apply_rotation_xyz(matrix_3x3 rotationMatrix, float& x, float& y, float& z); void apply_rotation_xyz(matrix_3x3 rotationMatrix, float& x, float& y, float& z);
#endif // AUTO_BED_LEVELING_FEATURE
#endif // AUTO_BED_LEVELING_FEATURE
#endif // VECTOR_3_H #endif // VECTOR_3_H
MK/module/sanitycheck.h
View file @ 55a2c790
...@@ -588,9 +588,6 @@ ...@@ -588,9 +588,6 @@
#if DISABLED(Z_PROBE_OFFSET_FROM_EXTRUDER) #if DISABLED(Z_PROBE_OFFSET_FROM_EXTRUDER)
#error DEPENDENCY ERROR: Missing setting Z_PROBE_OFFSET_FROM_EXTRUDER #error DEPENDENCY ERROR: Missing setting Z_PROBE_OFFSET_FROM_EXTRUDER
#endif #endif
#if DISABLED(Z_RAISE_BEFORE_HOMING)
#error DEPENDENCY ERROR: Missing setting Z_RAISE_BEFORE_HOMING
#endif
#if DISABLED(Z_RAISE_BEFORE_PROBING) #if DISABLED(Z_RAISE_BEFORE_PROBING)
#error DEPENDENCY ERROR: Missing setting Z_RAISE_BEFORE_PROBING #error DEPENDENCY ERROR: Missing setting Z_RAISE_BEFORE_PROBING
#endif #endif
...@@ -1627,8 +1624,8 @@ ...@@ -1627,8 +1624,8 @@
#if Z_ENDSTOP_SERVO_NR < 0 #if Z_ENDSTOP_SERVO_NR < 0
#error DEPENDENCY ERROR: You must have Z_ENDSTOP_SERVO_NR set to at least 0 or above to use Z_PROBE_ENDSTOP. #error DEPENDENCY ERROR: You must have Z_ENDSTOP_SERVO_NR set to at least 0 or above to use Z_PROBE_ENDSTOP.
#endif #endif
#if DISABLED(SERVO_ENDSTOP_ANGLES) #if DISABLED(Z_ENDSTOP_SERVO_ANGLES)
#error DEPENDENCY ERROR: You must have SERVO_ENDSTOP_ANGLES EXIST for Z Extend and Retract to use Z_PROBE_ENDSTOP. #error DEPENDENCY ERROR: You must have Z_ENDSTOP_SERVO_ANGLES EXIST for Z Extend and Retract to use Z_PROBE_ENDSTOP.
#endif #endif
#endif #endif
#endif #endif
......
MK/module/temperature/temperature.cpp
View file @ 55a2c790
...@@ -330,9 +330,9 @@ unsigned char getPwmCooler(bool soft = true) { ...@@ -330,9 +330,9 @@ unsigned char getPwmCooler(bool soft = true) {
void autotempShutdown() { void autotempShutdown() {
#if ENABLED(AUTOTEMP) #if ENABLED(AUTOTEMP)
if (autotemp_enabled) { if (planner.autotemp_enabled) {
autotemp_enabled = false; planner.autotemp_enabled = false;
if (degTargetHotend(active_extruder) > autotemp_min) if (degTargetHotend(active_extruder) > planner.autotemp_min)
setTargetHotend(0, active_extruder); setTargetHotend(0, active_extruder);
} }
#endif #endif
...@@ -789,7 +789,7 @@ float get_pid_output(int h) { ...@@ -789,7 +789,7 @@ float get_pid_output(int h) {
lpq[lpq_ptr++] = 0; lpq[lpq_ptr++] = 0;
} }
if (lpq_ptr >= lpq_len) lpq_ptr = 0; if (lpq_ptr >= lpq_len) lpq_ptr = 0;
cTerm[0] = (lpq[lpq_ptr] / axis_steps_per_unit[E_AXIS + active_extruder]) * PID_PARAM(Kc, 0); cTerm[0] = (lpq[lpq_ptr] / planner.axis_steps_per_unit[E_AXIS + active_extruder]) * PID_PARAM(Kc, 0);
pid_output += cTerm[0] / 100.0; pid_output += cTerm[0] / 100.0;
#else #else
if (h == active_extruder) { if (h == active_extruder) {
...@@ -802,7 +802,7 @@ float get_pid_output(int h) { ...@@ -802,7 +802,7 @@ float get_pid_output(int h) {
lpq[lpq_ptr++] = 0; lpq[lpq_ptr++] = 0;
} }
if (lpq_ptr >= lpq_len) lpq_ptr = 0; if (lpq_ptr >= lpq_len) lpq_ptr = 0;
cTerm[h] = (lpq[lpq_ptr] / axis_steps_per_unit[E_AXIS + active_extruder]) * PID_PARAM(Kc, h); cTerm[h] = (lpq[lpq_ptr] / planner.axis_steps_per_unit[E_AXIS + active_extruder]) * PID_PARAM(Kc, h);
pid_output += cTerm[h] / 100.0; pid_output += cTerm[h] / 100.0;
} }
#endif // SINGLENOZZLE #endif // SINGLENOZZLE
......
MK/module/temperature/temperature.h
View file @ 55a2c790
...@@ -199,6 +199,12 @@ FORCE_INLINE bool isCoolingBed() { return target_temperature_bed < current_tempe ...@@ -199,6 +199,12 @@ FORCE_INLINE bool isCoolingBed() { return target_temperature_bed < current_tempe
FORCE_INLINE bool isCoolingChamber() { return target_temperature_chamber < current_temperature_chamber; } FORCE_INLINE bool isCoolingChamber() { return target_temperature_chamber < current_temperature_chamber; }
FORCE_INLINE bool isCoolingCooler() { return target_temperature_cooler < current_temperature_cooler; } FORCE_INLINE bool isCoolingCooler() { return target_temperature_cooler < current_temperature_cooler; }
#if ENABLED(PREVENT_DANGEROUS_EXTRUDE)
FORCE_INLINE bool tooColdToHotend(uint8_t h) { return degHotend(h) < extrude_min_temp; }
#else
FORCE_INLINE bool tooColdToHotend(uint8_t h) { UNUSED(h); return false; }
#endif
#define HOTEND_ROUTINES(NR) \ #define HOTEND_ROUTINES(NR) \
FORCE_INLINE float degHotend##NR() { return degHotend(NR); } \ FORCE_INLINE float degHotend##NR() { return degHotend(NR); } \
FORCE_INLINE float degTargetHotend##NR() { return degTargetHotend(NR); } \ FORCE_INLINE float degTargetHotend##NR() { return degTargetHotend(NR); } \
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment