|
|
@ -44,9 +44,9 @@
|
|
|
|
* 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 rtarget[XYZE], // Destination position
|
|
|
|
const float (&cart)[XYZE], // Destination position
|
|
|
|
float *offset, // Center of rotation relative to current_position
|
|
|
|
const float (&offset)[2], // Center of rotation relative to current_position
|
|
|
|
uint8_t clockwise // Clockwise?
|
|
|
|
const uint8_t clockwise // Clockwise?
|
|
|
|
) {
|
|
|
|
) {
|
|
|
|
#if ENABLED(CNC_WORKSPACE_PLANES)
|
|
|
|
#if ENABLED(CNC_WORKSPACE_PLANES)
|
|
|
|
AxisEnum p_axis, q_axis, l_axis;
|
|
|
|
AxisEnum p_axis, q_axis, l_axis;
|
|
|
@ -66,10 +66,10 @@ void plan_arc(
|
|
|
|
const float radius = HYPOT(r_P, r_Q),
|
|
|
|
const float radius = HYPOT(r_P, r_Q),
|
|
|
|
center_P = current_position[p_axis] - r_P,
|
|
|
|
center_P = current_position[p_axis] - r_P,
|
|
|
|
center_Q = current_position[q_axis] - r_Q,
|
|
|
|
center_Q = current_position[q_axis] - r_Q,
|
|
|
|
rt_X = rtarget[p_axis] - center_P,
|
|
|
|
rt_X = cart[p_axis] - center_P,
|
|
|
|
rt_Y = rtarget[q_axis] - center_Q,
|
|
|
|
rt_Y = cart[q_axis] - center_Q,
|
|
|
|
linear_travel = rtarget[l_axis] - current_position[l_axis],
|
|
|
|
linear_travel = cart[l_axis] - current_position[l_axis],
|
|
|
|
extruder_travel = rtarget[E_AXIS] - current_position[E_AXIS];
|
|
|
|
extruder_travel = cart[E_AXIS] - current_position[E_AXIS];
|
|
|
|
|
|
|
|
|
|
|
|
// 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_P * rt_Y - r_Q * rt_X, r_P * rt_X + r_Q * rt_Y);
|
|
|
|
float angular_travel = ATAN2(r_P * rt_Y - r_Q * rt_X, r_P * rt_X + r_Q * rt_Y);
|
|
|
@ -77,7 +77,7 @@ void plan_arc(
|
|
|
|
if (clockwise) angular_travel -= RADIANS(360);
|
|
|
|
if (clockwise) angular_travel -= RADIANS(360);
|
|
|
|
|
|
|
|
|
|
|
|
// Make a circle if the angular rotation is 0 and the target is current position
|
|
|
|
// Make a circle if the angular rotation is 0 and the target is current position
|
|
|
|
if (angular_travel == 0 && current_position[p_axis] == rtarget[p_axis] && current_position[q_axis] == rtarget[q_axis])
|
|
|
|
if (angular_travel == 0 && current_position[p_axis] == cart[p_axis] && current_position[q_axis] == cart[q_axis])
|
|
|
|
angular_travel = RADIANS(360);
|
|
|
|
angular_travel = RADIANS(360);
|
|
|
|
|
|
|
|
|
|
|
|
const float mm_of_travel = HYPOT(angular_travel * radius, FABS(linear_travel));
|
|
|
|
const float mm_of_travel = HYPOT(angular_travel * radius, FABS(linear_travel));
|
|
|
@ -177,7 +177,7 @@ void plan_arc(
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
// Ensure last segment arrives at target location.
|
|
|
|
// Ensure last segment arrives at target location.
|
|
|
|
planner.buffer_line_kinematic(rtarget, fr_mm_s, active_extruder);
|
|
|
|
planner.buffer_line_kinematic(cart, fr_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 == target. 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
|
|
|
|