andrey
/
Marlin_FB4S
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Add LOGICAL_POSITION macro, apply to kinematics

pull/1/head
Scott Lahteine 9 years ago
parent
35a610abf9
commit
b3eb0c8569
1 changed files with 48 additions and 44 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 92
      Marlin/Marlin_main.cpp

92
Marlin/Marlin_main.cpp
View File

@ -330,6 +330,7 @@ float position_shift[3] = { 0 };
// Set by M206, M428, or menu item. Saved to EEPROM.
float home_offset[3] = { 0 };
#define LOGICAL_POSITION(POS, AXIS) (POS + home_offset[AXIS] + position_shift[AXIS])
#define RAW_POSITION(POS, AXIS) (POS - home_offset[AXIS] - position_shift[AXIS])
#define RAW_CURRENT_POSITION(AXIS) (RAW_POSITION(current_position[AXIS], AXIS))
@ -1402,7 +1403,7 @@ XYZ_CONSTS_FROM_CONFIG(signed char, home_dir, HOME_DIR);
static float x_home_pos(int extruder) {
if (extruder == 0)
return base_home_pos(X_AXIS) + home_offset[X_AXIS];
return LOGICAL_POSITION(base_home_pos(X_AXIS), X_AXIS);
else
/**
* In dual carriage mode the extruder offset provides an override of the
@ -1437,7 +1438,7 @@ XYZ_CONSTS_FROM_CONFIG(signed char, home_dir, HOME_DIR);
* at the same positions relative to the machine.
*/
static void update_software_endstops(AxisEnum axis) {
float offs = home_offset[axis] + position_shift[axis];
float offs = LOGICAL_POSITION(0, axis);
#if ENABLED(DUAL_X_CARRIAGE)
if (axis == X_AXIS) {
@ -1508,7 +1509,7 @@ static void set_axis_is_at_home(AxisEnum axis) {
if (active_extruder != 0)
current_position[X_AXIS] = x_home_pos(active_extruder);
else
current_position[X_AXIS] = base_home_pos(X_AXIS) + home_offset[X_AXIS];
current_position[X_AXIS] = LOGICAL_POSITION(base_home_pos(X_AXIS), X_AXIS);
update_software_endstops(X_AXIS);
return;
}
@ -1519,7 +1520,8 @@ static void set_axis_is_at_home(AxisEnum axis) {
if (axis == X_AXIS || axis == Y_AXIS) {
float homeposition[3];
for (int i = 0; i < 3; i++) homeposition[i] = base_home_pos(i);
for (uint8_t i = X_AXIS; i <= Z_AXIS; i++)
homeposition[i] = LOGICAL_POSITION(base_home_pos(i), i);
// SERIAL_ECHOPGM("homeposition[x]= "); SERIAL_ECHO(homeposition[0]);
// SERIAL_ECHOPGM("homeposition[y]= "); SERIAL_ECHOLN(homeposition[1]);
@ -1529,23 +1531,12 @@ static void set_axis_is_at_home(AxisEnum axis) {
* and calculates homing offset using forward kinematics
*/
inverse_kinematics(homeposition);
// SERIAL_ECHOPGM("base Theta= "); SERIAL_ECHO(delta[X_AXIS]);
// SERIAL_ECHOPGM(" base Psi+Theta="); SERIAL_ECHOLN(delta[Y_AXIS]);
for (int i = 0; i < 2; i++) delta[i] -= home_offset[i];
// SERIAL_ECHOPGM("addhome X="); SERIAL_ECHO(home_offset[X_AXIS]);
// SERIAL_ECHOPGM(" addhome Y="); SERIAL_ECHO(home_offset[Y_AXIS]);
// SERIAL_ECHOPGM(" addhome Theta="); SERIAL_ECHO(delta[X_AXIS]);
// SERIAL_ECHOPGM(" addhome Psi+Theta="); SERIAL_ECHOLN(delta[Y_AXIS]);
forward_kinematics_SCARA(delta);
// SERIAL_ECHOPGM("Delta X="); SERIAL_ECHO(delta[X_AXIS]);
// SERIAL_ECHOPAIR("Delta X=", delta[X_AXIS]);
// SERIAL_ECHOPGM(" Delta Y="); SERIAL_ECHOLN(delta[Y_AXIS]);
current_position[axis] = delta[axis];
current_position[axis] = LOGICAL_POSITION(delta[axis], axis);
/**
* SCARA home positions are based on configuration since the actual
@ -1557,7 +1548,7 @@ static void set_axis_is_at_home(AxisEnum axis) {
else
#endif
{
current_position[axis] = base_home_pos(axis) + home_offset[axis];
current_position[axis] = LOGICAL_POSITION(base_home_pos(axis), axis);
update_software_endstops(axis);
#if HAS_BED_PROBE && Z_HOME_DIR < 0 && DISABLED(Z_MIN_PROBE_ENDSTOP)
@ -1786,7 +1777,7 @@ static void clean_up_after_endstop_or_probe_move() {
SERIAL_ECHOLNPGM(")");
}
#endif
float z_dest = home_offset[Z_AXIS] + z_raise;
float z_dest = LOGICAL_POSITION(z_raise, Z_AXIS);
if (zprobe_zoffset < 0)
z_dest -= zprobe_zoffset;
@ -2089,7 +2080,7 @@ static void clean_up_after_endstop_or_probe_move() {
#if ENABLED(DELTA)
#define SET_Z_FROM_STEPPERS() set_current_from_steppers()
#else
#define SET_Z_FROM_STEPPERS() current_position[Z_AXIS] = stepper.get_axis_position_mm(Z_AXIS)
#define SET_Z_FROM_STEPPERS() current_position[Z_AXIS] = LOGICAL_POSITION(stepper.get_axis_position_mm(Z_AXIS), Z_AXIS)
#endif
// Do a single Z probe and return with current_position[Z_AXIS]
@ -2958,7 +2949,7 @@ inline void gcode_G28() {
if (home_all_axis || homeX || homeY) {
// Raise Z before homing any other axes and z is not already high enough (never lower z)
destination[Z_AXIS] = home_offset[Z_AXIS] + MIN_Z_HEIGHT_FOR_HOMING;
destination[Z_AXIS] = LOGICAL_POSITION(MIN_Z_HEIGHT_FOR_HOMING, Z_AXIS);
if (destination[Z_AXIS] > current_position[Z_AXIS]) {
#if ENABLED(DEBUG_LEVELING_FEATURE)
@ -3213,12 +3204,12 @@ inline void gcode_G28() {
;
line_to_current_position();
current_position[X_AXIS] = x + home_offset[X_AXIS];
current_position[Y_AXIS] = y + home_offset[Y_AXIS];
current_position[X_AXIS] = LOGICAL_POSITION(x, X_AXIS);
current_position[Y_AXIS] = LOGICAL_POSITION(y, Y_AXIS);
line_to_current_position();
#if Z_RAISE_BETWEEN_PROBINGS > 0 || MIN_Z_HEIGHT_FOR_HOMING > 0
current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
current_position[Z_AXIS] = LOGICAL_POSITION(MESH_HOME_SEARCH_Z, Z_AXIS);
line_to_current_position();
#endif
@ -3636,14 +3627,14 @@ inline void gcode_G28() {
#endif
// Probe at 3 arbitrary points
float z_at_pt_1 = probe_pt( ABL_PROBE_PT_1_X + home_offset[X_AXIS],
ABL_PROBE_PT_1_Y + home_offset[Y_AXIS],
float z_at_pt_1 = probe_pt( LOGICAL_POSITION(ABL_PROBE_PT_1_X, X_AXIS),
LOGICAL_POSITION(ABL_PROBE_PT_1_Y, Y_AXIS),
stow_probe_after_each, verbose_level),
z_at_pt_2 = probe_pt( ABL_PROBE_PT_2_X + home_offset[X_AXIS],
ABL_PROBE_PT_2_Y + home_offset[Y_AXIS],
z_at_pt_2 = probe_pt( LOGICAL_POSITION(ABL_PROBE_PT_2_X, X_AXIS),
LOGICAL_POSITION(ABL_PROBE_PT_2_Y, Y_AXIS),
stow_probe_after_each, verbose_level),
z_at_pt_3 = probe_pt( ABL_PROBE_PT_3_X + home_offset[X_AXIS],
ABL_PROBE_PT_3_Y + home_offset[Y_AXIS],
z_at_pt_3 = probe_pt( LOGICAL_POSITION(ABL_PROBE_PT_3_X, X_AXIS),
LOGICAL_POSITION(ABL_PROBE_PT_3_Y, Y_AXIS),
stow_probe_after_each, verbose_level);
if (!dryrun) set_bed_level_equation_3pts(z_at_pt_1, z_at_pt_2, z_at_pt_3);
@ -5174,9 +5165,9 @@ static void report_current_position() {
SERIAL_EOL;
SERIAL_PROTOCOLPGM("SCARA Cal - Theta:");
SERIAL_PROTOCOL(delta[X_AXIS] + home_offset[X_AXIS]);
SERIAL_PROTOCOL(delta[X_AXIS]);
SERIAL_PROTOCOLPGM(" Psi+Theta (90):");
SERIAL_PROTOCOL(delta[Y_AXIS] - delta[X_AXIS] - 90 + home_offset[Y_AXIS]);
SERIAL_PROTOCOL(delta[Y_AXIS] - delta[X_AXIS] - 90);
SERIAL_EOL;
SERIAL_PROTOCOLPGM("SCARA step Cal - Theta:");
@ -6143,7 +6134,7 @@ inline void gcode_M428() {
for (int8_t i = X_AXIS; i <= Z_AXIS; i++) {
if (axis_homed[i]) {
float base = (current_position[i] > (sw_endstop_min[i] + sw_endstop_max[i]) / 2) ? base_home_pos(i) : 0,
diff = current_position[i] - base;
diff = current_position[i] - LOGICAL_POSITION(base, i);
if (diff > -20 && diff < 20) {
set_home_offset((AxisEnum)i, home_offset[i] - diff);
}
@ -7739,6 +7730,12 @@ void clamp_to_software_endstops(float target[3]) {
void inverse_kinematics(const float in_cartesian[3]) {
const float cartesian[3] = {
RAW_POSITION(in_cartesian[X_AXIS], X_AXIS),
RAW_POSITION(in_cartesian[Y_AXIS], Y_AXIS),
RAW_POSITION(in_cartesian[Z_AXIS], Z_AXIS)
};
delta[TOWER_1] = sqrt(delta_diagonal_rod_2_tower_1
- sq(delta_tower1_x - cartesian[X_AXIS])
- sq(delta_tower1_y - cartesian[Y_AXIS])
@ -7763,10 +7760,14 @@ void clamp_to_software_endstops(float target[3]) {
}
float delta_safe_distance_from_top() {
float cartesian[3] = { 0 };
float cartesian[3] = {
LOGICAL_POSITION(0, X_AXIS),
LOGICAL_POSITION(0, Y_AXIS),
LOGICAL_POSITION(0, Z_AXIS)
};
inverse_kinematics(cartesian);
float distance = delta[TOWER_3];
cartesian[Y_AXIS] = DELTA_PRINTABLE_RADIUS;
cartesian[Y_AXIS] = LOGICAL_POSITION(DELTA_PRINTABLE_RADIUS, Y_AXIS);
inverse_kinematics(cartesian);
return abs(distance - delta[TOWER_3]);
}
@ -7846,8 +7847,8 @@ void clamp_to_software_endstops(float target[3]) {
void set_cartesian_from_steppers() {
forward_kinematics_DELTA(stepper.get_axis_position_mm(X_AXIS),
stepper.get_axis_position_mm(Y_AXIS),
stepper.get_axis_position_mm(Z_AXIS));
stepper.get_axis_position_mm(Y_AXIS),
stepper.get_axis_position_mm(Z_AXIS));
}
#if ENABLED(AUTO_BED_LEVELING_FEATURE)
@ -7858,8 +7859,8 @@ void clamp_to_software_endstops(float target[3]) {
int half = (AUTO_BED_LEVELING_GRID_POINTS - 1) / 2;
float h1 = 0.001 - half, h2 = half - 0.001,
grid_x = max(h1, min(h2, cartesian[X_AXIS] / delta_grid_spacing[0])),
grid_y = max(h1, min(h2, cartesian[Y_AXIS] / delta_grid_spacing[1]));
grid_x = max(h1, min(h2, RAW_POSITION(cartesian[X_AXIS], X_AXIS) / delta_grid_spacing[0])),
grid_y = max(h1, min(h2, RAW_POSITION(cartesian[Y_AXIS], Y_AXIS) / delta_grid_spacing[1]));
int floor_x = floor(grid_x), floor_y = floor(grid_y);
float ratio_x = grid_x - floor_x, ratio_y = grid_y - floor_y,
z1 = bed_level[floor_x + half][floor_y + half],
@ -7910,6 +7911,9 @@ void set_current_from_steppers() {
current_position[Y_AXIS] = stepper.get_axis_position_mm(Y_AXIS);
current_position[Z_AXIS] = stepper.get_axis_position_mm(Z_AXIS);
#endif
for (uint8_t i = X_AXIS; i <= Z_AXIS; i++)
current_position[i] += LOGICAL_POSITION(0, i);
}
#if ENABLED(MESH_BED_LEVELING)
@ -7940,14 +7944,14 @@ void mesh_line_to_destination(float fr_mm_m, uint8_t x_splits = 0xff, uint8_t y_
int8_t gcx = max(cx1, cx2), gcy = max(cy1, cy2);
if (cx2 != cx1 && TEST(x_splits, gcx)) {
memcpy(end, destination, sizeof(end));
destination[X_AXIS] = mbl.get_probe_x(gcx) + home_offset[X_AXIS] + position_shift[X_AXIS];
destination[X_AXIS] = LOGICAL_POSITION(mbl.get_probe_x(gcx), X_AXIS);
normalized_dist = (destination[X_AXIS] - current_position[X_AXIS]) / (end[X_AXIS] - current_position[X_AXIS]);
destination[Y_AXIS] = MBL_SEGMENT_END(Y);
CBI(x_splits, gcx);
}
else if (cy2 != cy1 && TEST(y_splits, gcy)) {
memcpy(end, destination, sizeof(end));
destination[Y_AXIS] = mbl.get_probe_y(gcy) + home_offset[Y_AXIS] + position_shift[Y_AXIS];
destination[Y_AXIS] = LOGICAL_POSITION(mbl.get_probe_y(gcy), Y_AXIS);
normalized_dist = (destination[Y_AXIS] - current_position[Y_AXIS]) / (end[Y_AXIS] - current_position[Y_AXIS]);
destination[X_AXIS] = MBL_SEGMENT_END(X);
CBI(y_splits, gcy);
@ -8362,8 +8366,8 @@ void prepare_move_to_destination() {
float SCARA_pos[2];
static float SCARA_C2, SCARA_S2, SCARA_K1, SCARA_K2, SCARA_theta, SCARA_psi;
SCARA_pos[X_AXIS] = cartesian[X_AXIS] * axis_scaling[X_AXIS] - SCARA_offset_x; //Translate SCARA to standard X Y
SCARA_pos[Y_AXIS] = cartesian[Y_AXIS] * axis_scaling[Y_AXIS] - SCARA_offset_y; // With scaling factor.
SCARA_pos[X_AXIS] = RAW_POSITION(cartesian[X_AXIS], X_AXIS) * axis_scaling[X_AXIS] - SCARA_offset_x; //Translate SCARA to standard X Y
SCARA_pos[Y_AXIS] = RAW_POSITION(cartesian[Y_AXIS], Y_AXIS) * axis_scaling[Y_AXIS] - SCARA_offset_y; // With scaling factor.
#if (Linkage_1 == Linkage_2)
SCARA_C2 = ((sq(SCARA_pos[X_AXIS]) + sq(SCARA_pos[Y_AXIS])) / (2 * (float)L1_2)) - 1;
@ -8381,7 +8385,7 @@ void prepare_move_to_destination() {
delta[X_AXIS] = SCARA_theta * SCARA_RAD2DEG; // Multiply by 180/Pi - theta is support arm angle
delta[Y_AXIS] = (SCARA_theta + SCARA_psi) * SCARA_RAD2DEG; // - equal to sub arm angle (inverted motor)
delta[Z_AXIS] = cartesian[Z_AXIS];
delta[Z_AXIS] = RAW_POSITION(cartesian[Z_AXIS], Z_AXIS);
/**
SERIAL_ECHOPGM("cartesian x="); SERIAL_ECHO(cartesian[X_AXIS]);

Write Preview
Loading…
Cancel
Save