MPCNC/Firmware
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Add realtime delta geometry in Marlin_main.cpp.

Browse Source
This commit is contained in:
Johann Rocholl 2012-12-10 01:04:12 -08:00
parent cec7283b21
commit 8e2519e88b
2 changed files with 66 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
Marlin/Marlin.h
View File

@ -159,6 +159,7 @@ void FlushSerialRequestResend();
void ClearToSend();
void get_coordinates();
void calculate_delta(float cartesian[3]);
void prepare_move();
void kill();
void Stop();

95
Marlin/Marlin_main.cpp
View File

@ -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];
}