Move get_axis_position_mm to Planner (#10718)

This commit is contained in:
Scott Lahteine 2018-05-12 09:59:11 -05:00 committed by GitHub
parent a1062eec5b
commit 8f8c6a9bc4
Signed by: GitHub
GPG key ID: 4AEE18F83AFDEB23
12 changed files with 75 additions and 80 deletions

View file

@ -338,9 +338,9 @@ void safe_delay(millis_t ms) {
#endif #endif
#if ABL_PLANAR #if ABL_PLANAR
const float diff[XYZ] = { const float diff[XYZ] = {
stepper.get_axis_position_mm(X_AXIS) - current_position[X_AXIS], planner.get_axis_position_mm(X_AXIS) - current_position[X_AXIS],
stepper.get_axis_position_mm(Y_AXIS) - current_position[Y_AXIS], planner.get_axis_position_mm(Y_AXIS) - current_position[Y_AXIS],
stepper.get_axis_position_mm(Z_AXIS) - current_position[Z_AXIS] planner.get_axis_position_mm(Z_AXIS) - current_position[Z_AXIS]
}; };
SERIAL_ECHOPGM("ABL Adjustment X"); SERIAL_ECHOPGM("ABL Adjustment X");
if (diff[X_AXIS] > 0) SERIAL_CHAR('+'); if (diff[X_AXIS] > 0) SERIAL_CHAR('+');

View file

@ -99,15 +99,15 @@ void I2CPositionEncoder::update() {
//the encoder likely lost its place when the error occured, so we'll reset and use the printer's //the encoder likely lost its place when the error occured, so we'll reset and use the printer's
//idea of where it the axis is to re-initialise //idea of where it the axis is to re-initialise
float position = stepper.get_axis_position_mm(encoderAxis); const float pos = planner.get_axis_position_mm(encoderAxis);
int32_t positionInTicks = position * get_ticks_unit(); int32_t positionInTicks = pos * get_ticks_unit();
//shift position from previous to current position //shift position from previous to current position
zeroOffset -= (positionInTicks - get_position()); zeroOffset -= (positionInTicks - get_position());
#ifdef I2CPE_DEBUG #ifdef I2CPE_DEBUG
SERIAL_ECHOPGM("Current position is "); SERIAL_ECHOPGM("Current position is ");
SERIAL_ECHOLN(position); SERIAL_ECHOLN(pos);
SERIAL_ECHOPGM("Position in encoder ticks is "); SERIAL_ECHOPGM("Position in encoder ticks is ");
SERIAL_ECHOLN(positionInTicks); SERIAL_ECHOLN(positionInTicks);
@ -254,7 +254,7 @@ bool I2CPositionEncoder::passes_test(const bool report) {
float I2CPositionEncoder::get_axis_error_mm(const bool report) { float I2CPositionEncoder::get_axis_error_mm(const bool report) {
float target, actual, error; float target, actual, error;
target = stepper.get_axis_position_mm(encoderAxis); target = planner.get_axis_position_mm(encoderAxis);
actual = mm_from_count(position); actual = mm_from_count(position);
error = actual - target; error = actual - target;
@ -349,8 +349,8 @@ bool I2CPositionEncoder::test_axis() {
ec = false; ec = false;
LOOP_NA(i) { LOOP_NA(i) {
startCoord[i] = stepper.get_axis_position_mm((AxisEnum)i); startCoord[i] = planner.get_axis_position_mm((AxisEnum)i);
endCoord[i] = stepper.get_axis_position_mm((AxisEnum)i); endCoord[i] = planner.get_axis_position_mm((AxisEnum)i);
} }
startCoord[encoderAxis] = startPosition; startCoord[encoderAxis] = startPosition;
@ -359,7 +359,7 @@ bool I2CPositionEncoder::test_axis() {
planner.synchronize(); planner.synchronize();
planner.buffer_line(startCoord[X_AXIS], startCoord[Y_AXIS], startCoord[Z_AXIS], planner.buffer_line(startCoord[X_AXIS], startCoord[Y_AXIS], startCoord[Z_AXIS],
stepper.get_axis_position_mm(E_AXIS), feedrate, 0); planner.get_axis_position_mm(E_AXIS), feedrate, 0);
planner.synchronize(); planner.synchronize();
// if the module isn't currently trusted, wait until it is (or until it should be if things are working) // if the module isn't currently trusted, wait until it is (or until it should be if things are working)
@ -371,7 +371,7 @@ bool I2CPositionEncoder::test_axis() {
if (trusted) { // if trusted, commence test if (trusted) { // if trusted, commence test
planner.buffer_line(endCoord[X_AXIS], endCoord[Y_AXIS], endCoord[Z_AXIS], planner.buffer_line(endCoord[X_AXIS], endCoord[Y_AXIS], endCoord[Z_AXIS],
stepper.get_axis_position_mm(E_AXIS), feedrate, 0); planner.get_axis_position_mm(E_AXIS), feedrate, 0);
planner.synchronize(); planner.synchronize();
} }
@ -408,8 +408,8 @@ void I2CPositionEncoder::calibrate_steps_mm(const uint8_t iter) {
travelDistance = endDistance - startDistance; travelDistance = endDistance - startDistance;
LOOP_NA(i) { LOOP_NA(i) {
startCoord[i] = stepper.get_axis_position_mm((AxisEnum)i); startCoord[i] = planner.get_axis_position_mm((AxisEnum)i);
endCoord[i] = stepper.get_axis_position_mm((AxisEnum)i); endCoord[i] = planner.get_axis_position_mm((AxisEnum)i);
} }
startCoord[encoderAxis] = startDistance; startCoord[encoderAxis] = startDistance;
@ -419,7 +419,7 @@ void I2CPositionEncoder::calibrate_steps_mm(const uint8_t iter) {
LOOP_L_N(i, iter) { LOOP_L_N(i, iter) {
planner.buffer_line(startCoord[X_AXIS], startCoord[Y_AXIS], startCoord[Z_AXIS], planner.buffer_line(startCoord[X_AXIS], startCoord[Y_AXIS], startCoord[Z_AXIS],
stepper.get_axis_position_mm(E_AXIS), feedrate, 0); planner.get_axis_position_mm(E_AXIS), feedrate, 0);
planner.synchronize(); planner.synchronize();
delay(250); delay(250);
@ -428,7 +428,7 @@ void I2CPositionEncoder::calibrate_steps_mm(const uint8_t iter) {
//do_blocking_move_to(endCoord[X_AXIS],endCoord[Y_AXIS],endCoord[Z_AXIS]); //do_blocking_move_to(endCoord[X_AXIS],endCoord[Y_AXIS],endCoord[Z_AXIS]);
planner.buffer_line(endCoord[X_AXIS], endCoord[Y_AXIS], endCoord[Z_AXIS], planner.buffer_line(endCoord[X_AXIS], endCoord[Y_AXIS], endCoord[Z_AXIS],
stepper.get_axis_position_mm(E_AXIS), feedrate, 0); planner.get_axis_position_mm(E_AXIS), feedrate, 0);
planner.synchronize(); planner.synchronize();
//Read encoder distance //Read encoder distance

View file

@ -448,8 +448,8 @@
#if IS_SCARA // scale the feed rate from mm/s to degrees/s #if IS_SCARA // scale the feed rate from mm/s to degrees/s
scara_feed_factor = cartesian_xy_mm * inv_segments * feedrate; scara_feed_factor = cartesian_xy_mm * inv_segments * feedrate;
scara_oldA = stepper.get_axis_position_degrees(A_AXIS); scara_oldA = planner.get_axis_position_degrees(A_AXIS);
scara_oldB = stepper.get_axis_position_degrees(B_AXIS); scara_oldB = planner.get_axis_position_degrees(B_AXIS);
#endif #endif
const float diff[XYZE] = { const float diff[XYZE] = {

View file

@ -90,8 +90,8 @@
#if IS_SCARA #if IS_SCARA
const float deg[XYZ] = { const float deg[XYZ] = {
stepper.get_axis_position_degrees(A_AXIS), planner.get_axis_position_degrees(A_AXIS),
stepper.get_axis_position_degrees(B_AXIS) planner.get_axis_position_degrees(B_AXIS)
}; };
SERIAL_PROTOCOLPGM("Degrees:"); SERIAL_PROTOCOLPGM("Degrees:");
report_xyze(deg, 2); report_xyze(deg, 2);
@ -99,7 +99,7 @@
SERIAL_PROTOCOLPGM("FromStp:"); SERIAL_PROTOCOLPGM("FromStp:");
get_cartesian_from_steppers(); // writes cartes[XYZ] (with forward kinematics) get_cartesian_from_steppers(); // writes cartes[XYZ] (with forward kinematics)
const float from_steppers[XYZE] = { cartes[X_AXIS], cartes[Y_AXIS], cartes[Z_AXIS], stepper.get_axis_position_mm(E_AXIS) }; const float from_steppers[XYZE] = { cartes[X_AXIS], cartes[Y_AXIS], cartes[Z_AXIS], planner.get_axis_position_mm(E_AXIS) };
report_xyze(from_steppers); report_xyze(from_steppers);
const float diff[XYZE] = { const float diff[XYZE] = {

View file

@ -193,21 +193,21 @@ void sync_plan_position_e() { planner.set_e_position_mm(current_position[E_AXIS]
void get_cartesian_from_steppers() { void get_cartesian_from_steppers() {
#if ENABLED(DELTA) #if ENABLED(DELTA)
forward_kinematics_DELTA( forward_kinematics_DELTA(
stepper.get_axis_position_mm(A_AXIS), planner.get_axis_position_mm(A_AXIS),
stepper.get_axis_position_mm(B_AXIS), planner.get_axis_position_mm(B_AXIS),
stepper.get_axis_position_mm(C_AXIS) planner.get_axis_position_mm(C_AXIS)
); );
#else #else
#if IS_SCARA #if IS_SCARA
forward_kinematics_SCARA( forward_kinematics_SCARA(
stepper.get_axis_position_degrees(A_AXIS), planner.get_axis_position_degrees(A_AXIS),
stepper.get_axis_position_degrees(B_AXIS) planner.get_axis_position_degrees(B_AXIS)
); );
#else #else
cartes[X_AXIS] = stepper.get_axis_position_mm(X_AXIS); cartes[X_AXIS] = planner.get_axis_position_mm(X_AXIS);
cartes[Y_AXIS] = stepper.get_axis_position_mm(Y_AXIS); cartes[Y_AXIS] = planner.get_axis_position_mm(Y_AXIS);
#endif #endif
cartes[Z_AXIS] = stepper.get_axis_position_mm(Z_AXIS); cartes[Z_AXIS] = planner.get_axis_position_mm(Z_AXIS);
#endif #endif
} }
@ -870,12 +870,7 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
} }
#endif #endif
// move duplicate extruder into correct duplication position. // move duplicate extruder into correct duplication position.
planner.set_position_mm( planner.set_position_mm(inactive_extruder_x_pos, current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
inactive_extruder_x_pos,
current_position[Y_AXIS],
current_position[Z_AXIS],
current_position[E_AXIS]
);
planner.buffer_line( planner.buffer_line(
current_position[X_AXIS] + duplicate_extruder_x_offset, current_position[X_AXIS] + duplicate_extruder_x_offset,
current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS],

View file

@ -1299,6 +1299,37 @@ void Planner::check_axes_activity() {
#endif // PLANNER_LEVELING #endif // PLANNER_LEVELING
/**
* Get an axis position according to stepper position(s)
* For CORE machines apply translation from ABC to XYZ.
*/
float Planner::get_axis_position_mm(const AxisEnum axis) {
float axis_steps;
#if IS_CORE
// Requesting one of the "core" axes?
if (axis == CORE_AXIS_1 || axis == CORE_AXIS_2) {
// Protect the access to the position.
const bool was_enabled = STEPPER_ISR_ENABLED();
DISABLE_STEPPER_DRIVER_INTERRUPT();
// ((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_steps = 0.5f * (
axis == CORE_AXIS_2 ? CORESIGN(stepper.position(CORE_AXIS_1) - stepper.position(CORE_AXIS_2))
: stepper.position(CORE_AXIS_1) + stepper.position(CORE_AXIS_2)
);
if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
}
else
axis_steps = stepper.position(axis);
#else
axis_steps = stepper.position(axis);
#endif
return axis_steps * steps_to_mm[axis];
}
/** /**
* Block until all buffered steps are executed / cleaned * Block until all buffered steps are executed / cleaned
*/ */

View file

@ -546,6 +546,17 @@ class Planner {
*/ */
static void sync_from_steppers(); static void sync_from_steppers();
/**
* Get an axis position according to stepper position(s)
* For CORE machines apply translation from ABC to XYZ.
*/
static float get_axis_position_mm(const AxisEnum axis);
// SCARA AB axes are in degrees, not mm
#if IS_SCARA
FORCE_INLINE static float get_axis_position_degrees(const AxisEnum axis) { return get_axis_position_mm(axis); }
#endif
/** /**
* Does the buffer have any blocks queued? * Does the buffer have any blocks queued?
*/ */

View file

@ -30,7 +30,7 @@
#include "scara.h" #include "scara.h"
#include "motion.h" #include "motion.h"
#include "stepper.h" #include "planner.h"
float delta_segments_per_second = SCARA_SEGMENTS_PER_SECOND; float delta_segments_per_second = SCARA_SEGMENTS_PER_SECOND;
@ -147,8 +147,8 @@ void inverse_kinematics(const float raw[XYZ]) {
} }
void scara_report_positions() { void scara_report_positions() {
SERIAL_PROTOCOLPAIR("SCARA Theta:", stepper.get_axis_position_degrees(A_AXIS)); SERIAL_PROTOCOLPAIR("SCARA Theta:", planner.get_axis_position_degrees(A_AXIS));
SERIAL_PROTOCOLLNPAIR(" Psi+Theta:", stepper.get_axis_position_degrees(B_AXIS)); SERIAL_PROTOCOLLNPAIR(" Psi+Theta:", planner.get_axis_position_degrees(B_AXIS));
SERIAL_EOL(); SERIAL_EOL();
} }

View file

@ -2037,32 +2037,6 @@ int32_t Stepper::position(const AxisEnum axis) {
return count_pos; return count_pos;
} }
/**
* Get an axis position according to stepper position(s)
* For CORE machines apply translation from ABC to XYZ.
*/
float Stepper::get_axis_position_mm(const AxisEnum axis) {
float axis_steps;
#if IS_CORE
// Requesting one of the "core" axes?
if (axis == CORE_AXIS_1 || axis == CORE_AXIS_2) {
CRITICAL_SECTION_START;
// ((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_steps = 0.5f * (
axis == CORE_AXIS_2 ? CORESIGN(count_position[CORE_AXIS_1] - count_position[CORE_AXIS_2])
: count_position[CORE_AXIS_1] + count_position[CORE_AXIS_2]
);
CRITICAL_SECTION_END;
}
else
axis_steps = position(axis);
#else
axis_steps = position(axis);
#endif
return axis_steps * planner.steps_to_mm[axis];
}
void Stepper::finish_and_disable() { void Stepper::finish_and_disable() {
planner.synchronize(); planner.synchronize();
disable_all_steppers(); disable_all_steppers();

View file

@ -228,18 +228,6 @@ class Stepper {
// //
static void report_positions(); static void report_positions();
//
// Get the position (mm) of an axis based on stepper position(s)
//
static float get_axis_position_mm(const AxisEnum axis);
//
// SCARA AB axes are in degrees, not mm
//
#if IS_SCARA
FORCE_INLINE static float get_axis_position_degrees(const AxisEnum axis) { return get_axis_position_mm(axis); }
#endif
// //
// The stepper subsystem goes to sleep when it runs out of things to execute. Call this // The stepper subsystem goes to sleep when it runs out of things to execute. Call this
// to notify the subsystem that it is time to go to work. // to notify the subsystem that it is time to go to work.

View file

@ -36,7 +36,7 @@
#include "../libs/private_spi.h" #include "../libs/private_spi.h"
#endif #endif
#if ENABLED(BABYSTEPPING) #if ENABLED(BABYSTEPPING) || ENABLED(PID_EXTRUSION_SCALING)
#include "stepper.h" #include "stepper.h"
#endif #endif

View file

@ -38,10 +38,6 @@
#include "../feature/power.h" #include "../feature/power.h"
#endif #endif
#if ENABLED(PID_EXTRUSION_SCALING)
#include "stepper.h"
#endif
#ifndef SOFT_PWM_SCALE #ifndef SOFT_PWM_SCALE
#define SOFT_PWM_SCALE 0 #define SOFT_PWM_SCALE 0
#endif #endif