/*****************************************************************************
* mp_arc() - setup an arc move for runtime
*
* Generates an arc by queueing line segments to the move buffer.
* The arc is approximated by generating a large number of tiny, linear
* segments. The length of the segments is configured in motion_control.h
* as MM_PER_ARC_SEGMENT.
*
* Parts of this routine were originally sourced from the grbl project.
*/
uint8_t ar_arc( const double target[],
const double i, const double j, const double k,
const double theta, // starting angle
const double radius, // radius of the circle in mm
const double angular_travel,// radians along arc (+CW, -CCW)
const double linear_travel,
const uint8_t axis_1, // circle plane in tool space
const uint8_t axis_2, // circle plane in tool space
const uint8_t axis_linear, // linear travel if helical motion
const double minutes) // time to complete the move
{
if (ar.run_state != MOVE_STATE_OFF) {
return (TG_INTERNAL_ERROR); // (not supposed to fail)
}
ar.linenum = cm_get_linenum(); // get line number as debugging convenience
// "move_length" is the total mm of travel of the helix (or just arc)
ar.length = hypot(angular_travel * radius, fabs(linear_travel));
if (ar.length < cfg.arc_segment_len) { // too short to draw
return (TG_ZERO_LENGTH_MOVE);
}
// load the move struct for an arc
cm_get_model_canonical_position_vector(ar.position);// set initial arc position
copy_axis_vector(ar.endpoint, target); // save the arc endpoint
ar.time = minutes;
ar.theta = theta;
ar.radius = radius;
ar.axis_1 = axis_1;
ar.axis_2 = axis_2;
ar.axis_linear = axis_linear;
ar.angular_travel = angular_travel;
ar.linear_travel = linear_travel;
// find the minimum segments by time and by distance as the segments
// can't be shorter than the min update interval or the min seg length
ar.segments = ceil(min(
(ar.time * MICROSECONDS_PER_MINUTE / MIN_ARC_SEGMENT_USEC),
(ar.length / cfg.arc_segment_len)));
ar.segment_count = (uint32_t)ar.segments;
ar.segment_theta = ar.angular_travel / ar.segments;
ar.segment_linear_travel = ar.linear_travel / ar.segments;
ar.segment_time = ar.time / ar.segments;
ar.center_1 = ar.position[ar.axis_1] - sin(ar.theta) * ar.radius;
ar.center_2 = ar.position[ar.axis_2] - cos(ar.theta) * ar.radius;
ar.target[ar.axis_linear] = ar.position[ar.axis_linear];
ar.run_state = MOVE_STATE_RUN;
return (TG_OK);
}
uint8_t ar_arc_callback()
{
if (ar.run_state == MOVE_STATE_OFF) { return (TG_NOOP);}
if (mp_test_write_buffer() == FALSE) { return (TG_EAGAIN);}
if (ar.run_state == MOVE_STATE_RUN) {
if (--ar.segment_count > 0) {
ar.theta += ar.segment_theta;
ar.target[ar.axis_1] = ar.center_1 + sin(ar.theta) * ar.radius;
ar.target[ar.axis_2] = ar.center_2 + cos(ar.theta) * ar.radius;
ar.target[ar.axis_linear] += ar.segment_linear_travel;
(void)MP_LINE(ar.target, ar.segment_time);
copy_axis_vector(ar.position, ar.target); // update runtime position
return (TG_EAGAIN);
} else {
(void)MP_LINE(ar.endpoint, ar.segment_time);// do last segment to the exact endpoint
}
}
ar.run_state = MOVE_STATE_OFF;
return (TG_OK);
}
void cm_set_gcode_model_endpoint_position(uint8_t status)
{
if (status == TG_OK) { copy_axis_vector(gm.position, gm.target);}
}
double *cm_get_model_canonical_position_vector(double position[])
{
copy_axis_vector(position, gm.position);
return (position);
}
void mp_set_axes_position(const float position[])
{
copy_axis_vector(mm.position, position);
copy_axis_vector(mr.position, position);
}
void mp_set_plan_position(const float position[])
{
copy_axis_vector(mm.position, position);
}
/*
* mp_set_plan_position() - sets planning position (for G92)
* mp_get_plan_position() - returns planning position
* mp_set_axis_position() - sets both planning and runtime positions (for G2/G3)
*
* Keeping track of position is complicated by the fact that moves exist in
* several reference frames. The scheme to keep this straight is:
*
* - mm.position - start and end position for planning
* - mr.position - current position of runtime segment
* - mr.target - target position of runtime segment
* - mr.endpoint - final target position of runtime segment
*
* Note that the positions are set immediately when they are computed and
* are not an accurate representation of the tool position. In reality
* the motors will still be processing the action and the real tool
* position is still close to the starting point.
*/
float *mp_get_plan_position(float position[])
{
copy_axis_vector(position, mm.position);
return (position);
}
