Reformat, rearrange, tweak, and document
- Add comments to several functions - Add an option to use Fast SQRT for Delta IK - Group related functions together - Outdent some leveling-related functions
This commit is contained in:
Scott Lahteine
parent
61284cbd8c
commit
b1a60e8954
@ -2055,6 +2055,23 @@ static void clean_up_after_endstop_or_probe_move() {
|
||||
|
||||
#if ENABLED(AUTO_BED_LEVELING_FEATURE)
|
||||
|
||||
/**
|
||||
* Reset calibration results to zero.
|
||||
*/
|
||||
void reset_bed_level() {
|
||||
#if ENABLED(DEBUG_LEVELING_FEATURE)
|
||||
if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("reset_bed_level");
|
||||
#endif
|
||||
#if ENABLED(AUTO_BED_LEVELING_LINEAR)
|
||||
planner.bed_level_matrix.set_to_identity();
|
||||
#elif ENABLED(AUTO_BED_LEVELING_NONLINEAR)
|
||||
memset(bed_level_grid, 0, sizeof(bed_level_grid));
|
||||
nonlinear_grid_spacing[X_AXIS] = nonlinear_grid_spacing[Y_AXIS] = 0;
|
||||
#endif
|
||||
}
|
||||
|
||||
#endif // AUTO_BED_LEVELING_FEATURE
|
||||
|
||||
#if ENABLED(AUTO_BED_LEVELING_LINEAR)
|
||||
|
||||
/**
|
||||
@ -2132,24 +2149,7 @@ static void clean_up_after_endstop_or_probe_move() {
|
||||
}
|
||||
}
|
||||
|
||||
#endif // DELTA
|
||||
|
||||
/**
|
||||
* Reset calibration results to zero.
|
||||
*/
|
||||
void reset_bed_level() {
|
||||
#if ENABLED(DEBUG_LEVELING_FEATURE)
|
||||
if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("reset_bed_level");
|
||||
#endif
|
||||
#if ENABLED(AUTO_BED_LEVELING_LINEAR)
|
||||
planner.bed_level_matrix.set_to_identity();
|
||||
#elif ENABLED(AUTO_BED_LEVELING_NONLINEAR)
|
||||
memset(bed_level_grid, 0, sizeof(bed_level_grid));
|
||||
nonlinear_grid_spacing[X_AXIS] = nonlinear_grid_spacing[Y_AXIS] = 0;
|
||||
#endif
|
||||
}
|
||||
|
||||
#endif // AUTO_BED_LEVELING_FEATURE
|
||||
#endif // AUTO_BED_LEVELING_NONLINEAR
|
||||
|
||||
/**
|
||||
* Home an individual axis
|
||||
@ -2441,6 +2441,10 @@ bool position_is_reachable(float target[XYZ]) {
|
||||
#endif
|
||||
}
|
||||
|
||||
/**************************************************
|
||||
***************** GCode Handlers *****************
|
||||
**************************************************/
|
||||
|
||||
/**
|
||||
* G0, G1: Coordinated movement of X Y Z E axes
|
||||
*/
|
||||
@ -2589,16 +2593,12 @@ inline void gcode_G4() {
|
||||
/**
|
||||
* G20: Set input mode to inches
|
||||
*/
|
||||
inline void gcode_G20() {
|
||||
set_input_linear_units(LINEARUNIT_INCH);
|
||||
}
|
||||
inline void gcode_G20() { set_input_linear_units(LINEARUNIT_INCH); }
|
||||
|
||||
/**
|
||||
* G21: Set input mode to millimeters
|
||||
*/
|
||||
inline void gcode_G21() {
|
||||
set_input_linear_units(LINEARUNIT_MM);
|
||||
}
|
||||
inline void gcode_G21() { set_input_linear_units(LINEARUNIT_MM); }
|
||||
#endif
|
||||
|
||||
#if ENABLED(NOZZLE_PARK_FEATURE)
|
||||
@ -3431,12 +3431,12 @@ inline void gcode_G28() {
|
||||
#endif // AUTO_BED_LEVELING_LINEAR
|
||||
|
||||
int probePointCounter = 0;
|
||||
bool zig = auto_bed_leveling_grid_points & 1; //always end at [RIGHT_PROBE_BED_POSITION, BACK_PROBE_BED_POSITION]
|
||||
uint8_t zig = auto_bed_leveling_grid_points & 1; //always end at [RIGHT_PROBE_BED_POSITION, BACK_PROBE_BED_POSITION]
|
||||
|
||||
for (int yCount = 0; yCount < auto_bed_leveling_grid_points; yCount++) {
|
||||
for (uint8_t yCount = 0; yCount < auto_bed_leveling_grid_points; yCount++) {
|
||||
float yBase = front_probe_bed_position + yGridSpacing * yCount,
|
||||
yProbe = floor(yBase + (yBase < 0 ? 0 : 0.5));
|
||||
int xStart, xStop, xInc;
|
||||
int8_t xStart, xStop, xInc;
|
||||
|
||||
if (zig) {
|
||||
xStart = 0;
|
||||
@ -3451,7 +3451,7 @@ inline void gcode_G28() {
|
||||
|
||||
zig = !zig;
|
||||
|
||||
for (int xCount = xStart; xCount != xStop; xCount += xInc) {
|
||||
for (uint8_t xCount = xStart; xCount != xStop; xCount += xInc) {
|
||||
float xBase = left_probe_bed_position + xGridSpacing * xCount,
|
||||
xProbe = floor(xBase + (xBase < 0 ? 0 : 0.5));
|
||||
|
||||
@ -3514,7 +3514,7 @@ inline void gcode_G28() {
|
||||
planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
|
||||
}
|
||||
|
||||
#endif // !AUTO_BED_LEVELING_GRID
|
||||
#endif // AUTO_BED_LEVELING_3POINT
|
||||
|
||||
// Raise to _Z_PROBE_DEPLOY_HEIGHT. Stow the probe.
|
||||
if (STOW_PROBE()) return;
|
||||
@ -3527,7 +3527,6 @@ inline void gcode_G28() {
|
||||
#endif
|
||||
|
||||
// Calculate leveling, print reports, correct the position
|
||||
#if ENABLED(AUTO_BED_LEVELING_GRID)
|
||||
#if ENABLED(AUTO_BED_LEVELING_NONLINEAR)
|
||||
|
||||
if (!dryrun) extrapolate_unprobed_bed_level();
|
||||
@ -3580,12 +3579,11 @@ inline void gcode_G28() {
|
||||
|
||||
float min_diff = 999;
|
||||
|
||||
for (int yy = auto_bed_leveling_grid_points - 1; yy >= 0; yy--) {
|
||||
for (int xx = 0; xx < auto_bed_leveling_grid_points; xx++) {
|
||||
for (int8_t yy = auto_bed_leveling_grid_points - 1; yy >= 0; yy--) {
|
||||
for (uint8_t xx = 0; xx < auto_bed_leveling_grid_points; xx++) {
|
||||
int ind = indexIntoAB[xx][yy];
|
||||
float diff = eqnBVector[ind] - mean;
|
||||
|
||||
float x_tmp = eqnAMatrix[ind + 0 * abl2],
|
||||
float diff = eqnBVector[ind] - mean,
|
||||
x_tmp = eqnAMatrix[ind + 0 * abl2],
|
||||
y_tmp = eqnAMatrix[ind + 1 * abl2],
|
||||
z_tmp = 0;
|
||||
|
||||
@ -3629,12 +3627,6 @@ inline void gcode_G28() {
|
||||
}
|
||||
} //do_topography_map
|
||||
|
||||
#endif // AUTO_BED_LEVELING_LINEAR
|
||||
|
||||
#endif // AUTO_BED_LEVELING_GRID
|
||||
|
||||
#if ENABLED(AUTO_BED_LEVELING_LINEAR)
|
||||
|
||||
if (verbose_level > 0)
|
||||
planner.bed_level_matrix.debug("\n\nBed Level Correction Matrix:");
|
||||
|
||||
@ -3682,7 +3674,7 @@ inline void gcode_G28() {
|
||||
#endif
|
||||
}
|
||||
|
||||
#endif // !DELTA
|
||||
#endif // AUTO_BED_LEVELING_LINEAR
|
||||
|
||||
#ifdef Z_PROBE_END_SCRIPT
|
||||
#if ENABLED(DEBUG_LEVELING_FEATURE)
|
||||
@ -3886,23 +3878,17 @@ inline void gcode_M17() {
|
||||
/**
|
||||
* M21: Init SD Card
|
||||
*/
|
||||
inline void gcode_M21() {
|
||||
card.initsd();
|
||||
}
|
||||
inline void gcode_M21() { card.initsd(); }
|
||||
|
||||
/**
|
||||
* M22: Release SD Card
|
||||
*/
|
||||
inline void gcode_M22() {
|
||||
card.release();
|
||||
}
|
||||
inline void gcode_M22() { card.release(); }
|
||||
|
||||
/**
|
||||
* M23: Open a file
|
||||
*/
|
||||
inline void gcode_M23() {
|
||||
card.openFile(current_command_args, true);
|
||||
}
|
||||
inline void gcode_M23() { card.openFile(current_command_args, true); }
|
||||
|
||||
/**
|
||||
* M24: Start SD Print
|
||||
@ -3915,9 +3901,7 @@ inline void gcode_M17() {
|
||||
/**
|
||||
* M25: Pause SD Print
|
||||
*/
|
||||
inline void gcode_M25() {
|
||||
card.pauseSDPrint();
|
||||
}
|
||||
inline void gcode_M25() { card.pauseSDPrint(); }
|
||||
|
||||
/**
|
||||
* M26: Set SD Card file index
|
||||
@ -3930,16 +3914,12 @@ inline void gcode_M17() {
|
||||
/**
|
||||
* M27: Get SD Card status
|
||||
*/
|
||||
inline void gcode_M27() {
|
||||
card.getStatus();
|
||||
}
|
||||
inline void gcode_M27() { card.getStatus(); }
|
||||
|
||||
/**
|
||||
* M28: Start SD Write
|
||||
*/
|
||||
inline void gcode_M28() {
|
||||
card.openFile(current_command_args, false);
|
||||
}
|
||||
inline void gcode_M28() { card.openFile(current_command_args, false); }
|
||||
|
||||
/**
|
||||
* M29: Stop SD Write
|
||||
@ -4318,7 +4298,8 @@ inline void gcode_M77() { print_job_timer.stop(); }
|
||||
// "M78 S78" will reset the statistics
|
||||
if (code_seen('S') && code_value_int() == 78)
|
||||
print_job_timer.initStats();
|
||||
else print_job_timer.showStats();
|
||||
else
|
||||
print_job_timer.showStats();
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -4885,7 +4866,7 @@ inline void gcode_M140() {
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
#endif // ULTIPANEL
|
||||
|
||||
#if ENABLED(TEMPERATURE_UNITS_SUPPORT)
|
||||
/**
|
||||
@ -4959,7 +4940,6 @@ inline void gcode_M81() {
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* M82: Set E codes absolute (default)
|
||||
*/
|
||||
@ -5485,7 +5465,6 @@ inline void gcode_M221() {
|
||||
inline void gcode_M226() {
|
||||
if (code_seen('P')) {
|
||||
int pin_number = code_value_int();
|
||||
|
||||
int pin_state = code_seen('S') ? code_value_int() : -1; // required pin state - default is inverted
|
||||
|
||||
if (pin_state >= -1 && pin_state <= 1) {
|
||||
@ -6536,6 +6515,10 @@ inline void invalid_extruder_error(const uint8_t &e) {
|
||||
SERIAL_ECHOLN(MSG_INVALID_EXTRUDER);
|
||||
}
|
||||
|
||||
/**
|
||||
* Perform a tool-change, which may result in moving the
|
||||
* previous tool out of the way and the new tool into place.
|
||||
*/
|
||||
void tool_change(const uint8_t tmp_extruder, const float fr_mm_s/*=0.0*/, bool no_move/*=false*/) {
|
||||
#if ENABLED(MIXING_EXTRUDER) && MIXING_VIRTUAL_TOOLS > 1
|
||||
|
||||
@ -7562,6 +7545,10 @@ ExitUnknownCommand:
|
||||
ok_to_send();
|
||||
}
|
||||
|
||||
/**
|
||||
* Send a "Resend: nnn" message to the host to
|
||||
* indicate that a command needs to be re-sent.
|
||||
*/
|
||||
void FlushSerialRequestResend() {
|
||||
//char command_queue[cmd_queue_index_r][100]="Resend:";
|
||||
MYSERIAL.flush();
|
||||
@ -7570,6 +7557,15 @@ void FlushSerialRequestResend() {
|
||||
ok_to_send();
|
||||
}
|
||||
|
||||
/**
|
||||
* Send an "ok" message to the host, indicating
|
||||
* that a command was successfully processed.
|
||||
*
|
||||
* If ADVANCED_OK is enabled also include:
|
||||
* N<int> Line number of the command, if any
|
||||
* P<int> Planner space remaining
|
||||
* B<int> Block queue space remaining
|
||||
*/
|
||||
void ok_to_send() {
|
||||
refresh_cmd_timeout();
|
||||
if (!send_ok[cmd_queue_index_r]) return;
|
||||
@ -7590,6 +7586,9 @@ void ok_to_send() {
|
||||
|
||||
#if ENABLED(min_software_endstops) || ENABLED(max_software_endstops)
|
||||
|
||||
/**
|
||||
* Constrain the given coordinates to the software endstops.
|
||||
*/
|
||||
void clamp_to_software_endstops(float target[XYZ]) {
|
||||
#if ENABLED(min_software_endstops)
|
||||
NOLESS(target[X_AXIS], soft_endstop_min[X_AXIS]);
|
||||
@ -7607,6 +7606,10 @@ void ok_to_send() {
|
||||
|
||||
#if ENABLED(DELTA)
|
||||
|
||||
/**
|
||||
* Recalculate factors used for delta kinematics whenever
|
||||
* settings have been changed (e.g., by M665).
|
||||
*/
|
||||
void recalc_delta_settings(float radius, float diagonal_rod) {
|
||||
delta_tower1_x = -SIN_60 * (radius + DELTA_RADIUS_TRIM_TOWER_1); // front left tower
|
||||
delta_tower1_y = -COS_60 * (radius + DELTA_RADIUS_TRIM_TOWER_1);
|
||||
@ -7619,37 +7622,85 @@ void ok_to_send() {
|
||||
delta_diagonal_rod_2_tower_3 = sq(diagonal_rod + delta_diagonal_rod_trim_tower_3);
|
||||
}
|
||||
|
||||
void inverse_kinematics(const float in_cartesian[XYZ]) {
|
||||
|
||||
const float cartesian[XYZ] = {
|
||||
RAW_X_POSITION(in_cartesian[X_AXIS]),
|
||||
RAW_Y_POSITION(in_cartesian[Y_AXIS]),
|
||||
RAW_Z_POSITION(in_cartesian[Z_AXIS])
|
||||
};
|
||||
|
||||
delta[A_AXIS] = sqrt(delta_diagonal_rod_2_tower_1
|
||||
- sq(delta_tower1_x - cartesian[X_AXIS])
|
||||
- sq(delta_tower1_y - cartesian[Y_AXIS])
|
||||
) + cartesian[Z_AXIS];
|
||||
delta[B_AXIS] = sqrt(delta_diagonal_rod_2_tower_2
|
||||
- sq(delta_tower2_x - cartesian[X_AXIS])
|
||||
- sq(delta_tower2_y - cartesian[Y_AXIS])
|
||||
) + cartesian[Z_AXIS];
|
||||
delta[C_AXIS] = sqrt(delta_diagonal_rod_2_tower_3
|
||||
- sq(delta_tower3_x - cartesian[X_AXIS])
|
||||
- sq(delta_tower3_y - cartesian[Y_AXIS])
|
||||
) + cartesian[Z_AXIS];
|
||||
#if ENABLED(DELTA_FAST_SQRT)
|
||||
/**
|
||||
SERIAL_ECHOPAIR("cartesian x=", cartesian[X_AXIS]);
|
||||
SERIAL_ECHOPAIR(" y=", cartesian[Y_AXIS]);
|
||||
SERIAL_ECHOLNPAIR(" z=", cartesian[Z_AXIS]);
|
||||
|
||||
SERIAL_ECHOPAIR("delta a=", delta[A_AXIS]);
|
||||
SERIAL_ECHOPAIR(" b=", delta[B_AXIS]);
|
||||
SERIAL_ECHOLNPAIR(" c=", delta[C_AXIS]);
|
||||
* Fast inverse sqrt from Quake III Arena
|
||||
* See: https://en.wikipedia.org/wiki/Fast_inverse_square_root
|
||||
*/
|
||||
float Q_rsqrt(float number) {
|
||||
long i;
|
||||
float x2, y;
|
||||
const float threehalfs = 1.5f;
|
||||
x2 = number * 0.5f;
|
||||
y = number;
|
||||
i = * ( long * ) &y; // evil floating point bit level hacking
|
||||
i = 0x5f3759df - ( i >> 1 ); // what the f***?
|
||||
y = * ( float * ) &i;
|
||||
y = y * ( threehalfs - ( x2 * y * y ) ); // 1st iteration
|
||||
// y = y * ( threehalfs - ( x2 * y * y ) ); // 2nd iteration, this can be removed
|
||||
return y;
|
||||
}
|
||||
|
||||
#define _SQRT(n) (1.0f / Q_rsqrt(n))
|
||||
|
||||
#else
|
||||
|
||||
#define _SQRT(n) sqrt(n)
|
||||
|
||||
#endif
|
||||
|
||||
/**
|
||||
* Delta Inverse Kinematics
|
||||
*
|
||||
* Calculate the tower positions for a given logical
|
||||
* position, storing the result in the delta[] array.
|
||||
*
|
||||
* This is an expensive calculation, requiring 3 square
|
||||
* roots per segmented linear move, and strains the limits
|
||||
* of a Mega2560 with a Graphical Display.
|
||||
*
|
||||
* Suggested optimizations include:
|
||||
*
|
||||
* - Disable the home_offset (M206) and/or position_shift (G92)
|
||||
* features to remove up to 12 float additions.
|
||||
*
|
||||
* - Use a fast-inverse-sqrt function and add the reciprocal.
|
||||
* (see above)
|
||||
*/
|
||||
void inverse_kinematics(const float logical[XYZ]) {
|
||||
|
||||
const float cartesian[XYZ] = {
|
||||
RAW_X_POSITION(logical[X_AXIS]),
|
||||
RAW_Y_POSITION(logical[Y_AXIS]),
|
||||
RAW_Z_POSITION(logical[Z_AXIS])
|
||||
};
|
||||
|
||||
// Macro to obtain the Z position of an individual tower
|
||||
#define DELTA_Z(T) cartesian[Z_AXIS] + _SQRT( \
|
||||
delta_diagonal_rod_2_tower_##T - HYPOT2( \
|
||||
delta_tower##T##_x - cartesian[X_AXIS], \
|
||||
delta_tower##T##_y - cartesian[Y_AXIS] \
|
||||
) \
|
||||
)
|
||||
|
||||
delta[A_AXIS] = DELTA_Z(1);
|
||||
delta[B_AXIS] = DELTA_Z(2);
|
||||
delta[C_AXIS] = DELTA_Z(3);
|
||||
|
||||
/*
|
||||
SERIAL_ECHOPAIR("cartesian X:", cartesian[X_AXIS]);
|
||||
SERIAL_ECHOPAIR(" Y:", cartesian[Y_AXIS]);
|
||||
SERIAL_ECHOLNPAIR(" Z:", cartesian[Z_AXIS]);
|
||||
SERIAL_ECHOPAIR("delta A:", delta[A_AXIS]);
|
||||
SERIAL_ECHOPAIR(" B:", delta[B_AXIS]);
|
||||
SERIAL_ECHOLNPAIR(" C:", delta[C_AXIS]);
|
||||
//*/
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate the highest Z position where the
|
||||
* effector has the full range of XY motion.
|
||||
*/
|
||||
float delta_safe_distance_from_top() {
|
||||
float cartesian[XYZ] = {
|
||||
LOGICAL_X_POSITION(0),
|
||||
@ -7663,70 +7714,77 @@ void ok_to_send() {
|
||||
return abs(distance - delta[A_AXIS]);
|
||||
}
|
||||
|
||||
/**
|
||||
* Delta Forward Kinematics
|
||||
*
|
||||
* See the Wikipedia article "Trilateration"
|
||||
* https://en.wikipedia.org/wiki/Trilateration
|
||||
*
|
||||
* Establish a new coordinate system in the plane of the
|
||||
* three carriage points. This system has its origin at
|
||||
* tower1, with tower2 on the X axis. Tower3 is in the X-Y
|
||||
* plane with a Z component of zero.
|
||||
* We will define unit vectors in this coordinate system
|
||||
* in our original coordinate system. Then when we calculate
|
||||
* the Xnew, Ynew and Znew values, we can translate back into
|
||||
* the original system by moving along those unit vectors
|
||||
* by the corresponding values.
|
||||
*
|
||||
* Variable names matched to Marlin, c-version, and avoid the
|
||||
* use of any vector library.
|
||||
*
|
||||
* by Andreas Hardtung 2016-06-07
|
||||
* based on a Java function from "Delta Robot Kinematics V3"
|
||||
* by Steve Graves
|
||||
*
|
||||
* The result is stored in the cartes[] array.
|
||||
*/
|
||||
void forward_kinematics_DELTA(float z1, float z2, float z3) {
|
||||
//As discussed in Wikipedia "Trilateration"
|
||||
//we are establishing a new coordinate
|
||||
//system in the plane of the three carriage points.
|
||||
//This system will have the origin at tower1 and
|
||||
//tower2 is on the x axis. tower3 is in the X-Y
|
||||
//plane with a Z component of zero. We will define unit
|
||||
//vectors in this coordinate system in our original
|
||||
//coordinate system. Then when we calculate the
|
||||
//Xnew, Ynew and Znew values, we can translate back into
|
||||
//the original system by moving along those unit vectors
|
||||
//by the corresponding values.
|
||||
// https://en.wikipedia.org/wiki/Trilateration
|
||||
|
||||
// Variable names matched to Marlin, c-version
|
||||
// and avoiding a vector library
|
||||
// by Andreas Hardtung 2016-06-7
|
||||
// based on a Java function from
|
||||
// "Delta Robot Kinematics by Steve Graves" V3
|
||||
|
||||
// Result is in cartes[].
|
||||
|
||||
// Create a vector in old coordinates along x axis of new coordinate
|
||||
float p12[3] = { delta_tower2_x - delta_tower1_x, delta_tower2_y - delta_tower1_y, z2 - z1 };
|
||||
|
||||
// Get the Magnitude of vector.
|
||||
float d = sqrt( p12[0]*p12[0] + p12[1]*p12[1] + p12[2]*p12[2] );
|
||||
float d = sqrt( sq(p12[0]) + sq(p12[1]) + sq(p12[2]) );
|
||||
|
||||
// Create unit vector by dividing by magnitude.
|
||||
float ex[3] = { p12[0] / d, p12[1] / d, p12[2] / d };
|
||||
|
||||
//Now find vector from the origin of the new system to the third point.
|
||||
// Get the vector from the origin of the new system to the third point.
|
||||
float p13[3] = { delta_tower3_x - delta_tower1_x, delta_tower3_y - delta_tower1_y, z3 - z1 };
|
||||
|
||||
//Now use dot product to find the component of this vector on the X axis.
|
||||
// Use the dot product to find the component of this vector on the X axis.
|
||||
float i = ex[0] * p13[0] + ex[1] * p13[1] + ex[2] * p13[2];
|
||||
|
||||
//Now create a vector along the x axis that represents the x component of p13.
|
||||
// Create a vector along the x axis that represents the x component of p13.
|
||||
float iex[3] = { ex[0] * i, ex[1] * i, ex[2] * i };
|
||||
|
||||
//Now subtract the X component away from the original vector leaving only the Y component. We use the
|
||||
// Subtract the X component from the original vector leaving only Y. We use the
|
||||
// variable that will be the unit vector after we scale it.
|
||||
float ey[3] = { p13[0] - iex[0], p13[1] - iex[1], p13[2] - iex[2] };
|
||||
|
||||
// The magnitude of Y component
|
||||
float j = sqrt( sq(ey[0]) + sq(ey[1]) + sq(ey[2]) );
|
||||
|
||||
//Now make vector a unit vector
|
||||
// Convert to a unit vector
|
||||
ey[0] /= j; ey[1] /= j; ey[2] /= j;
|
||||
|
||||
// The cross product of the unit x and y is the unit z
|
||||
// float[] ez = vectorCrossProd(ex, ey);
|
||||
float ez[3] = { ex[1]*ey[2] - ex[2]*ey[1], ex[2]*ey[0] - ex[0]*ey[2], ex[0]*ey[1] - ex[1]*ey[0] };
|
||||
float ez[3] = {
|
||||
ex[1] * ey[2] - ex[2] * ey[1],
|
||||
ex[2] * ey[0] - ex[0] * ey[2],
|
||||
ex[0] * ey[1] - ex[1] * ey[0]
|
||||
};
|
||||
|
||||
//Now we have the d, i and j values defined in Wikipedia.
|
||||
//We can plug them into the equations defined in
|
||||
//Wikipedia for Xnew, Ynew and Znew
|
||||
float Xnew = (delta_diagonal_rod_2_tower_1 - delta_diagonal_rod_2_tower_2 + d*d)/(d*2);
|
||||
float Ynew = ((delta_diagonal_rod_2_tower_1 - delta_diagonal_rod_2_tower_3 + i*i + j*j)/2 - i*Xnew) /j;
|
||||
float Znew = sqrt(delta_diagonal_rod_2_tower_1 - Xnew*Xnew - Ynew*Ynew);
|
||||
// We now have the d, i and j values defined in Wikipedia.
|
||||
// Plug them into the equations defined in Wikipedia for Xnew, Ynew and Znew
|
||||
float Xnew = (delta_diagonal_rod_2_tower_1 - delta_diagonal_rod_2_tower_2 + sq(d)) / (d * 2),
|
||||
Ynew = ((delta_diagonal_rod_2_tower_1 - delta_diagonal_rod_2_tower_3 + HYPOT2(i, j)) / 2 - i * Xnew) / j,
|
||||
Znew = sqrt(delta_diagonal_rod_2_tower_1 - HYPOT2(Xnew, Ynew));
|
||||
|
||||
//Now we can start from the origin in the old coords and
|
||||
//add vectors in the old coords that represent the
|
||||
//Xnew, Ynew and Znew to find the point in the old system
|
||||
// Start from the origin of the old coordinates and add vectors in the
|
||||
// old coords that represent the Xnew, Ynew and Znew to find the point
|
||||
// in the old system.
|
||||
cartes[X_AXIS] = delta_tower1_x + ex[0] * Xnew + ey[0] * Ynew - ez[0] * Znew;
|
||||
cartes[Y_AXIS] = delta_tower1_y + ex[1] * Xnew + ey[1] * Ynew - ez[1] * Znew;
|
||||
cartes[Z_AXIS] = z1 + ex[2] * Xnew + ey[2] * Ynew - ez[2] * Znew;
|
||||
@ -7780,6 +7838,42 @@ void ok_to_send() {
|
||||
|
||||
#endif // DELTA
|
||||
|
||||
/**
|
||||
* Get the stepper positions in the cartes[] array.
|
||||
* Forward kinematics are applied for DELTA and SCARA.
|
||||
*
|
||||
* The result is in the current coordinate space with
|
||||
* leveling applied. The coordinates need to be run through
|
||||
* unapply_leveling to obtain the "ideal" coordinates
|
||||
* suitable for current_position, etc.
|
||||
*/
|
||||
void get_cartesian_from_steppers() {
|
||||
#if ENABLED(DELTA)
|
||||
forward_kinematics_DELTA(
|
||||
stepper.get_axis_position_mm(A_AXIS),
|
||||
stepper.get_axis_position_mm(B_AXIS),
|
||||
stepper.get_axis_position_mm(C_AXIS)
|
||||
);
|
||||
#elif IS_SCARA
|
||||
forward_kinematics_SCARA(
|
||||
stepper.get_axis_position_degrees(A_AXIS),
|
||||
stepper.get_axis_position_degrees(B_AXIS)
|
||||
);
|
||||
cartes[Z_AXIS] = stepper.get_axis_position_mm(Z_AXIS);
|
||||
#else
|
||||
cartes[X_AXIS] = stepper.get_axis_position_mm(X_AXIS);
|
||||
cartes[Y_AXIS] = stepper.get_axis_position_mm(Y_AXIS);
|
||||
cartes[Z_AXIS] = stepper.get_axis_position_mm(Z_AXIS);
|
||||
#endif
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the current_position for an axis based on
|
||||
* the stepper positions, removing any leveling that
|
||||
* may have been applied.
|
||||
*
|
||||
* << INCOMPLETE! Still needs to unapply leveling! >>
|
||||
*/
|
||||
void set_current_from_steppers_for_axis(AxisEnum axis) {
|
||||
#if ENABLED(AUTO_BED_LEVELING_LINEAR)
|
||||
vector_3 pos = untilted_stepper_position();
|
||||
@ -7794,7 +7888,10 @@ void set_current_from_steppers_for_axis(AxisEnum axis) {
|
||||
|
||||
#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
|
||||
/**
|
||||
* Prepare a mesh-leveled linear move in a Cartesian setup,
|
||||
* splitting the move where it crosses mesh borders.
|
||||
*/
|
||||
void mesh_line_to_destination(float fr_mm_s, uint8_t x_splits = 0xff, uint8_t y_splits = 0xff) {
|
||||
int cx1 = mbl.cell_index_x(RAW_CURRENT_POSITION(X_AXIS)),
|
||||
cy1 = mbl.cell_index_y(RAW_CURRENT_POSITION(Y_AXIS)),
|
||||
@ -7849,10 +7946,17 @@ void mesh_line_to_destination(float fr_mm_s, uint8_t x_splits = 0xff, uint8_t y_
|
||||
memcpy(destination, end, sizeof(end));
|
||||
mesh_line_to_destination(fr_mm_s, x_splits, y_splits);
|
||||
}
|
||||
|
||||
#endif // MESH_BED_LEVELING
|
||||
|
||||
#if IS_KINEMATIC
|
||||
|
||||
/**
|
||||
* Prepare a linear move in a DELTA or SCARA setup.
|
||||
*
|
||||
* This calls planner.buffer_line several times, adding
|
||||
* small incremental moves for DELTA or SCARA.
|
||||
*/
|
||||
inline bool prepare_kinematic_move_to(float target[NUM_AXIS]) {
|
||||
float difference[NUM_AXIS];
|
||||
LOOP_XYZE(i) difference[i] = target[i] - current_position[i];
|
||||
@ -7890,10 +7994,37 @@ void mesh_line_to_destination(float fr_mm_s, uint8_t x_splits = 0xff, uint8_t y_
|
||||
return true;
|
||||
}
|
||||
|
||||
#endif // IS_KINEMATIC
|
||||
#else
|
||||
|
||||
/**
|
||||
* Prepare a linear move in a Cartesian setup.
|
||||
* If Mesh Bed Leveling is enabled, perform a mesh move.
|
||||
*/
|
||||
inline bool prepare_move_to_destination_cartesian() {
|
||||
// Do not use feedrate_percentage for E or Z only moves
|
||||
if (current_position[X_AXIS] == destination[X_AXIS] && current_position[Y_AXIS] == destination[Y_AXIS]) {
|
||||
line_to_destination();
|
||||
}
|
||||
else {
|
||||
#if ENABLED(MESH_BED_LEVELING)
|
||||
if (mbl.active()) {
|
||||
mesh_line_to_destination(MMS_SCALED(feedrate_mm_s));
|
||||
return false;
|
||||
}
|
||||
else
|
||||
#endif
|
||||
line_to_destination(MMS_SCALED(feedrate_mm_s));
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
#endif // !IS_KINEMATIC
|
||||
|
||||
#if ENABLED(DUAL_X_CARRIAGE)
|
||||
|
||||
/**
|
||||
* Prepare a linear move in a dual X axis setup
|
||||
*/
|
||||
inline bool prepare_move_to_destination_dualx() {
|
||||
if (active_extruder_parked) {
|
||||
if (dual_x_carriage_mode == DXC_DUPLICATION_MODE && active_extruder == 0) {
|
||||
@ -7936,42 +8067,28 @@ void mesh_line_to_destination(float fr_mm_s, uint8_t x_splits = 0xff, uint8_t y_
|
||||
|
||||
#endif // DUAL_X_CARRIAGE
|
||||
|
||||
#if !IS_KINEMATIC
|
||||
|
||||
inline bool prepare_move_to_destination_cartesian() {
|
||||
// Do not use feedrate_percentage for E or Z only moves
|
||||
if (current_position[X_AXIS] == destination[X_AXIS] && current_position[Y_AXIS] == destination[Y_AXIS]) {
|
||||
line_to_destination();
|
||||
}
|
||||
else {
|
||||
#if ENABLED(MESH_BED_LEVELING)
|
||||
if (mbl.active()) {
|
||||
mesh_line_to_destination(MMS_SCALED(feedrate_mm_s));
|
||||
return false;
|
||||
}
|
||||
else
|
||||
#endif
|
||||
line_to_destination(MMS_SCALED(feedrate_mm_s));
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
#endif // !IS_KINEMATIC
|
||||
/**
|
||||
* Prepare a single move and get ready for the next one
|
||||
*
|
||||
* This may result in several calls to planner.buffer_line to
|
||||
* do smaller moves for DELTA, SCARA, mesh moves, etc.
|
||||
*/
|
||||
void prepare_move_to_destination() {
|
||||
clamp_to_software_endstops(destination);
|
||||
refresh_cmd_timeout();
|
||||
|
||||
#if ENABLED(PREVENT_COLD_EXTRUSION)
|
||||
|
||||
inline void prevent_dangerous_extrude(float& curr_e, float& dest_e) {
|
||||
if (DEBUGGING(DRYRUN)) return;
|
||||
float de = dest_e - curr_e;
|
||||
if (de) {
|
||||
if (!DEBUGGING(DRYRUN)) {
|
||||
if (destination[E_AXIS] != current_position[E_AXIS]) {
|
||||
if (thermalManager.tooColdToExtrude(active_extruder)) {
|
||||
curr_e = dest_e; // Behave as if the move really took place, but ignore E part
|
||||
current_position[E_AXIS] = destination[E_AXIS]; // Behave as if the move really took place, but ignore E part
|
||||
SERIAL_ECHO_START;
|
||||
SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP);
|
||||
}
|
||||
#if ENABLED(PREVENT_LENGTHY_EXTRUDE)
|
||||
if (labs(de) > EXTRUDE_MAXLENGTH) {
|
||||
curr_e = dest_e; // Behave as if the move really took place, but ignore E part
|
||||
if (labs(destination[E_AXIS] - current_position[E_AXIS]) > EXTRUDE_MAXLENGTH) {
|
||||
current_position[E_AXIS] = destination[E_AXIS]; // Behave as if the move really took place, but ignore E part
|
||||
SERIAL_ECHO_START;
|
||||
SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP);
|
||||
}
|
||||
@ -7979,20 +8096,6 @@ void mesh_line_to_destination(float fr_mm_s, uint8_t x_splits = 0xff, uint8_t y_
|
||||
}
|
||||
}
|
||||
|
||||
#endif // PREVENT_COLD_EXTRUSION
|
||||
|
||||
/**
|
||||
* Prepare a single move and get ready for the next one
|
||||
*
|
||||
* (This may call planner.buffer_line several times to put
|
||||
* smaller moves into the planner for DELTA or SCARA.)
|
||||
*/
|
||||
void prepare_move_to_destination() {
|
||||
clamp_to_software_endstops(destination);
|
||||
refresh_cmd_timeout();
|
||||
|
||||
#if ENABLED(PREVENT_COLD_EXTRUSION)
|
||||
prevent_dangerous_extrude(current_position[E_AXIS], destination[E_AXIS]);
|
||||
#endif
|
||||
|
||||
#if IS_KINEMATIC
|
||||
@ -8281,26 +8384,6 @@ void prepare_move_to_destination() {
|
||||
|
||||
#endif // IS_SCARA
|
||||
|
||||
void get_cartesian_from_steppers() {
|
||||
#if ENABLED(DELTA)
|
||||
forward_kinematics_DELTA(
|
||||
stepper.get_axis_position_mm(A_AXIS),
|
||||
stepper.get_axis_position_mm(B_AXIS),
|
||||
stepper.get_axis_position_mm(C_AXIS)
|
||||
);
|
||||
#elif IS_SCARA
|
||||
forward_kinematics_SCARA(
|
||||
stepper.get_axis_position_degrees(A_AXIS),
|
||||
stepper.get_axis_position_degrees(B_AXIS)
|
||||
);
|
||||
cartes[Z_AXIS] = stepper.get_axis_position_mm(Z_AXIS);
|
||||
#else
|
||||
cartes[X_AXIS] = stepper.get_axis_position_mm(X_AXIS);
|
||||
cartes[Y_AXIS] = stepper.get_axis_position_mm(Y_AXIS);
|
||||
cartes[Z_AXIS] = stepper.get_axis_position_mm(Z_AXIS);
|
||||
#endif
|
||||
}
|
||||
|
||||
#if ENABLED(TEMP_STAT_LEDS)
|
||||
|
||||
static bool red_led = false;
|
||||
@ -8327,6 +8410,91 @@ void get_cartesian_from_steppers() {
|
||||
|
||||
#endif
|
||||
|
||||
#if ENABLED(FILAMENT_RUNOUT_SENSOR)
|
||||
|
||||
void handle_filament_runout() {
|
||||
if (!filament_ran_out) {
|
||||
filament_ran_out = true;
|
||||
enqueue_and_echo_commands_P(PSTR(FILAMENT_RUNOUT_SCRIPT));
|
||||
stepper.synchronize();
|
||||
}
|
||||
}
|
||||
|
||||
#endif // FILAMENT_RUNOUT_SENSOR
|
||||
|
||||
#if ENABLED(FAST_PWM_FAN)
|
||||
|
||||
void setPwmFrequency(uint8_t pin, int val) {
|
||||
val &= 0x07;
|
||||
switch (digitalPinToTimer(pin)) {
|
||||
#if defined(TCCR0A)
|
||||
case TIMER0A:
|
||||
case TIMER0B:
|
||||
// TCCR0B &= ~(_BV(CS00) | _BV(CS01) | _BV(CS02));
|
||||
// TCCR0B |= val;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TCCR1A)
|
||||
case TIMER1A:
|
||||
case TIMER1B:
|
||||
// TCCR1B &= ~(_BV(CS10) | _BV(CS11) | _BV(CS12));
|
||||
// TCCR1B |= val;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TCCR2)
|
||||
case TIMER2:
|
||||
case TIMER2:
|
||||
TCCR2 &= ~(_BV(CS10) | _BV(CS11) | _BV(CS12));
|
||||
TCCR2 |= val;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TCCR2A)
|
||||
case TIMER2A:
|
||||
case TIMER2B:
|
||||
TCCR2B &= ~(_BV(CS20) | _BV(CS21) | _BV(CS22));
|
||||
TCCR2B |= val;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TCCR3A)
|
||||
case TIMER3A:
|
||||
case TIMER3B:
|
||||
case TIMER3C:
|
||||
TCCR3B &= ~(_BV(CS30) | _BV(CS31) | _BV(CS32));
|
||||
TCCR3B |= val;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TCCR4A)
|
||||
case TIMER4A:
|
||||
case TIMER4B:
|
||||
case TIMER4C:
|
||||
TCCR4B &= ~(_BV(CS40) | _BV(CS41) | _BV(CS42));
|
||||
TCCR4B |= val;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TCCR5A)
|
||||
case TIMER5A:
|
||||
case TIMER5B:
|
||||
case TIMER5C:
|
||||
TCCR5B &= ~(_BV(CS50) | _BV(CS51) | _BV(CS52));
|
||||
TCCR5B |= val;
|
||||
break;
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
#endif // FAST_PWM_FAN
|
||||
|
||||
float calculate_volumetric_multiplier(float diameter) {
|
||||
if (!volumetric_enabled || diameter == 0) return 1.0;
|
||||
float d2 = diameter * 0.5;
|
||||
return 1.0 / (M_PI * d2 * d2);
|
||||
}
|
||||
|
||||
void calculate_volumetric_multipliers() {
|
||||
for (uint8_t i = 0; i < COUNT(filament_size); i++)
|
||||
volumetric_multiplier[i] = calculate_volumetric_multiplier(filament_size[i]);
|
||||
}
|
||||
|
||||
void enable_all_steppers() {
|
||||
enable_x();
|
||||
enable_y();
|
||||
@ -8347,33 +8515,6 @@ void disable_all_steppers() {
|
||||
disable_e3();
|
||||
}
|
||||
|
||||
/**
|
||||
* Standard idle routine keeps the machine alive
|
||||
*/
|
||||
void idle(
|
||||
#if ENABLED(FILAMENT_CHANGE_FEATURE)
|
||||
bool no_stepper_sleep/*=false*/
|
||||
#endif
|
||||
) {
|
||||
lcd_update();
|
||||
host_keepalive();
|
||||
manage_inactivity(
|
||||
#if ENABLED(FILAMENT_CHANGE_FEATURE)
|
||||
no_stepper_sleep
|
||||
#endif
|
||||
);
|
||||
|
||||
thermalManager.manage_heater();
|
||||
|
||||
#if ENABLED(PRINTCOUNTER)
|
||||
print_job_timer.tick();
|
||||
#endif
|
||||
|
||||
#if HAS_BUZZER && PIN_EXISTS(BEEPER)
|
||||
buzzer.tick();
|
||||
#endif
|
||||
}
|
||||
|
||||
/**
|
||||
* Manage several activities:
|
||||
* - Check for Filament Runout
|
||||
@ -8551,6 +8692,37 @@ void manage_inactivity(bool ignore_stepper_queue/*=false*/) {
|
||||
planner.check_axes_activity();
|
||||
}
|
||||
|
||||
/**
|
||||
* Standard idle routine keeps the machine alive
|
||||
*/
|
||||
void idle(
|
||||
#if ENABLED(FILAMENT_CHANGE_FEATURE)
|
||||
bool no_stepper_sleep/*=false*/
|
||||
#endif
|
||||
) {
|
||||
lcd_update();
|
||||
host_keepalive();
|
||||
manage_inactivity(
|
||||
#if ENABLED(FILAMENT_CHANGE_FEATURE)
|
||||
no_stepper_sleep
|
||||
#endif
|
||||
);
|
||||
|
||||
thermalManager.manage_heater();
|
||||
|
||||
#if ENABLED(PRINTCOUNTER)
|
||||
print_job_timer.tick();
|
||||
#endif
|
||||
|
||||
#if HAS_BUZZER && PIN_EXISTS(BEEPER)
|
||||
buzzer.tick();
|
||||
#endif
|
||||
}
|
||||
|
||||
/**
|
||||
* Kill all activity and lock the machine.
|
||||
* After this the machine will need to be reset.
|
||||
*/
|
||||
void kill(const char* lcd_msg) {
|
||||
SERIAL_ERROR_START;
|
||||
SERIAL_ERRORLNPGM(MSG_ERR_KILLED);
|
||||
@ -8579,79 +8751,10 @@ void kill(const char* lcd_msg) {
|
||||
} // Wait for reset
|
||||
}
|
||||
|
||||
#if ENABLED(FILAMENT_RUNOUT_SENSOR)
|
||||
|
||||
void handle_filament_runout() {
|
||||
if (!filament_ran_out) {
|
||||
filament_ran_out = true;
|
||||
enqueue_and_echo_commands_P(PSTR(FILAMENT_RUNOUT_SCRIPT));
|
||||
stepper.synchronize();
|
||||
}
|
||||
}
|
||||
|
||||
#endif // FILAMENT_RUNOUT_SENSOR
|
||||
|
||||
#if ENABLED(FAST_PWM_FAN)
|
||||
|
||||
void setPwmFrequency(uint8_t pin, int val) {
|
||||
val &= 0x07;
|
||||
switch (digitalPinToTimer(pin)) {
|
||||
#if defined(TCCR0A)
|
||||
case TIMER0A:
|
||||
case TIMER0B:
|
||||
// TCCR0B &= ~(_BV(CS00) | _BV(CS01) | _BV(CS02));
|
||||
// TCCR0B |= val;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TCCR1A)
|
||||
case TIMER1A:
|
||||
case TIMER1B:
|
||||
// TCCR1B &= ~(_BV(CS10) | _BV(CS11) | _BV(CS12));
|
||||
// TCCR1B |= val;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TCCR2)
|
||||
case TIMER2:
|
||||
case TIMER2:
|
||||
TCCR2 &= ~(_BV(CS10) | _BV(CS11) | _BV(CS12));
|
||||
TCCR2 |= val;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TCCR2A)
|
||||
case TIMER2A:
|
||||
case TIMER2B:
|
||||
TCCR2B &= ~(_BV(CS20) | _BV(CS21) | _BV(CS22));
|
||||
TCCR2B |= val;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TCCR3A)
|
||||
case TIMER3A:
|
||||
case TIMER3B:
|
||||
case TIMER3C:
|
||||
TCCR3B &= ~(_BV(CS30) | _BV(CS31) | _BV(CS32));
|
||||
TCCR3B |= val;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TCCR4A)
|
||||
case TIMER4A:
|
||||
case TIMER4B:
|
||||
case TIMER4C:
|
||||
TCCR4B &= ~(_BV(CS40) | _BV(CS41) | _BV(CS42));
|
||||
TCCR4B |= val;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TCCR5A)
|
||||
case TIMER5A:
|
||||
case TIMER5B:
|
||||
case TIMER5C:
|
||||
TCCR5B &= ~(_BV(CS50) | _BV(CS51) | _BV(CS52));
|
||||
TCCR5B |= val;
|
||||
break;
|
||||
#endif
|
||||
}
|
||||
}
|
||||
#endif // FAST_PWM_FAN
|
||||
|
||||
/**
|
||||
* Turn off heaters and stop the print in progress
|
||||
* After a stop the machine may be resumed with M999
|
||||
*/
|
||||
void stop() {
|
||||
thermalManager.disable_all_heaters();
|
||||
if (IsRunning()) {
|
||||
@ -8663,17 +8766,6 @@ void stop() {
|
||||
}
|
||||
}
|
||||
|
||||
float calculate_volumetric_multiplier(float diameter) {
|
||||
if (!volumetric_enabled || diameter == 0) return 1.0;
|
||||
float d2 = diameter * 0.5;
|
||||
return 1.0 / (M_PI * d2 * d2);
|
||||
}
|
||||
|
||||
void calculate_volumetric_multipliers() {
|
||||
for (uint8_t i = 0; i < COUNT(filament_size); i++)
|
||||
volumetric_multiplier[i] = calculate_volumetric_multiplier(filament_size[i]);
|
||||
}
|
||||
|
||||
/**
|
||||
* Marlin entry-point: Set up before the program loop
|
||||
* - Set up the kill pin, filament runout, power hold
|
||||
|
||||
Loading…
Reference in New Issue
Block a user