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

Use 'logical' rather than 'target' or 'cartesian'

Browse Source
This commit is contained in:
Scott Lahteine
2016-09-15 02:31:36 -05:00
parent 40d3e854f6
commit 5f2f991192
2 changed files with 27 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
Marlin/Marlin.h
View File

@@ -303,7 +303,7 @@ float code_value_temp_diff();
#if IS_KINEMATIC #if IS_KINEMATIC
extern float delta[ABC]; extern float delta[ABC];
void inverse_kinematics(const float cartesian[XYZ]); void inverse_kinematics(const float logical[XYZ]);
#endif #endif
#if ENABLED(DELTA) #if ENABLED(DELTA)

52
Marlin/Marlin_main.cpp
View File

@@ -7992,9 +7992,9 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
* This calls planner.buffer_line several times, adding * This calls planner.buffer_line several times, adding
* small incremental moves for DELTA or SCARA. * small incremental moves for DELTA or SCARA.
*/ */
inline bool prepare_kinematic_move_to(float target[NUM_AXIS]) { inline bool prepare_kinematic_move_to(float logical[NUM_AXIS]) {
float difference[NUM_AXIS]; float difference[NUM_AXIS];
LOOP_XYZE(i) difference[i] = target[i] - current_position[i]; LOOP_XYZE(i) difference[i] = logical[i] - current_position[i];
float cartesian_mm = sqrt(sq(difference[X_AXIS]) + sq(difference[Y_AXIS]) + sq(difference[Z_AXIS])); float cartesian_mm = sqrt(sq(difference[X_AXIS]) + sq(difference[Y_AXIS]) + sq(difference[Z_AXIS]));
if (UNEAR_ZERO(cartesian_mm)) cartesian_mm = abs(difference[E_AXIS]); if (UNEAR_ZERO(cartesian_mm)) cartesian_mm = abs(difference[E_AXIS]);
@@ -8013,18 +8013,18 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
float fraction = float(s) * inv_steps; float fraction = float(s) * inv_steps;
LOOP_XYZE(i) LOOP_XYZE(i)
target[i] = current_position[i] + difference[i] * fraction; logical[i] = current_position[i] + difference[i] * fraction;
inverse_kinematics(target); inverse_kinematics(logical);
#if ENABLED(DELTA) && ENABLED(AUTO_BED_LEVELING_NONLINEAR) #if ENABLED(DELTA) && ENABLED(AUTO_BED_LEVELING_NONLINEAR)
if (!bed_leveling_in_progress) adjust_delta(target); if (!bed_leveling_in_progress) adjust_delta(logical);
#endif #endif
//DEBUG_POS("prepare_kinematic_move_to", target); //DEBUG_POS("prepare_kinematic_move_to", logical);
//DEBUG_POS("prepare_kinematic_move_to", delta); //DEBUG_POS("prepare_kinematic_move_to", delta);
planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], target[E_AXIS], _feedrate_mm_s, active_extruder); planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], logical[E_AXIS], _feedrate_mm_s, active_extruder);
} }
return true; return true;
} }
@@ -8156,20 +8156,20 @@ void prepare_move_to_destination() {
* options for G2/G3 arc generation. In future these options may be GCode tunable. * options for G2/G3 arc generation. In future these options may be GCode tunable.
*/ */
void plan_arc( void plan_arc(
float target[NUM_AXIS], // Destination position float logical[NUM_AXIS], // Destination position
float* offset, // Center of rotation relative to current_position float* offset, // Center of rotation relative to current_position
uint8_t clockwise // Clockwise? uint8_t clockwise // Clockwise?
) { ) {
float radius = HYPOT(offset[X_AXIS], offset[Y_AXIS]), float radius = HYPOT(offset[X_AXIS], offset[Y_AXIS]),
center_X = current_position[X_AXIS] + offset[X_AXIS], center_X = current_position[X_AXIS] + offset[X_AXIS],
center_Y = current_position[Y_AXIS] + offset[Y_AXIS], center_Y = current_position[Y_AXIS] + offset[Y_AXIS],
linear_travel = target[Z_AXIS] - current_position[Z_AXIS], linear_travel = logical[Z_AXIS] - current_position[Z_AXIS],
extruder_travel = target[E_AXIS] - current_position[E_AXIS], extruder_travel = logical[E_AXIS] - current_position[E_AXIS],
r_X = -offset[X_AXIS], // Radius vector from center to current location r_X = -offset[X_AXIS], // Radius vector from center to current location
r_Y = -offset[Y_AXIS], r_Y = -offset[Y_AXIS],
rt_X = target[X_AXIS] - center_X, rt_X = logical[X_AXIS] - center_X,
rt_Y = target[Y_AXIS] - center_Y; rt_Y = logical[Y_AXIS] - center_Y;
// CCW angle of rotation between position and target from the circle center. Only one atan2() trig computation required. // CCW angle of rotation between position and target from the circle center. Only one atan2() trig computation required.
float angular_travel = atan2(r_X * rt_Y - r_Y * rt_X, r_X * rt_X + r_Y * rt_Y); float angular_travel = atan2(r_X * rt_Y - r_Y * rt_X, r_X * rt_X + r_Y * rt_Y);
@@ -8177,7 +8177,7 @@ void prepare_move_to_destination() {
if (clockwise) angular_travel -= RADIANS(360); if (clockwise) angular_travel -= RADIANS(360);
// Make a circle if the angular rotation is 0 // Make a circle if the angular rotation is 0
if (angular_travel == 0 && current_position[X_AXIS] == target[X_AXIS] && current_position[Y_AXIS] == target[Y_AXIS]) if (angular_travel == 0 && current_position[X_AXIS] == logical[X_AXIS] && current_position[Y_AXIS] == logical[Y_AXIS])
angular_travel += RADIANS(360); angular_travel += RADIANS(360);
float mm_of_travel = HYPOT(angular_travel * radius, fabs(linear_travel)); float mm_of_travel = HYPOT(angular_travel * radius, fabs(linear_travel));
@@ -8282,13 +8282,13 @@ void prepare_move_to_destination() {
// Ensure last segment arrives at target location. // Ensure last segment arrives at target location.
#if IS_KINEMATIC #if IS_KINEMATIC
inverse_kinematics(target); inverse_kinematics(logical);
#if ENABLED(DELTA) && ENABLED(AUTO_BED_LEVELING_NONLINEAR) #if ENABLED(DELTA) && ENABLED(AUTO_BED_LEVELING_NONLINEAR)
adjust_delta(target); adjust_delta(logical);
#endif #endif
planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], target[E_AXIS], fr_mm_s, active_extruder); planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], logical[E_AXIS], fr_mm_s, active_extruder);
#else #else
planner.buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], fr_mm_s, active_extruder); planner.buffer_line(logical[X_AXIS], logical[Y_AXIS], logical[Z_AXIS], logical[E_AXIS], fr_mm_s, active_extruder);
#endif #endif
// As far as the parser is concerned, the position is now == target. In reality the // As far as the parser is concerned, the position is now == target. In reality the
@@ -8303,7 +8303,7 @@ void prepare_move_to_destination() {
void plan_cubic_move(const float offset[4]) { void plan_cubic_move(const float offset[4]) {
cubic_b_spline(current_position, destination, offset, MMS_SCALED(feedrate_mm_s), active_extruder); cubic_b_spline(current_position, destination, offset, MMS_SCALED(feedrate_mm_s), active_extruder);
// As far as the parser is concerned, the position is now == target. In reality the // As far as the parser is concerned, the position is now == destination. In reality the
// motion control system might still be processing the action and the real tool position // motion control system might still be processing the action and the real tool position
// in any intermediate location. // in any intermediate location.
set_current_to_destination(); set_current_to_destination();
@@ -8376,7 +8376,7 @@ void prepare_move_to_destination() {
//*/ //*/
} }
void inverse_kinematics(const float cartesian[XYZ]) { void inverse_kinematics(const float logical[XYZ]) {
// Inverse kinematics. // Inverse kinematics.
// Perform SCARA IK and place results in delta[]. // Perform SCARA IK and place results in delta[].
// The maths and first version were done by QHARLEY. // The maths and first version were done by QHARLEY.
@@ -8384,8 +8384,8 @@ void prepare_move_to_destination() {
static float C2, S2, SK1, SK2, THETA, PSI; static float C2, S2, SK1, SK2, THETA, PSI;
float sx = RAW_X_POSITION(cartesian[X_AXIS]) - SCARA_OFFSET_X, //Translate SCARA to standard X Y float sx = RAW_X_POSITION(logical[X_AXIS]) - SCARA_OFFSET_X, // Translate SCARA to standard X Y
sy = RAW_Y_POSITION(cartesian[Y_AXIS]) - SCARA_OFFSET_Y; // With scaling factor. sy = RAW_Y_POSITION(logical[Y_AXIS]) - SCARA_OFFSET_Y; // With scaling factor.
#if (L1 == L2) #if (L1 == L2)
C2 = HYPOT2(sx, sy) / (2 * L1_2) - 1; C2 = HYPOT2(sx, sy) / (2 * L1_2) - 1;
@@ -8403,10 +8403,10 @@ void prepare_move_to_destination() {
delta[A_AXIS] = DEGREES(THETA); // theta is support arm angle delta[A_AXIS] = DEGREES(THETA); // theta is support arm angle
delta[B_AXIS] = DEGREES(THETA + PSI); // equal to sub arm angle (inverted motor) delta[B_AXIS] = DEGREES(THETA + PSI); // equal to sub arm angle (inverted motor)
delta[Z_AXIS] = cartesian[Z_AXIS]; delta[Z_AXIS] = logical[Z_AXIS];
/** /*
DEBUG_POS("SCARA IK", cartesian); DEBUG_POS("SCARA IK", logical);
DEBUG_POS("SCARA IK", delta); DEBUG_POS("SCARA IK", delta);
SERIAL_ECHOPAIR(" SCARA (x,y) ", sx); SERIAL_ECHOPAIR(" SCARA (x,y) ", sx);
SERIAL_ECHOPAIR(",", sy); SERIAL_ECHOPAIR(",", sy);