andrey
/
Marlin_FB4S
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Use arrays for delta tower parameters

pull/1/head
Scott Lahteine 8 years ago
parent
47242a40bb
commit
58b3e98878
3 changed files with 73 additions and 99 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 8
      Marlin/Marlin.h
  2. 88
      Marlin/Marlin_main.cpp
  3. 76
      Marlin/configuration_store.cpp

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;

Write Preview
Loading…
Cancel
Save