bissen21/Marlin_Firmware
1
0
Fork 0
Code Issues Pull Requests Projects Releases 4 Wiki Activity

Operate in Native Machine Space

Browse Source
This commit is contained in:
Scott Lahteine
2017-11-02 23:59:42 -05:00
parent 31f112cf58
commit f8393a0908
36 changed files with 449 additions and 489 deletions
Download Patch File Download Diff File Expand all files Collapse all files

140
Marlin/src/module/motion.cpp
View File

@ -73,7 +73,7 @@ bool relative_mode = false;
/**
* Cartesian Current Position
* Used to track the logical position as moves are queued.
* Used to track the native machine position as moves are queued.
* Used by 'line_to_current_position' to do a move after changing it.
* Used by 'SYNC_PLAN_POSITION_KINEMATIC' to update 'planner.position'.
*/
@ -197,20 +197,16 @@ void get_cartesian_from_steppers() {
stepper.get_axis_position_mm(B_AXIS),
stepper.get_axis_position_mm(C_AXIS)
);
cartes[X_AXIS] += LOGICAL_X_POSITION(0);
cartes[Y_AXIS] += LOGICAL_Y_POSITION(0);
cartes[Z_AXIS] += LOGICAL_Z_POSITION(0);
#elif IS_SCARA
forward_kinematics_SCARA(
stepper.get_axis_position_degrees(A_AXIS),
stepper.get_axis_position_degrees(B_AXIS)
);
cartes[X_AXIS] += LOGICAL_X_POSITION(0);
cartes[Y_AXIS] += LOGICAL_Y_POSITION(0);
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);
#if IS_SCARA
forward_kinematics_SCARA(
stepper.get_axis_position_degrees(A_AXIS),
stepper.get_axis_position_degrees(B_AXIS)
);
#else
cartes[X_AXIS] = stepper.get_axis_position_mm(X_AXIS);
cartes[Y_AXIS] = stepper.get_axis_position_mm(Y_AXIS);
#endif
cartes[Z_AXIS] = stepper.get_axis_position_mm(Z_AXIS);
#endif
}
@ -288,16 +284,16 @@ void line_to_destination(const float fr_mm_s) {
* Plan a move to (X, Y, Z) and set the current_position
* The final current_position may not be the one that was requested
*/
void do_blocking_move_to(const float &lx, const float &ly, const float &lz, const float &fr_mm_s/*=0.0*/) {
void do_blocking_move_to(const float &rx, const float &ry, const float &rz, const float &fr_mm_s/*=0.0*/) {
const float old_feedrate_mm_s = feedrate_mm_s;
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) print_xyz(PSTR(">>> do_blocking_move_to"), NULL, lx, ly, lz);
if (DEBUGGING(LEVELING)) print_xyz(PSTR(">>> do_blocking_move_to"), NULL, rx, ry, rz);
#endif
#if ENABLED(DELTA)
if (!position_is_reachable_xy(lx, ly)) return;
if (!position_is_reachable(rx, ry)) return;
feedrate_mm_s = fr_mm_s ? fr_mm_s : XY_PROBE_FEEDRATE_MM_S;
@ -309,10 +305,10 @@ void do_blocking_move_to(const float &lx, const float &ly, const float &lz, cons
// when in the danger zone
if (current_position[Z_AXIS] > delta_clip_start_height) {
if (lz > delta_clip_start_height) { // staying in the danger zone
destination[X_AXIS] = lx; // move directly (uninterpolated)
destination[Y_AXIS] = ly;
destination[Z_AXIS] = lz;
if (rz > delta_clip_start_height) { // staying in the danger zone
destination[X_AXIS] = rx; // move directly (uninterpolated)
destination[Y_AXIS] = ry;
destination[Z_AXIS] = rz;
prepare_uninterpolated_move_to_destination(); // set_current_from_destination()
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("danger zone move", current_position);
@ -328,23 +324,23 @@ void do_blocking_move_to(const float &lx, const float &ly, const float &lz, cons
}
}
if (lz > current_position[Z_AXIS]) { // raising?
destination[Z_AXIS] = lz;
if (rz > current_position[Z_AXIS]) { // raising?
destination[Z_AXIS] = rz;
prepare_uninterpolated_move_to_destination(); // set_current_from_destination()
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("z raise move", current_position);
#endif
}
destination[X_AXIS] = lx;
destination[Y_AXIS] = ly;
destination[X_AXIS] = rx;
destination[Y_AXIS] = ry;
prepare_move_to_destination(); // set_current_from_destination()
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("xy move", current_position);
#endif
if (lz < current_position[Z_AXIS]) { // lowering?
destination[Z_AXIS] = lz;
if (rz < current_position[Z_AXIS]) { // lowering?
destination[Z_AXIS] = rz;
prepare_uninterpolated_move_to_destination(); // set_current_from_destination()
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("z lower move", current_position);
@ -353,44 +349,44 @@ void do_blocking_move_to(const float &lx, const float &ly, const float &lz, cons
#elif IS_SCARA
if (!position_is_reachable_xy(lx, ly)) return;
if (!position_is_reachable(rx, ry)) return;
set_destination_from_current();
// If Z needs to raise, do it before moving XY
if (destination[Z_AXIS] < lz) {
destination[Z_AXIS] = lz;
if (destination[Z_AXIS] < rz) {
destination[Z_AXIS] = rz;
prepare_uninterpolated_move_to_destination(fr_mm_s ? fr_mm_s : homing_feedrate(Z_AXIS));
}
destination[X_AXIS] = lx;
destination[Y_AXIS] = ly;
destination[X_AXIS] = rx;
destination[Y_AXIS] = ry;
prepare_uninterpolated_move_to_destination(fr_mm_s ? fr_mm_s : XY_PROBE_FEEDRATE_MM_S);
// If Z needs to lower, do it after moving XY
if (destination[Z_AXIS] > lz) {
destination[Z_AXIS] = lz;
if (destination[Z_AXIS] > rz) {
destination[Z_AXIS] = rz;
prepare_uninterpolated_move_to_destination(fr_mm_s ? fr_mm_s : homing_feedrate(Z_AXIS));
}
#else
// If Z needs to raise, do it before moving XY
if (current_position[Z_AXIS] < lz) {
if (current_position[Z_AXIS] < rz) {
feedrate_mm_s = fr_mm_s ? fr_mm_s : homing_feedrate(Z_AXIS);
current_position[Z_AXIS] = lz;
current_position[Z_AXIS] = rz;
line_to_current_position();
}
feedrate_mm_s = fr_mm_s ? fr_mm_s : XY_PROBE_FEEDRATE_MM_S;
current_position[X_AXIS] = lx;
current_position[Y_AXIS] = ly;
current_position[X_AXIS] = rx;
current_position[Y_AXIS] = ry;
line_to_current_position();
// If Z needs to lower, do it after moving XY
if (current_position[Z_AXIS] > lz) {
if (current_position[Z_AXIS] > rz) {
feedrate_mm_s = fr_mm_s ? fr_mm_s : homing_feedrate(Z_AXIS);
current_position[Z_AXIS] = lz;
current_position[Z_AXIS] = rz;
line_to_current_position();
}
@ -404,14 +400,14 @@ void do_blocking_move_to(const float &lx, const float &ly, const float &lz, cons
if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("<<< do_blocking_move_to");
#endif
}
void do_blocking_move_to_x(const float &lx, const float &fr_mm_s/*=0.0*/) {
do_blocking_move_to(lx, current_position[Y_AXIS], current_position[Z_AXIS], fr_mm_s);
void do_blocking_move_to_x(const float &rx, const float &fr_mm_s/*=0.0*/) {
do_blocking_move_to(rx, current_position[Y_AXIS], current_position[Z_AXIS], fr_mm_s);
}
void do_blocking_move_to_z(const float &lz, const float &fr_mm_s/*=0.0*/) {
do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], lz, fr_mm_s);
void do_blocking_move_to_z(const float &rz, const float &fr_mm_s/*=0.0*/) {
do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], rz, fr_mm_s);
}
void do_blocking_move_to_xy(const float &lx, const float &ly, const float &fr_mm_s/*=0.0*/) {
do_blocking_move_to(lx, ly, current_position[Z_AXIS], fr_mm_s);
void do_blocking_move_to_xy(const float &rx, const float &ry, const float &fr_mm_s/*=0.0*/) {
do_blocking_move_to(rx, ry, current_position[Z_AXIS], fr_mm_s);
}
//
@ -521,26 +517,26 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
* This calls planner.buffer_line several times, adding
* small incremental moves for DELTA or SCARA.
*/
inline bool prepare_kinematic_move_to(float ltarget[XYZE]) {
inline bool prepare_kinematic_move_to(float rtarget[XYZE]) {
// Get the top feedrate of the move in the XY plane
const float _feedrate_mm_s = MMS_SCALED(feedrate_mm_s);
// If the move is only in Z/E don't split up the move
if (ltarget[X_AXIS] == current_position[X_AXIS] && ltarget[Y_AXIS] == current_position[Y_AXIS]) {
planner.buffer_line_kinematic(ltarget, _feedrate_mm_s, active_extruder);
if (rtarget[X_AXIS] == current_position[X_AXIS] && rtarget[Y_AXIS] == current_position[Y_AXIS]) {
planner.buffer_line_kinematic(rtarget, _feedrate_mm_s, active_extruder);
return false;
}
// Fail if attempting move outside printable radius
if (!position_is_reachable_xy(ltarget[X_AXIS], ltarget[Y_AXIS])) return true;
if (!position_is_reachable(rtarget[X_AXIS], rtarget[Y_AXIS])) return true;
// Get the cartesian distances moved in XYZE
const float difference[XYZE] = {
ltarget[X_AXIS] - current_position[X_AXIS],
ltarget[Y_AXIS] - current_position[Y_AXIS],
ltarget[Z_AXIS] - current_position[Z_AXIS],
ltarget[E_AXIS] - current_position[E_AXIS]
rtarget[X_AXIS] - current_position[X_AXIS],
rtarget[Y_AXIS] - current_position[Y_AXIS],
rtarget[Z_AXIS] - current_position[Z_AXIS],
rtarget[E_AXIS] - current_position[E_AXIS]
};
// Get the linear distance in XYZ
@ -588,9 +584,9 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
oldB = stepper.get_axis_position_degrees(B_AXIS);
#endif
// Get the logical current position as starting point
float logical[XYZE];
COPY(logical, current_position);
// Get the current position as starting point
float raw[XYZE];
COPY(raw, current_position);
// Drop one segment so the last move is to the exact target.
// If there's only 1 segment, loops will be skipped entirely.
@ -598,25 +594,25 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
// Calculate and execute the segments
for (uint16_t s = segments + 1; --s;) {
LOOP_XYZE(i) logical[i] += segment_distance[i];
LOOP_XYZE(i) raw[i] += segment_distance[i];
#if ENABLED(DELTA)
DELTA_LOGICAL_IK(); // Delta can inline its kinematics
DELTA_RAW_IK(); // Delta can inline its kinematics
#else
inverse_kinematics(logical);
inverse_kinematics(raw);
#endif
ADJUST_DELTA(logical); // Adjust Z if bed leveling is enabled
ADJUST_DELTA(raw); // Adjust Z if bed leveling is enabled
#if IS_SCARA && ENABLED(SCARA_FEEDRATE_SCALING)
// For SCARA scale the feed rate from mm/s to degrees/s
// Use ratio between the length of the move and the larger angle change
const float adiff = abs(delta[A_AXIS] - oldA),
bdiff = abs(delta[B_AXIS] - oldB);
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], logical[E_AXIS], max(adiff, bdiff) * feed_factor, active_extruder);
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], max(adiff, bdiff) * feed_factor, active_extruder);
oldA = delta[A_AXIS];
oldB = delta[B_AXIS];
#else
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], logical[E_AXIS], _feedrate_mm_s, active_extruder);
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], _feedrate_mm_s, active_extruder);
#endif
}
@ -626,13 +622,13 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
#if IS_SCARA && ENABLED(SCARA_FEEDRATE_SCALING)
// For SCARA scale the feed rate from mm/s to degrees/s
// With segments > 1 length is 1 segment, otherwise total length
inverse_kinematics(ltarget);
ADJUST_DELTA(ltarget);
inverse_kinematics(rtarget);
ADJUST_DELTA(rtarget);
const float adiff = abs(delta[A_AXIS] - oldA),
bdiff = abs(delta[B_AXIS] - oldB);
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], logical[E_AXIS], max(adiff, bdiff) * feed_factor, active_extruder);
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], max(adiff, bdiff) * feed_factor, active_extruder);
#else
planner.buffer_line_kinematic(ltarget, _feedrate_mm_s, active_extruder);
planner.buffer_line_kinematic(rtarget, _feedrate_mm_s, active_extruder);
#endif
return false;
@ -687,7 +683,7 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
float x_home_pos(const int extruder) {
if (extruder == 0)
return LOGICAL_X_POSITION(base_home_pos(X_AXIS));
return base_home_pos(X_AXIS);
else
/**
* In dual carriage mode the extruder offset provides an override of the
@ -695,7 +691,7 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
* This allows soft recalibration of the second extruder home position
* without firmware reflash (through the M218 command).
*/
return LOGICAL_X_POSITION(hotend_offset[X_AXIS][1] > 0 ? hotend_offset[X_AXIS][1] : X2_HOME_POS);
return hotend_offset[X_AXIS][1] > 0 ? hotend_offset[X_AXIS][1] : X2_HOME_POS;
}
/**
@ -740,13 +736,13 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
if (active_extruder == 0) {
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) {
SERIAL_ECHOPAIR("Set planner X", LOGICAL_X_POSITION(inactive_extruder_x_pos));
SERIAL_ECHOPAIR("Set planner X", inactive_extruder_x_pos);
SERIAL_ECHOLNPAIR(" ... Line to X", current_position[X_AXIS] + duplicate_extruder_x_offset);
}
#endif
// move duplicate extruder into correct duplication position.
planner.set_position_mm(
LOGICAL_X_POSITION(inactive_extruder_x_pos),
inactive_extruder_x_pos,
current_position[Y_AXIS],
current_position[Z_AXIS],
current_position[E_AXIS]
@ -970,7 +966,7 @@ void set_axis_is_at_home(const AxisEnum axis) {
#if ENABLED(MORGAN_SCARA)
scara_set_axis_is_at_home(axis);
#else
current_position[axis] = LOGICAL_POSITION(base_home_pos(axis), axis);
current_position[axis] = base_home_pos(axis);
#endif
/**