melch/Marlin
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Use arrays for delta tower parameters

Browse Source
This commit is contained in:
Scott Lahteine 2017-03-07 17:42:04 -06:00
parent 47242a40bb
commit 58b3e98878
3 changed files with 73 additions and 99 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
Marlin/Marlin.h
View File

@ -332,12 +332,8 @@ float code_value_temp_diff();
delta_radius,
delta_diagonal_rod,
delta_segments_per_second,
delta_diagonal_rod_trim_tower_1,
delta_diagonal_rod_trim_tower_2,
delta_diagonal_rod_trim_tower_3,
delta_tower_angle_trim_1,
delta_tower_angle_trim_2,
delta_tower_angle_trim_3,
delta_diagonal_rod_trim[ABC],
delta_tower_angle_trim[ABC],
delta_clip_start_height;
void recalc_delta_settings(float radius, float diagonal_rod);
#elif IS_SCARA

88
Marlin/Marlin_main.cpp
View File

@ -559,25 +559,15 @@ static uint8_t target_extruder;
float delta[ABC],
endstop_adj[ABC] = { 0 };
// these are the default values, can be overriden with M665
float delta_radius = DELTA_RADIUS,
delta_tower_angle_trim_1 = DELTA_TOWER_ANGLE_TRIM_1,
delta_tower_angle_trim_2 = DELTA_TOWER_ANGLE_TRIM_2,
delta_tower_angle_trim_3 = DELTA_TOWER_ANGLE_TRIM_3,
delta_tower1_x = -sin(RADIANS(60 - delta_tower_angle_trim_1)) * (delta_radius + DELTA_RADIUS_TRIM_TOWER_1), // front left tower
delta_tower1_y = -cos(RADIANS(60 - delta_tower_angle_trim_1)) * (delta_radius + DELTA_RADIUS_TRIM_TOWER_1),
delta_tower2_x = sin(RADIANS(60 + delta_tower_angle_trim_2)) * (delta_radius + DELTA_RADIUS_TRIM_TOWER_2), // front right tower
delta_tower2_y = -cos(RADIANS(60 + delta_tower_angle_trim_2)) * (delta_radius + DELTA_RADIUS_TRIM_TOWER_2),
delta_tower3_x = -sin(RADIANS( delta_tower_angle_trim_3)), // back middle tower
delta_tower3_y = cos(RADIANS( delta_tower_angle_trim_3)) * (delta_radius + DELTA_RADIUS_TRIM_TOWER_3),
delta_diagonal_rod = DELTA_DIAGONAL_ROD,
delta_diagonal_rod_trim_tower_1 = DELTA_DIAGONAL_ROD_TRIM_TOWER_1,
delta_diagonal_rod_trim_tower_2 = DELTA_DIAGONAL_ROD_TRIM_TOWER_2,
delta_diagonal_rod_trim_tower_3 = DELTA_DIAGONAL_ROD_TRIM_TOWER_3,
delta_diagonal_rod_2_tower_1 = sq(delta_diagonal_rod + delta_diagonal_rod_trim_tower_1),
delta_diagonal_rod_2_tower_2 = sq(delta_diagonal_rod + delta_diagonal_rod_trim_tower_2),
delta_diagonal_rod_2_tower_3 = sq(delta_diagonal_rod + delta_diagonal_rod_trim_tower_3),
delta_segments_per_second = DELTA_SEGMENTS_PER_SECOND,
// These values are loaded or reset at boot time when setup() calls
// Config_RetrieveSettings(), which calls recalc_delta_settings().
float delta_radius,
delta_tower_angle_trim[ABC],
delta_tower[ABC][2],
delta_diagonal_rod,
delta_diagonal_rod_trim[ABC],
delta_diagonal_rod_2_tower[ABC],
delta_segments_per_second,
delta_clip_start_height = Z_MAX_POS;
float delta_safe_distance_from_top();
@ -6334,12 +6324,12 @@ inline void gcode_M205() {
if (code_seen('L')) delta_diagonal_rod = code_value_linear_units();
if (code_seen('R')) delta_radius = code_value_linear_units();
if (code_seen('S')) delta_segments_per_second = code_value_float();
if (code_seen('A')) delta_diagonal_rod_trim_tower_1 = code_value_linear_units();
if (code_seen('B')) delta_diagonal_rod_trim_tower_2 = code_value_linear_units();
if (code_seen('C')) delta_diagonal_rod_trim_tower_3 = code_value_linear_units();
if (code_seen('I')) delta_tower_angle_trim_1 = code_value_linear_units();
if (code_seen('J')) delta_tower_angle_trim_2 = code_value_linear_units();
if (code_seen('K')) delta_tower_angle_trim_3 = code_value_linear_units();
if (code_seen('A')) delta_diagonal_rod_trim[A_AXIS] = code_value_linear_units();
if (code_seen('B')) delta_diagonal_rod_trim[B_AXIS] = code_value_linear_units();
if (code_seen('C')) delta_diagonal_rod_trim[C_AXIS] = code_value_linear_units();
if (code_seen('I')) delta_tower_angle_trim[A_AXIS] = code_value_linear_units();
if (code_seen('J')) delta_tower_angle_trim[B_AXIS] = code_value_linear_units();
if (code_seen('K')) delta_tower_angle_trim[C_AXIS] = code_value_linear_units();
recalc_delta_settings(delta_radius, delta_diagonal_rod);
}
/**
@ -9143,15 +9133,15 @@ void ok_to_send() {
* settings have been changed (e.g., by M665).
*/
void recalc_delta_settings(float radius, float diagonal_rod) {
delta_tower1_x = -sin(RADIANS(60 - delta_tower_angle_trim_1)) * (delta_radius + DELTA_RADIUS_TRIM_TOWER_1), // front left tower
delta_tower1_y = -cos(RADIANS(60 - delta_tower_angle_trim_1)) * (delta_radius + DELTA_RADIUS_TRIM_TOWER_1),
delta_tower2_x = sin(RADIANS(60 + delta_tower_angle_trim_2)) * (delta_radius + DELTA_RADIUS_TRIM_TOWER_2), // front right tower
delta_tower2_y = -cos(RADIANS(60 + delta_tower_angle_trim_2)) * (delta_radius + DELTA_RADIUS_TRIM_TOWER_2),
delta_tower3_x = -sin(RADIANS( delta_tower_angle_trim_3)), // back middle tower
delta_tower3_y = cos(RADIANS( delta_tower_angle_trim_3)) * (delta_radius + DELTA_RADIUS_TRIM_TOWER_3),
delta_diagonal_rod_2_tower_1 = sq(diagonal_rod + delta_diagonal_rod_trim_tower_1);
delta_diagonal_rod_2_tower_2 = sq(diagonal_rod + delta_diagonal_rod_trim_tower_2);
delta_diagonal_rod_2_tower_3 = sq(diagonal_rod + delta_diagonal_rod_trim_tower_3);
delta_tower[A_AXIS][X_AXIS] = -sin(RADIANS(60 - delta_tower_angle_trim[A_AXIS])) * (delta_radius + DELTA_RADIUS_TRIM_TOWER_1), // front left tower
delta_tower[A_AXIS][Y_AXIS] = -cos(RADIANS(60 - delta_tower_angle_trim[A_AXIS])) * (delta_radius + DELTA_RADIUS_TRIM_TOWER_1),
delta_tower[B_AXIS][X_AXIS] = sin(RADIANS(60 + delta_tower_angle_trim[B_AXIS])) * (delta_radius + DELTA_RADIUS_TRIM_TOWER_2), // front right tower
delta_tower[B_AXIS][Y_AXIS] = -cos(RADIANS(60 + delta_tower_angle_trim[B_AXIS])) * (delta_radius + DELTA_RADIUS_TRIM_TOWER_2),
delta_tower[C_AXIS][X_AXIS] = -sin(RADIANS( delta_tower_angle_trim[C_AXIS])) * (delta_radius + DELTA_RADIUS_TRIM_TOWER_3), // back middle tower
delta_tower[C_AXIS][Y_AXIS] = cos(RADIANS( delta_tower_angle_trim[C_AXIS])) * (delta_radius + DELTA_RADIUS_TRIM_TOWER_3),
delta_diagonal_rod_2_tower[A_AXIS] = sq(diagonal_rod + delta_diagonal_rod_trim[A_AXIS]);
delta_diagonal_rod_2_tower[B_AXIS] = sq(diagonal_rod + delta_diagonal_rod_trim[B_AXIS]);
delta_diagonal_rod_2_tower[C_AXIS] = sq(diagonal_rod + delta_diagonal_rod_trim[C_AXIS]);
}
#if ENABLED(DELTA_FAST_SQRT)
@ -9201,17 +9191,17 @@ void ok_to_send() {
*/
// Macro to obtain the Z position of an individual tower
#define DELTA_Z(T) raw[Z_AXIS] + _SQRT( \
delta_diagonal_rod_2_tower_##T - HYPOT2( \
delta_tower##T##_x - raw[X_AXIS], \
delta_tower##T##_y - raw[Y_AXIS] \
) \
#define DELTA_Z(T) raw[Z_AXIS] + _SQRT( \
delta_diagonal_rod_2_tower[T] - HYPOT2( \
delta_tower[T][X_AXIS] - raw[X_AXIS], \
delta_tower[T][Y_AXIS] - raw[Y_AXIS] \
) \
)
#define DELTA_RAW_IK() do { \
delta[A_AXIS] = DELTA_Z(1); \
delta[B_AXIS] = DELTA_Z(2); \
delta[C_AXIS] = DELTA_Z(3); \
delta[A_AXIS] = DELTA_Z(A_AXIS); \
delta[B_AXIS] = DELTA_Z(B_AXIS); \
delta[C_AXIS] = DELTA_Z(C_AXIS); \
} while(0)
#define DELTA_LOGICAL_IK() do { \
@ -9281,7 +9271,7 @@ void ok_to_send() {
*/
void forward_kinematics_DELTA(float z1, float z2, float z3) {
// 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_tower[B_AXIS][X_AXIS] - delta_tower[A_AXIS][X_AXIS], delta_tower[B_AXIS][Y_AXIS] - delta_tower[A_AXIS][Y_AXIS], z2 - z1 };
// Get the Magnitude of vector.
float d = sqrt( sq(p12[0]) + sq(p12[1]) + sq(p12[2]) );
@ -9290,7 +9280,7 @@ void ok_to_send() {
float ex[3] = { p12[0] / d, p12[1] / d, p12[2] / d };
// 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_tower[C_AXIS][X_AXIS] - delta_tower[A_AXIS][X_AXIS], delta_tower[C_AXIS][Y_AXIS] - delta_tower[A_AXIS][Y_AXIS], z3 - z1 };
// 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];
@ -9318,15 +9308,15 @@ void ok_to_send() {
// 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));
float Xnew = (delta_diagonal_rod_2_tower[A_AXIS] - delta_diagonal_rod_2_tower[B_AXIS] + sq(d)) / (d * 2),
Ynew = ((delta_diagonal_rod_2_tower[A_AXIS] - delta_diagonal_rod_2_tower[C_AXIS] + HYPOT2(i, j)) / 2 - i * Xnew) / j,
Znew = sqrt(delta_diagonal_rod_2_tower[A_AXIS] - HYPOT2(Xnew, Ynew));
// 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[X_AXIS] = delta_tower[A_AXIS][X_AXIS] + ex[0] * Xnew + ey[0] * Ynew - ez[0] * Znew;
cartes[Y_AXIS] = delta_tower[A_AXIS][Y_AXIS] + ex[1] * Xnew + ey[1] * Ynew - ez[1] * Znew;
cartes[Z_AXIS] = z1 + ex[2] * Xnew + ey[2] * Ynew - ez[2] * Znew;
}

76
Marlin/configuration_store.cpp
View File

@ -84,17 +84,17 @@
* 308 G29 L F bilinear_start (int x2)
* 312 bed_level_grid[][] (float x9, up to float x256) +988
*
* DELTA (if deltabot): 48 bytes
* 348 M666 XYZ endstop_adj (float x3)
* 360 M665 R delta_radius (float)
* 364 M665 L delta_diagonal_rod (float)
* 368 M665 S delta_segments_per_second (float)
* 372 M665 A delta_diagonal_rod_trim_tower_1 (float)
* 376 M665 B delta_diagonal_rod_trim_tower_2 (float)
* 380 M665 C delta_diagonal_rod_trim_tower_3 (float)
* 384 M665 I delta_tower_angle_trim_1 (float)
* 388 M665 J delta_tower_angle_trim_2 (float)
* 392 M665 K delta_tower_angle_trim_3 (float)
* DELTA (if deltabot): 48 bytes
* 348 M666 XYZ endstop_adj (float x3)
* 360 M665 R delta_radius (float)
* 364 M665 L delta_diagonal_rod (float)
* 368 M665 S delta_segments_per_second (float)
* 372 M665 A delta_diagonal_rod_trim[A] (float)
* 376 M665 B delta_diagonal_rod_trim[B] (float)
* 380 M665 C delta_diagonal_rod_trim[C] (float)
* 384 M665 I delta_tower_angle_trim[A] (float)
* 388 M665 J delta_tower_angle_trim[B] (float)
* 392 M665 K delta_tower_angle_trim[C] (float)
*
* Z_DUAL_ENDSTOPS (if not deltabot): 48 bytes
* 348 M666 Z z_endstop_adj (float)
@ -357,14 +357,10 @@ void Config_Postprocess() {
EEPROM_WRITE(delta_radius); // 1 float
EEPROM_WRITE(delta_diagonal_rod); // 1 float
EEPROM_WRITE(delta_segments_per_second); // 1 float
EEPROM_WRITE(delta_diagonal_rod_trim_tower_1); // 1 float
EEPROM_WRITE(delta_diagonal_rod_trim_tower_2); // 1 float
EEPROM_WRITE(delta_diagonal_rod_trim_tower_3); // 1 float
EEPROM_WRITE(delta_tower_angle_trim_1); // 1 float
EEPROM_WRITE(delta_tower_angle_trim_2); // 1 float
EEPROM_WRITE(delta_tower_angle_trim_3); // 1 float
EEPROM_WRITE(delta_diagonal_rod_trim); // 3 floats
EEPROM_WRITE(delta_tower_angle_trim); // 3 floats
#elif ENABLED(Z_DUAL_ENDSTOPS)
EEPROM_WRITE(z_endstop_adj); // 1 float
EEPROM_WRITE(z_endstop_adj); // 1 float
dummy = 0.0f;
for (uint8_t q = 11; q--;) EEPROM_WRITE(dummy);
#else
@ -681,16 +677,12 @@ void Config_Postprocess() {
}
#if ENABLED(DELTA)
EEPROM_READ(endstop_adj); // 3 floats
EEPROM_READ(delta_radius); // 1 float
EEPROM_READ(delta_diagonal_rod); // 1 float
EEPROM_READ(delta_segments_per_second); // 1 float
EEPROM_READ(delta_diagonal_rod_trim_tower_1); // 1 float
EEPROM_READ(delta_diagonal_rod_trim_tower_2); // 1 float
EEPROM_READ(delta_diagonal_rod_trim_tower_3); // 1 float
EEPROM_READ(delta_tower_angle_trim_1); // 1 float
EEPROM_READ(delta_tower_angle_trim_2); // 1 float
EEPROM_READ(delta_tower_angle_trim_3); // 1 float
EEPROM_READ(endstop_adj); // 3 floats
EEPROM_READ(delta_radius); // 1 float
EEPROM_READ(delta_diagonal_rod); // 1 float
EEPROM_READ(delta_segments_per_second); // 1 float
EEPROM_READ(delta_diagonal_rod_trim); // 3 floats
EEPROM_READ(delta_tower_angle_trim); // 3 floats
#elif ENABLED(Z_DUAL_ENDSTOPS)
EEPROM_READ(z_endstop_adj);
dummy = 0.0f;
@ -909,19 +901,15 @@ void Config_ResetDefault() {
#endif
#if ENABLED(DELTA)
const float adj[ABC] = DELTA_ENDSTOP_ADJ;
endstop_adj[A_AXIS] = adj[A_AXIS];
endstop_adj[B_AXIS] = adj[B_AXIS];
endstop_adj[C_AXIS] = adj[C_AXIS];
const float adj[ABC] = DELTA_ENDSTOP_ADJ,
drt[ABC] = { DELTA_DIAGONAL_ROD_TRIM_TOWER_1, DELTA_DIAGONAL_ROD_TRIM_TOWER_2, DELTA_DIAGONAL_ROD_TRIM_TOWER_3 },
dta[ABC] = { DELTA_TOWER_ANGLE_TRIM_1, DELTA_TOWER_ANGLE_TRIM_2, DELTA_TOWER_ANGLE_TRIM_3 };
COPY(endstop_adj, adj);
delta_radius = DELTA_RADIUS;
delta_diagonal_rod = DELTA_DIAGONAL_ROD;
delta_segments_per_second = DELTA_SEGMENTS_PER_SECOND;
delta_diagonal_rod_trim_tower_1 = DELTA_DIAGONAL_ROD_TRIM_TOWER_1;
delta_diagonal_rod_trim_tower_2 = DELTA_DIAGONAL_ROD_TRIM_TOWER_2;
delta_diagonal_rod_trim_tower_3 = DELTA_DIAGONAL_ROD_TRIM_TOWER_3;
delta_tower_angle_trim_1 = DELTA_TOWER_ANGLE_TRIM_1;
delta_tower_angle_trim_2 = DELTA_TOWER_ANGLE_TRIM_2;
delta_tower_angle_trim_3 = DELTA_TOWER_ANGLE_TRIM_3;
COPY(delta_diagonal_rod_trim, drt);
COPY(delta_tower_angle_trim, dta);
#elif ENABLED(Z_DUAL_ENDSTOPS)
z_endstop_adj = 0;
#endif
@ -1198,12 +1186,12 @@ void Config_ResetDefault() {
SERIAL_ECHOPAIR(" M665 L", delta_diagonal_rod);
SERIAL_ECHOPAIR(" R", delta_radius);
SERIAL_ECHOPAIR(" S", delta_segments_per_second);
SERIAL_ECHOPAIR(" A", delta_diagonal_rod_trim_tower_1);
SERIAL_ECHOPAIR(" B", delta_diagonal_rod_trim_tower_2);
SERIAL_ECHOPAIR(" C", delta_diagonal_rod_trim_tower_3);
SERIAL_ECHOPAIR(" I", delta_tower_angle_trim_1);
SERIAL_ECHOPAIR(" J", delta_tower_angle_trim_2);
SERIAL_ECHOPAIR(" K", delta_tower_angle_trim_3);
SERIAL_ECHOPAIR(" A", delta_diagonal_rod_trim[A_AXIS]);
SERIAL_ECHOPAIR(" B", delta_diagonal_rod_trim[B_AXIS]);
SERIAL_ECHOPAIR(" C", delta_diagonal_rod_trim[C_AXIS]);
SERIAL_ECHOPAIR(" I", delta_tower_angle_trim[A_AXIS]);
SERIAL_ECHOPAIR(" J", delta_tower_angle_trim[B_AXIS]);
SERIAL_ECHOPAIR(" K", delta_tower_angle_trim[C_AXIS]);
SERIAL_EOL;
#elif ENABLED(Z_DUAL_ENDSTOPS)
CONFIG_ECHO_START;