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

Fixes for COREXY and COREXZ coordinates

Browse Source
This commit is contained in:
Scott Lahteine 2016-02-11 18:17:17 -08:00
parent 159ff94afd
commit 373e8a1ad4
3 changed files with 75 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
Marlin/Marlin_main.cpp
View File

@ -1321,7 +1321,7 @@ static void setup_for_endstop_move() {
st_synchronize(); st_synchronize();
// Tell the planner where we ended up - Get this from the stepper handler // Tell the planner where we ended up - Get this from the stepper handler
zPosition = st_get_position_mm(Z_AXIS); zPosition = st_get_axis_position_mm(Z_AXIS);
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS]); plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS]);
// move up the retract distance // move up the retract distance
@ -1339,7 +1339,7 @@ static void setup_for_endstop_move() {
endstops_hit_on_purpose(); // clear endstop hit flags endstops_hit_on_purpose(); // clear endstop hit flags
// Get the current stepper position after bumping an endstop // Get the current stepper position after bumping an endstop
current_position[Z_AXIS] = st_get_position_mm(Z_AXIS); current_position[Z_AXIS] = st_get_axis_position_mm(Z_AXIS);
sync_plan_position(); sync_plan_position();
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
@ -3143,7 +3143,7 @@ inline void gcode_G28() {
float x_tmp = current_position[X_AXIS] + X_PROBE_OFFSET_FROM_EXTRUDER, float x_tmp = current_position[X_AXIS] + X_PROBE_OFFSET_FROM_EXTRUDER,
y_tmp = current_position[Y_AXIS] + Y_PROBE_OFFSET_FROM_EXTRUDER, y_tmp = current_position[Y_AXIS] + Y_PROBE_OFFSET_FROM_EXTRUDER,
z_tmp = current_position[Z_AXIS], z_tmp = current_position[Z_AXIS],
real_z = st_get_position_mm(Z_AXIS); //get the real Z (since plan_get_position is now correcting the plane) real_z = st_get_axis_position_mm(Z_AXIS); //get the real Z (since plan_get_position is now correcting the plane)
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
if (marlin_debug_flags & DEBUG_LEVELING) { if (marlin_debug_flags & DEBUG_LEVELING) {
@ -3585,10 +3585,10 @@ inline void gcode_M42() {
} }
} }
double X_current = st_get_position_mm(X_AXIS), double X_current = st_get_axis_position_mm(X_AXIS),
Y_current = st_get_position_mm(Y_AXIS), Y_current = st_get_axis_position_mm(Y_AXIS),
Z_current = st_get_position_mm(Z_AXIS), Z_current = st_get_axis_position_mm(Z_AXIS),
E_current = st_get_position_mm(E_AXIS), E_current = st_get_axis_position_mm(E_AXIS),
X_probe_location = X_current, Y_probe_location = Y_current, X_probe_location = X_current, Y_probe_location = Y_current,
Z_start_location = Z_current + Z_RAISE_BEFORE_PROBING; Z_start_location = Z_current + Z_RAISE_BEFORE_PROBING;
@ -3642,10 +3642,10 @@ inline void gcode_M42() {
active_extruder); active_extruder);
st_synchronize(); st_synchronize();
current_position[X_AXIS] = X_current = st_get_position_mm(X_AXIS); current_position[X_AXIS] = X_current = st_get_axis_position_mm(X_AXIS);
current_position[Y_AXIS] = Y_current = st_get_position_mm(Y_AXIS); current_position[Y_AXIS] = Y_current = st_get_axis_position_mm(Y_AXIS);
current_position[Z_AXIS] = Z_current = st_get_position_mm(Z_AXIS); current_position[Z_AXIS] = Z_current = st_get_axis_position_mm(Z_AXIS);
current_position[E_AXIS] = E_current = st_get_position_mm(E_AXIS); current_position[E_AXIS] = E_current = st_get_axis_position_mm(E_AXIS);
// //
// OK, do the initial probe to get us close to the bed. // OK, do the initial probe to get us close to the bed.
@ -3657,15 +3657,15 @@ inline void gcode_M42() {
setup_for_endstop_move(); setup_for_endstop_move();
run_z_probe(); run_z_probe();
current_position[Z_AXIS] = Z_current = st_get_position_mm(Z_AXIS); Z_current = current_position[Z_AXIS] = st_get_axis_position_mm(Z_AXIS);
Z_start_location = st_get_position_mm(Z_AXIS) + Z_RAISE_BEFORE_PROBING; Z_start_location = Z_current + Z_RAISE_BEFORE_PROBING;
plan_buffer_line(X_probe_location, Y_probe_location, Z_start_location, plan_buffer_line(X_probe_location, Y_probe_location, Z_start_location,
E_current, E_current,
homing_feedrate[X_AXIS] / 60, homing_feedrate[X_AXIS] / 60,
active_extruder); active_extruder);
st_synchronize(); st_synchronize();
current_position[Z_AXIS] = Z_current = st_get_position_mm(Z_AXIS); Z_current = current_position[Z_AXIS] = st_get_axis_position_mm(Z_AXIS);
if (deploy_probe_for_each_reading) stow_z_probe(); if (deploy_probe_for_each_reading) stow_z_probe();

2
Marlin/planner.cpp
View File

@ -1005,7 +1005,7 @@ float junction_deviation = 0.1;
#if ENABLED(AUTO_BED_LEVELING_FEATURE) && DISABLED(DELTA) #if ENABLED(AUTO_BED_LEVELING_FEATURE) && DISABLED(DELTA)
vector_3 plan_get_position() { vector_3 plan_get_position() {
vector_3 position = vector_3(st_get_position_mm(X_AXIS), st_get_position_mm(Y_AXIS), st_get_position_mm(Z_AXIS)); vector_3 position = vector_3(st_get_axis_position_mm(X_AXIS), st_get_axis_position_mm(Y_AXIS), st_get_axis_position_mm(Z_AXIS));
//position.debug("in plan_get position"); //position.debug("in plan_get position");
//plan_bed_level_matrix.debug("in plan_get_position"); //plan_bed_level_matrix.debug("in plan_get_position");

82
Marlin/stepper.cpp
View File

@ -93,8 +93,8 @@ static volatile char endstop_hit_bits = 0; // use X_MIN, Y_MIN, Z_MIN and Z_MIN_
static bool check_endstops = true; static bool check_endstops = true;
volatile long count_position[NUM_AXIS] = { 0 }; volatile long count_position[NUM_AXIS] = { 0 }; // Positions of stepper motors, in step units
volatile signed char count_direction[NUM_AXIS] = { 1, 1, 1, 1 }; volatile signed char count_direction[NUM_AXIS] = { 1 };
//=========================================================================== //===========================================================================
@ -286,9 +286,17 @@ void checkHitEndstops() {
} }
} }
#if ENABLED(COREXY) || ENABLED(COREXZ)
#if ENABLED(COREXY)
#define CORE_AXIS_2 B_AXIS
#else
#define CORE_AXIS_2 C_AXIS
#endif
#endif
void enable_endstops(bool check) { check_endstops = check; } void enable_endstops(bool check) { check_endstops = check; }
// Check endstops // Check endstops - called from ISR!
inline void update_endstops() { inline void update_endstops() {
#if ENABLED(Z_DUAL_ENDSTOPS) #if ENABLED(Z_DUAL_ENDSTOPS)
@ -311,23 +319,36 @@ inline void update_endstops() {
// TEST_ENDSTOP: test the old and the current status of an endstop // TEST_ENDSTOP: test the old and the current status of an endstop
#define TEST_ENDSTOP(ENDSTOP) (TEST(current_endstop_bits, ENDSTOP) && TEST(old_endstop_bits, ENDSTOP)) #define TEST_ENDSTOP(ENDSTOP) (TEST(current_endstop_bits, ENDSTOP) && TEST(old_endstop_bits, ENDSTOP))
#define UPDATE_ENDSTOP(AXIS,MINMAX) \ #if ENABLED(COREXY) || ENABLED(COREXZ)
SET_ENDSTOP_BIT(AXIS, MINMAX); \
if (TEST_ENDSTOP(_ENDSTOP(AXIS, MINMAX)) && (current_block->steps[_AXIS(AXIS)] > 0)) { \
endstops_trigsteps[_AXIS(AXIS)] = count_position[_AXIS(AXIS)]; \
_ENDSTOP_HIT(AXIS); \
step_events_completed = current_block->step_event_count; \
}
#if ENABLED(COREXY) #define _SET_TRIGSTEPS(AXIS) do { \
// Head direction in -X axis for CoreXY bots. float axis_pos = count_position[_AXIS(AXIS)]; \
// If DeltaX == -DeltaY, the movement is only in Y axis if (_AXIS(AXIS) == A_AXIS) \
if ((current_block->steps[A_AXIS] != current_block->steps[B_AXIS]) || (TEST(out_bits, A_AXIS) == TEST(out_bits, B_AXIS))) { axis_pos = (axis_pos + count_position[CORE_AXIS_2]) / 2; \
if (TEST(out_bits, X_HEAD)) else if (_AXIS(AXIS) == CORE_AXIS_2) \
#elif ENABLED(COREXZ) axis_pos = (count_position[A_AXIS] - axis_pos) / 2; \
// Head direction in -X axis for CoreXZ bots. endstops_trigsteps[_AXIS(AXIS)] = axis_pos; \
// If DeltaX == -DeltaZ, the movement is only in Z axis } while(0)
if ((current_block->steps[A_AXIS] != current_block->steps[C_AXIS]) || (TEST(out_bits, A_AXIS) == TEST(out_bits, C_AXIS))) {
#else
#define _SET_TRIGSTEPS(AXIS) endstops_trigsteps[_AXIS(AXIS)] = count_position[_AXIS(AXIS)]
#endif // COREXY || COREXZ
#define UPDATE_ENDSTOP(AXIS,MINMAX) do { \
SET_ENDSTOP_BIT(AXIS, MINMAX); \
if (TEST_ENDSTOP(_ENDSTOP(AXIS, MINMAX)) && current_block->steps[_AXIS(AXIS)] > 0) { \
_SET_TRIGSTEPS(AXIS); \
_ENDSTOP_HIT(AXIS); \
step_events_completed = current_block->step_event_count; \
} \
} while(0)
#if ENABLED(COREXY) || ENABLED(COREXZ)
// Head direction in -X axis for CoreXY and CoreXZ bots.
// If Delta1 == -Delta2, the movement is only in Y or Z axis
if ((current_block->steps[A_AXIS] != current_block->steps[CORE_AXIS_2]) || (TEST(out_bits, A_AXIS) == TEST(out_bits, CORE_AXIS_2))) {
if (TEST(out_bits, X_HEAD)) if (TEST(out_bits, X_HEAD))
#else #else
if (TEST(out_bits, X_AXIS)) // stepping along -X axis (regular Cartesian bot) if (TEST(out_bits, X_AXIS)) // stepping along -X axis (regular Cartesian bot)
@ -1087,14 +1108,31 @@ void st_set_e_position(const long& e) {
} }
long st_get_position(uint8_t axis) { long st_get_position(uint8_t axis) {
long count_pos;
CRITICAL_SECTION_START; CRITICAL_SECTION_START;
count_pos = count_position[axis]; long count_pos = count_position[axis];
CRITICAL_SECTION_END; CRITICAL_SECTION_END;
return count_pos; return count_pos;
} }
float st_get_position_mm(AxisEnum axis) { return st_get_position(axis) / axis_steps_per_unit[axis]; } float st_get_axis_position_mm(AxisEnum axis) {
float axis_pos;
#if ENABLED(COREXY) | ENABLED(COREXZ)
if (axis == X_AXIS || axis == CORE_AXIS_2) {
CRITICAL_SECTION_START;
long pos1 = count_position[A_AXIS],
pos2 = count_position[CORE_AXIS_2];
CRITICAL_SECTION_END;
// ((a1+a2)+(a1-a2))/2 -> (a1+a2+a1-a2)/2 -> (a1+a1)/2 -> a1
// ((a1+a2)-(a1-a2))/2 -> (a1+a2-a1+a2)/2 -> (a2+a2)/2 -> a2
axis_pos = (pos1 + ((axis == X_AXIS) ? pos2 : -pos2)) / 2.0f;
}
else
axis_pos = st_get_position(axis);
#else
axis_pos = st_get_position(axis);
#endif
return axis_pos / axis_steps_per_unit[axis];
}
void finishAndDisableSteppers() { void finishAndDisableSteppers() {
st_synchronize(); st_synchronize();