From 8e2519e88b29fc6dccf6600a7455e4e49a313aca Mon Sep 17 00:00:00 2001
From: Johann Rocholl <johann@rocholl.net>
Date: Mon, 10 Dec 2012 01:04:12 -0800
Subject: [PATCH] Add realtime delta geometry in Marlin_main.cpp.
---
Marlin/Marlin.h | 1 +
Marlin/Marlin_main.cpp | 95 +++++++++++++++++++++++++++++-------------
2 files changed, 66 insertions(+), 30 deletions(-)
diff --git a/Marlin/Marlin.h b/Marlin/Marlin.h
index a6aa43e00a..9b5af9c055 100644
--- a/Marlin/Marlin.h
+++ b/Marlin/Marlin.h
@@ -159,6 +159,7 @@ void FlushSerialRequestResend();
void ClearToSend();
void get_coordinates();
+void calculate_delta(float cartesian[3]);
void prepare_move();
void kill();
void Stop();
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 7cc574962a..a4d09b3724 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -169,6 +169,7 @@ int fanSpeed=0;
//===========================================================================
const char axis_codes[NUM_AXIS] = {'X', 'Y', 'Z', 'E'};
static float destination[NUM_AXIS] = { 0.0, 0.0, 0.0, 0.0};
+static float delta[3] = {0.0, 0.0, 0.0};
static float offset[3] = {0.0, 0.0, 0.0};
static bool home_all_axis = true;
static float feedrate = 1500.0, next_feedrate, saved_feedrate;
@@ -731,34 +732,25 @@ void process_commands()
feedrate = 0.0;
home_all_axis = !((code_seen(axis_codes[0])) || (code_seen(axis_codes[1])) || (code_seen(axis_codes[2])));
- #if Z_HOME_DIR > 0 // If homing away from BED do Z first
- if((home_all_axis) || (code_seen(axis_codes[Z_AXIS]))) {
- HOMEAXIS(Z);
- }
- #endif
-
#ifdef QUICK_HOME
- if((home_all_axis)||( code_seen(axis_codes[X_AXIS]) && code_seen(axis_codes[Y_AXIS])) ) //first diagonal move
+ if (home_all_axis) // Move all carriages up together until the first endstop is hit.
{
- current_position[X_AXIS] = 0;current_position[Y_AXIS] = 0;
-
+ current_position[X_AXIS] = 0;
+ current_position[Y_AXIS] = 0;
+ current_position[Z_AXIS] = 0;
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
- destination[X_AXIS] = 1.5 * X_MAX_LENGTH * X_HOME_DIR;destination[Y_AXIS] = 1.5 * Y_MAX_LENGTH * Y_HOME_DIR;
- feedrate = homing_feedrate[X_AXIS];
- if(homing_feedrate[Y_AXIS]<feedrate)
- feedrate =homing_feedrate[Y_AXIS];
+
+ destination[X_AXIS] = 1.5 * X_MAX_LENGTH * X_HOME_DIR;
+ destination[Y_AXIS] = 1.5 * Y_MAX_LENGTH * Y_HOME_DIR;
+ destination[Z_AXIS] = 1.5 * Z_MAX_LENGTH * Z_HOME_DIR;
+ feedrate = 1.732 * homing_feedrate[X_AXIS];
plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
st_synchronize();
-
- axis_is_at_home(X_AXIS);
- axis_is_at_home(Y_AXIS);
- plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
- destination[X_AXIS] = current_position[X_AXIS];
- destination[Y_AXIS] = current_position[Y_AXIS];
- plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
- feedrate = 0.0;
- st_synchronize();
endstops_hit_on_purpose();
+
+ current_position[X_AXIS] = destination[X_AXIS];
+ current_position[Y_AXIS] = destination[Y_AXIS];
+ current_position[Z_AXIS] = destination[Z_AXIS];
}
#endif
@@ -771,11 +763,9 @@ void process_commands()
HOMEAXIS(Y);
}
- #if Z_HOME_DIR < 0 // If homing towards BED do Z last
if((home_all_axis) || (code_seen(axis_codes[Z_AXIS]))) {
HOMEAXIS(Z);
}
- #endif
if(code_seen(axis_codes[X_AXIS]))
{
@@ -795,7 +785,8 @@ void process_commands()
current_position[Z_AXIS]=code_value()+add_homeing[2];
}
}
- plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+ calculate_delta(current_position);
+ plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]);
#ifdef ENDSTOPS_ONLY_FOR_HOMING
enable_endstops(false);
@@ -1688,18 +1679,62 @@ void clamp_to_software_endstops(float target[3])
}
}
+void calculate_delta(float cartesian[3])
+{
+ delta[X_AXIS] = sqrt(sq(DELTA_DIAGONAL_ROD)
+ - sq(DELTA_TOWER1_X-cartesian[X_AXIS])
+ - sq(DELTA_TOWER1_Y-cartesian[Y_AXIS])
+ ) + cartesian[Z_AXIS];
+ delta[Y_AXIS] = sqrt(sq(DELTA_DIAGONAL_ROD)
+ - sq(DELTA_TOWER2_X-cartesian[X_AXIS])
+ - sq(DELTA_TOWER2_Y-cartesian[Y_AXIS])
+ ) + cartesian[Z_AXIS];
+ delta[Z_AXIS] = sqrt(sq(DELTA_DIAGONAL_ROD)
+ - sq(DELTA_TOWER3_X-cartesian[X_AXIS])
+ - sq(DELTA_TOWER3_Y-cartesian[Y_AXIS])
+ ) + cartesian[Z_AXIS];
+ /*
+ SERIAL_ECHOPGM("cartesian x="); SERIAL_ECHO(cartesian[X_AXIS]);
+ SERIAL_ECHOPGM(" y="); SERIAL_ECHO(cartesian[Y_AXIS]);
+ SERIAL_ECHOPGM(" z="); SERIAL_ECHOLN(cartesian[Z_AXIS]);
+
+ SERIAL_ECHOPGM("delta x="); SERIAL_ECHO(delta[X_AXIS]);
+ SERIAL_ECHOPGM(" y="); SERIAL_ECHO(delta[Y_AXIS]);
+ SERIAL_ECHOPGM(" z="); SERIAL_ECHOLN(delta[Z_AXIS]);
+ */
+}
+
void prepare_move()
{
clamp_to_software_endstops(destination);
previous_millis_cmd = millis();
- // Do not use feedmultiply for E or Z only moves
- if( (current_position[X_AXIS] == destination [X_AXIS]) && (current_position[Y_AXIS] == destination [Y_AXIS])) {
- plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+
+ float difference[NUM_AXIS];
+ for (int8_t i=0; i < NUM_AXIS; i++) {
+ difference[i] = destination[i] - current_position[i];
}
- else {
- plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate*feedmultiply/60/100.0, active_extruder);
+ float cartesian_mm = sqrt(sq(difference[X_AXIS]) +
+ sq(difference[Y_AXIS]) +
+ sq(difference[Z_AXIS]));
+ if (cartesian_mm < 0.000001) { cartesian_mm = abs(difference[E_AXIS]); }
+ if (cartesian_mm < 0.000001) { return; }
+ float seconds = 6000 * cartesian_mm / feedrate / feedmultiply;
+ int steps = max(1, int(DELTA_SEGMENTS_PER_SECOND * seconds));
+ // SERIAL_ECHOPGM("mm="); SERIAL_ECHO(cartesian_mm);
+ // SERIAL_ECHOPGM(" seconds="); SERIAL_ECHO(seconds);
+ // SERIAL_ECHOPGM(" steps="); SERIAL_ECHOLN(steps);
+ for (int s = 1; s <= steps; s++) {
+ float fraction = float(s) / float(steps);
+ for(int8_t i=0; i < NUM_AXIS; i++) {
+ destination[i] = current_position[i] + difference[i] * fraction;
+ }
+ calculate_delta(destination);
+ plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS],
+ destination[E_AXIS], feedrate*feedmultiply/60/100.0,
+ active_extruder);
}
+
for(int8_t i=0; i < NUM_AXIS; i++) {
current_position[i] = destination[i];
}