microlab
/
scube
1
1
Fork 0
Code Issues Pull Requests Releases Activity

Reformat, rearrange, tweak, and document

Browse Source 
- Add comments to several functions
- Add an option to use Fast SQRT for Delta IK
- Group related functions together
- Outdent some leveling-related functions
2.0.x
Scott Lahteine 8 years ago
parent 61284cbd8c
commit b1a60e8954
1 changed files with 590 additions and 498 deletions
Show Stats Download Patch File Download Diff File

696
Marlin/Marlin_main.cpp
Unescape Escape View File

@ -2055,6 +2055,23 @@ static void clean_up_after_endstop_or_probe_move() {
#if ENABLED(AUTO_BED_LEVELING_FEATURE) #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) #if ENABLED(AUTO_BED_LEVELING_LINEAR)
/** /**
@ -2132,24 +2149,7 @@ static void clean_up_after_endstop_or_probe_move() {
} }
} }
#endif // DELTA #endif // AUTO_BED_LEVELING_NONLINEAR
/**
* 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
/** /**
* Home an individual axis * Home an individual axis
@ -2441,6 +2441,10 @@ bool position_is_reachable(float target[XYZ]) {
#endif #endif
} }
/**************************************************
***************** GCode Handlers *****************
**************************************************/
/** /**
* G0, G1: Coordinated movement of X Y Z E axes * G0, G1: Coordinated movement of X Y Z E axes
*/ */
@ -2589,16 +2593,12 @@ inline void gcode_G4() {
/** /**
* G20: Set input mode to inches * G20: Set input mode to inches
*/ */
inline void gcode_G20() { inline void gcode_G20() { set_input_linear_units(LINEARUNIT_INCH); }
set_input_linear_units(LINEARUNIT_INCH);
}
/** /**
* G21: Set input mode to millimeters * G21: Set input mode to millimeters
*/ */
inline void gcode_G21() { inline void gcode_G21() { set_input_linear_units(LINEARUNIT_MM); }
set_input_linear_units(LINEARUNIT_MM);
}
#endif #endif
#if ENABLED(NOZZLE_PARK_FEATURE) #if ENABLED(NOZZLE_PARK_FEATURE)
@ -3431,12 +3431,12 @@ inline void gcode_G28() {
#endif // AUTO_BED_LEVELING_LINEAR #endif // AUTO_BED_LEVELING_LINEAR
int probePointCounter = 0; 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, float yBase = front_probe_bed_position + yGridSpacing * yCount,
yProbe = floor(yBase + (yBase < 0 ? 0 : 0.5)); yProbe = floor(yBase + (yBase < 0 ? 0 : 0.5));
int xStart, xStop, xInc; int8_t xStart, xStop, xInc;
if (zig) { if (zig) {
xStart = 0; xStart = 0;
@ -3451,7 +3451,7 @@ inline void gcode_G28() {
zig = !zig; 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, float xBase = left_probe_bed_position + xGridSpacing * xCount,
xProbe = floor(xBase + (xBase < 0 ? 0 : 0.5)); 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); 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. // Raise to _Z_PROBE_DEPLOY_HEIGHT. Stow the probe.
if (STOW_PROBE()) return; if (STOW_PROBE()) return;
@ -3527,7 +3527,6 @@ inline void gcode_G28() {
#endif #endif
// Calculate leveling, print reports, correct the position // Calculate leveling, print reports, correct the position
#if ENABLED(AUTO_BED_LEVELING_GRID)
#if ENABLED(AUTO_BED_LEVELING_NONLINEAR) #if ENABLED(AUTO_BED_LEVELING_NONLINEAR)
if (!dryrun) extrapolate_unprobed_bed_level(); if (!dryrun) extrapolate_unprobed_bed_level();
@ -3580,12 +3579,11 @@ inline void gcode_G28() {
float min_diff = 999; float min_diff = 999;
for (int yy = auto_bed_leveling_grid_points - 1; yy >= 0; yy--) { for (int8_t yy = auto_bed_leveling_grid_points - 1; yy >= 0; yy--) {
for (int xx = 0; xx < auto_bed_leveling_grid_points; xx++) { for (uint8_t xx = 0; xx < auto_bed_leveling_grid_points; xx++) {
int ind = indexIntoAB[xx][yy]; int ind = indexIntoAB[xx][yy];
float diff = eqnBVector[ind] - mean; float diff = eqnBVector[ind] - mean,
x_tmp = eqnAMatrix[ind + 0 * abl2],
float x_tmp = eqnAMatrix[ind + 0 * abl2],
y_tmp = eqnAMatrix[ind + 1 * abl2], y_tmp = eqnAMatrix[ind + 1 * abl2],
z_tmp = 0; z_tmp = 0;
@ -3629,12 +3627,6 @@ inline void gcode_G28() {
} }
} //do_topography_map } //do_topography_map
#endif // AUTO_BED_LEVELING_LINEAR
#endif // AUTO_BED_LEVELING_GRID
#if ENABLED(AUTO_BED_LEVELING_LINEAR)
if (verbose_level > 0) if (verbose_level > 0)
planner.bed_level_matrix.debug("\n\nBed Level Correction Matrix:"); planner.bed_level_matrix.debug("\n\nBed Level Correction Matrix:");
@ -3682,7 +3674,7 @@ inline void gcode_G28() {
#endif #endif
} }
#endif // !DELTA #endif // AUTO_BED_LEVELING_LINEAR
#ifdef Z_PROBE_END_SCRIPT #ifdef Z_PROBE_END_SCRIPT
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
@ -3886,23 +3878,17 @@ inline void gcode_M17() {
/** /**
* M21: Init SD Card * M21: Init SD Card
*/ */
inline void gcode_M21() { inline void gcode_M21() { card.initsd(); }
card.initsd();
}
/** /**
* M22: Release SD Card * M22: Release SD Card
*/ */
inline void gcode_M22() { inline void gcode_M22() { card.release(); }
card.release();
}
/** /**
* M23: Open a file * M23: Open a file
*/ */
inline void gcode_M23() { inline void gcode_M23() { card.openFile(current_command_args, true); }
card.openFile(current_command_args, true);
}
/** /**
* M24: Start SD Print * M24: Start SD Print
@ -3915,9 +3901,7 @@ inline void gcode_M17() {
/** /**
* M25: Pause SD Print * M25: Pause SD Print
*/ */
inline void gcode_M25() { inline void gcode_M25() { card.pauseSDPrint(); }
card.pauseSDPrint();
}
/** /**
* M26: Set SD Card file index * M26: Set SD Card file index
@ -3930,16 +3914,12 @@ inline void gcode_M17() {
/** /**
* M27: Get SD Card status * M27: Get SD Card status
*/ */
inline void gcode_M27() { inline void gcode_M27() { card.getStatus(); }
card.getStatus();
}
/** /**
* M28: Start SD Write * M28: Start SD Write
*/ */
inline void gcode_M28() { inline void gcode_M28() { card.openFile(current_command_args, false); }
card.openFile(current_command_args, false);
}
/** /**
* M29: Stop SD Write * M29: Stop SD Write
@ -4318,7 +4298,8 @@ inline void gcode_M77() { print_job_timer.stop(); }
// "M78 S78" will reset the statistics // "M78 S78" will reset the statistics
if (code_seen('S') && code_value_int() == 78) if (code_seen('S') && code_value_int() == 78)
print_job_timer.initStats(); print_job_timer.initStats();
else print_job_timer.showStats(); else
print_job_timer.showStats();
} }
#endif #endif
@ -4885,7 +4866,7 @@ inline void gcode_M140() {
} }
} }
#endif #endif // ULTIPANEL
#if ENABLED(TEMPERATURE_UNITS_SUPPORT) #if ENABLED(TEMPERATURE_UNITS_SUPPORT)
/** /**
@ -4959,7 +4940,6 @@ inline void gcode_M81() {
#endif #endif
} }
/** /**
* M82: Set E codes absolute (default) * M82: Set E codes absolute (default)
*/ */
@ -5485,7 +5465,6 @@ inline void gcode_M221() {
inline void gcode_M226() { inline void gcode_M226() {
if (code_seen('P')) { if (code_seen('P')) {
int pin_number = code_value_int(); int pin_number = code_value_int();
int pin_state = code_seen('S') ? code_value_int() : -1; // required pin state - default is inverted int pin_state = code_seen('S') ? code_value_int() : -1; // required pin state - default is inverted
if (pin_state >= -1 && pin_state <= 1) { 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); 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*/) { 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 #if ENABLED(MIXING_EXTRUDER) && MIXING_VIRTUAL_TOOLS > 1
@ -7562,6 +7545,10 @@ ExitUnknownCommand:
ok_to_send(); ok_to_send();
} }
/**
* Send a "Resend: nnn" message to the host to
* indicate that a command needs to be re-sent.
*/
void FlushSerialRequestResend() { void FlushSerialRequestResend() {
//char command_queue[cmd_queue_index_r][100]="Resend:"; //char command_queue[cmd_queue_index_r][100]="Resend:";
MYSERIAL.flush(); MYSERIAL.flush();
@ -7570,6 +7557,15 @@ void FlushSerialRequestResend() {
ok_to_send(); 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() { void ok_to_send() {
refresh_cmd_timeout(); refresh_cmd_timeout();
if (!send_ok[cmd_queue_index_r]) return; 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) #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]) { void clamp_to_software_endstops(float target[XYZ]) {
#if ENABLED(min_software_endstops) #if ENABLED(min_software_endstops)
NOLESS(target[X_AXIS], soft_endstop_min[X_AXIS]); NOLESS(target[X_AXIS], soft_endstop_min[X_AXIS]);
@ -7607,6 +7606,10 @@ void ok_to_send() {
#if ENABLED(DELTA) #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) { 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_x = -SIN_60 * (radius + DELTA_RADIUS_TRIM_TOWER_1); // front left tower
delta_tower1_y = -COS_60 * (radius + DELTA_RADIUS_TRIM_TOWER_1); 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); delta_diagonal_rod_2_tower_3 = sq(diagonal_rod + delta_diagonal_rod_trim_tower_3);
} }
void inverse_kinematics(const float in_cartesian[XYZ]) { #if ENABLED(DELTA_FAST_SQRT)
/**
* 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] = { const float cartesian[XYZ] = {
RAW_X_POSITION(in_cartesian[X_AXIS]), RAW_X_POSITION(logical[X_AXIS]),
RAW_Y_POSITION(in_cartesian[Y_AXIS]), RAW_Y_POSITION(logical[Y_AXIS]),
RAW_Z_POSITION(in_cartesian[Z_AXIS]) RAW_Z_POSITION(logical[Z_AXIS])
}; };
delta[A_AXIS] = sqrt(delta_diagonal_rod_2_tower_1 // Macro to obtain the Z position of an individual tower
- sq(delta_tower1_x - cartesian[X_AXIS]) #define DELTA_Z(T) cartesian[Z_AXIS] + _SQRT( \
- sq(delta_tower1_y - cartesian[Y_AXIS]) delta_diagonal_rod_2_tower_##T - HYPOT2( \
) + cartesian[Z_AXIS]; delta_tower##T##_x - cartesian[X_AXIS], \
delta[B_AXIS] = sqrt(delta_diagonal_rod_2_tower_2 delta_tower##T##_y - cartesian[Y_AXIS] \
- 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];
/**
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]); delta[A_AXIS] = DELTA_Z(1);
SERIAL_ECHOPAIR(" b=", delta[B_AXIS]); delta[B_AXIS] = DELTA_Z(2);
SERIAL_ECHOLNPAIR(" c=", delta[C_AXIS]); 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 delta_safe_distance_from_top() {
float cartesian[XYZ] = { float cartesian[XYZ] = {
LOGICAL_X_POSITION(0), LOGICAL_X_POSITION(0),
@ -7663,70 +7714,77 @@ void ok_to_send() {
return abs(distance - delta[A_AXIS]); 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) { 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 // 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 }; float p12[3] = { delta_tower2_x - delta_tower1_x, delta_tower2_y - delta_tower1_y, z2 - z1 };
// Get the Magnitude of vector. // 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. // Create unit vector by dividing by magnitude.
float ex[3] = { p12[0] / d, p12[1] / d, p12[2] / d }; 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 }; 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]; 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 }; 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. // 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] }; float ey[3] = { p13[0] - iex[0], p13[1] - iex[1], p13[2] - iex[2] };
// The magnitude of Y component // The magnitude of Y component
float j = sqrt( sq(ey[0]) + sq(ey[1]) + sq(ey[2]) ); 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; ey[0] /= j; ey[1] /= j; ey[2] /= j;
// The cross product of the unit x and y is the unit z // The cross product of the unit x and y is the unit z
// float[] ez = vectorCrossProd(ex, ey); // 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],
//Now we have the d, i and j values defined in Wikipedia. ex[2] * ey[0] - ex[0] * ey[2],
//We can plug them into the equations defined in ex[0] * ey[1] - ex[1] * ey[0]
//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; // We now have the d, i and j values defined in Wikipedia.
float Znew = sqrt(delta_diagonal_rod_2_tower_1 - Xnew*Xnew - Ynew*Ynew); // 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),
//Now we can start from the origin in the old coords and Ynew = ((delta_diagonal_rod_2_tower_1 - delta_diagonal_rod_2_tower_3 + HYPOT2(i, j)) / 2 - i * Xnew) / j,
//add vectors in the old coords that represent the Znew = sqrt(delta_diagonal_rod_2_tower_1 - HYPOT2(Xnew, Ynew));
//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[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[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; cartes[Z_AXIS] = z1 + ex[2] * Xnew + ey[2] * Ynew - ez[2] * Znew;
@ -7780,6 +7838,42 @@ void ok_to_send() {
#endif // DELTA #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) { void set_current_from_steppers_for_axis(AxisEnum axis) {
#if ENABLED(AUTO_BED_LEVELING_LINEAR) #if ENABLED(AUTO_BED_LEVELING_LINEAR)
vector_3 pos = untilted_stepper_position(); vector_3 pos = untilted_stepper_position();
@ -7794,7 +7888,10 @@ void set_current_from_steppers_for_axis(AxisEnum axis) {
#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 /**
* 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) { 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)), int cx1 = mbl.cell_index_x(RAW_CURRENT_POSITION(X_AXIS)),
cy1 = mbl.cell_index_y(RAW_CURRENT_POSITION(Y_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)); memcpy(destination, end, sizeof(end));
mesh_line_to_destination(fr_mm_s, x_splits, y_splits); mesh_line_to_destination(fr_mm_s, x_splits, y_splits);
} }
#endif // MESH_BED_LEVELING #endif // MESH_BED_LEVELING
#if IS_KINEMATIC #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]) { inline bool prepare_kinematic_move_to(float target[NUM_AXIS]) {
float difference[NUM_AXIS]; float difference[NUM_AXIS];
LOOP_XYZE(i) difference[i] = target[i] - current_position[i]; 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; 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) #if ENABLED(DUAL_X_CARRIAGE)
/**
* Prepare a linear move in a dual X axis setup
*/
inline bool prepare_move_to_destination_dualx() { inline bool prepare_move_to_destination_dualx() {
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) {
@ -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 #endif // DUAL_X_CARRIAGE
#if !IS_KINEMATIC /**
* Prepare a single move and get ready for the next one
inline bool prepare_move_to_destination_cartesian() { *
// Do not use feedrate_percentage for E or Z only moves * This may result in several calls to planner.buffer_line to
if (current_position[X_AXIS] == destination[X_AXIS] && current_position[Y_AXIS] == destination[Y_AXIS]) { * do smaller moves for DELTA, SCARA, mesh moves, etc.
line_to_destination(); */
} void prepare_move_to_destination() {
else { clamp_to_software_endstops(destination);
#if ENABLED(MESH_BED_LEVELING) refresh_cmd_timeout();
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(PREVENT_COLD_EXTRUSION) #if ENABLED(PREVENT_COLD_EXTRUSION)
inline void prevent_dangerous_extrude(float& curr_e, float& dest_e) { if (!DEBUGGING(DRYRUN)) {
if (DEBUGGING(DRYRUN)) return; if (destination[E_AXIS] != current_position[E_AXIS]) {
float de = dest_e - curr_e;
if (de) {
if (thermalManager.tooColdToExtrude(active_extruder)) { 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_ECHO_START;
SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP); SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP);
} }
#if ENABLED(PREVENT_LENGTHY_EXTRUDE) #if ENABLED(PREVENT_LENGTHY_EXTRUDE)
if (labs(de) > EXTRUDE_MAXLENGTH) { if (labs(destination[E_AXIS] - current_position[E_AXIS]) > EXTRUDE_MAXLENGTH) {
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_ECHO_START;
SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP); 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 #endif
#if IS_KINEMATIC #if IS_KINEMATIC
@ -8281,26 +8384,6 @@ void prepare_move_to_destination() {
#endif // IS_SCARA #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) #if ENABLED(TEMP_STAT_LEDS)
static bool red_led = false; static bool red_led = false;
@ -8327,6 +8410,91 @@ void get_cartesian_from_steppers() {
#endif #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() { void enable_all_steppers() {
enable_x(); enable_x();
enable_y(); enable_y();
@ -8347,33 +8515,6 @@ void disable_all_steppers() {
disable_e3(); 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: * Manage several activities:
* - Check for Filament Runout * - Check for Filament Runout
@ -8551,6 +8692,37 @@ void manage_inactivity(bool ignore_stepper_queue/*=false*/) {
planner.check_axes_activity(); 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) { void kill(const char* lcd_msg) {
SERIAL_ERROR_START; SERIAL_ERROR_START;
SERIAL_ERRORLNPGM(MSG_ERR_KILLED); SERIAL_ERRORLNPGM(MSG_ERR_KILLED);
@ -8579,79 +8751,10 @@ void kill(const char* lcd_msg) {
} // Wait for reset } // Wait for reset
} }
#if ENABLED(FILAMENT_RUNOUT_SENSOR) /**
* Turn off heaters and stop the print in progress
void handle_filament_runout() { * After a stop the machine may be resumed with M999
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
void stop() { void stop() {
thermalManager.disable_all_heaters(); thermalManager.disable_all_heaters();
if (IsRunning()) { 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 * Marlin entry-point: Set up before the program loop
* - Set up the kill pin, filament runout, power hold * - Set up the kill pin, filament runout, power hold

Write Preview
Loading…
Cancel
Save