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

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

pull/1/head
Scott Lahteine 8 years ago
parent
40d3e854f6
commit
5f2f991192
2 changed files with 27 additions and 27 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      Marlin/Marlin.h
  2. 48
      Marlin/Marlin_main.cpp

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)

48
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,7 +8156,7 @@ 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?
) { ) {
@ -8164,12 +8164,12 @@ void prepare_move_to_destination() {
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);

Write Preview
Loading…
Cancel
Save