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

Merge G2/G3 for Delta (PR#2469)

Browse Source
This commit is contained in:
Richard Wackerbarth 2015-07-19 16:19:37 -05:00
commit adfcfcba95
1 changed files with 156 additions and 136 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

292
Marlin/Marlin_main.cpp
View File

@ -410,6 +410,8 @@ bool target_direction;
void process_next_command();
void plan_arc(float target[NUM_AXIS], float *offset, uint8_t clockwise);
bool setTargetedHotend(int code);
void serial_echopair_P(const char *s_P, float v) { serialprintPGM(s_P); SERIAL_ECHO(v); }
@ -1885,130 +1887,6 @@ inline void gcode_G0_G1() {
}
}
/**
* Plan an arc in 2 dimensions
*
* The arc is approximated by generating many small linear segments.
* The length of each segment is configured in MM_PER_ARC_SEGMENT (Default 1mm)
* Arcs should only be made relatively large (over 5mm), as larger arcs with
* larger segments will tend to be more efficient. Your slicer should have
* options for G2/G3 arc generation. In future these options may be GCode tunable.
*/
void plan_arc(
float *target, // Destination position
float *offset, // Center of rotation relative to current_position
uint8_t clockwise // Clockwise?
) {
float radius = hypot(offset[X_AXIS], offset[Y_AXIS]),
center_axis0 = current_position[X_AXIS] + offset[X_AXIS],
center_axis1 = current_position[Y_AXIS] + offset[Y_AXIS],
linear_travel = target[Z_AXIS] - current_position[Z_AXIS],
extruder_travel = target[E_AXIS] - current_position[E_AXIS],
r_axis0 = -offset[X_AXIS], // Radius vector from center to current location
r_axis1 = -offset[Y_AXIS],
rt_axis0 = target[X_AXIS] - center_axis0,
rt_axis1 = target[Y_AXIS] - center_axis1;
// CCW angle of rotation between position and target from the circle center. Only one atan2() trig computation required.
float angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1);
if (angular_travel < 0) { angular_travel += RADIANS(360); }
if (clockwise) { angular_travel -= RADIANS(360); }
// Make a circle if the angular rotation is 0
if (current_position[X_AXIS] == target[X_AXIS] && current_position[Y_AXIS] == target[Y_AXIS] && angular_travel == 0)
angular_travel += RADIANS(360);
float mm_of_travel = hypot(angular_travel*radius, fabs(linear_travel));
if (mm_of_travel < 0.001) { return; }
uint16_t segments = floor(mm_of_travel / MM_PER_ARC_SEGMENT);
if (segments == 0) segments = 1;
float theta_per_segment = angular_travel/segments;
float linear_per_segment = linear_travel/segments;
float extruder_per_segment = extruder_travel/segments;
/* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
r_T = [cos(phi) -sin(phi);
sin(phi) cos(phi] * r ;
For arc generation, the center of the circle is the axis of rotation and the radius vector is
defined from the circle center to the initial position. Each line segment is formed by successive
vector rotations. This requires only two cos() and sin() computations to form the rotation
matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
all double numbers are single precision on the Arduino. (True double precision will not have
round off issues for CNC applications.) Single precision error can accumulate to be greater than
tool precision in some cases. Therefore, arc path correction is implemented.
Small angle approximation may be used to reduce computation overhead further. This approximation
holds for everything, but very small circles and large MM_PER_ARC_SEGMENT values. In other words,
theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
issue for CNC machines with the single precision Arduino calculations.
This approximation also allows plan_arc to immediately insert a line segment into the planner
without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
a correction, the planner should have caught up to the lag caused by the initial plan_arc overhead.
This is important when there are successive arc motions.
*/
// Vector rotation matrix values
float cos_T = 1-0.5*theta_per_segment*theta_per_segment; // Small angle approximation
float sin_T = theta_per_segment;
float arc_target[4];
float sin_Ti;
float cos_Ti;
float r_axisi;
uint16_t i;
int8_t count = 0;
// Initialize the linear axis
arc_target[Z_AXIS] = current_position[Z_AXIS];
// Initialize the extruder axis
arc_target[E_AXIS] = current_position[E_AXIS];
float feed_rate = feedrate*feedrate_multiplier/60/100.0;
for (i = 1; i < segments; i++) { // Increment (segments-1)
if (count < N_ARC_CORRECTION) {
// Apply vector rotation matrix to previous r_axis0 / 1
r_axisi = r_axis0*sin_T + r_axis1*cos_T;
r_axis0 = r_axis0*cos_T - r_axis1*sin_T;
r_axis1 = r_axisi;
count++;
}
else {
// Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
// Compute exact location by applying transformation matrix from initial radius vector(=-offset).
cos_Ti = cos(i*theta_per_segment);
sin_Ti = sin(i*theta_per_segment);
r_axis0 = -offset[X_AXIS]*cos_Ti + offset[Y_AXIS]*sin_Ti;
r_axis1 = -offset[X_AXIS]*sin_Ti - offset[Y_AXIS]*cos_Ti;
count = 0;
}
// Update arc_target location
arc_target[X_AXIS] = center_axis0 + r_axis0;
arc_target[Y_AXIS] = center_axis1 + r_axis1;
arc_target[Z_AXIS] += linear_per_segment;
arc_target[E_AXIS] += extruder_per_segment;
clamp_to_software_endstops(arc_target);
plan_buffer_line(arc_target[X_AXIS], arc_target[Y_AXIS], arc_target[Z_AXIS], arc_target[E_AXIS], feed_rate, active_extruder);
}
// Ensure last segment arrives at target location.
plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], feed_rate, active_extruder);
// 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
// in any intermediate location.
set_current_to_destination();
}
/**
* G2: Clockwise Arc
* G3: Counterclockwise Arc
@ -6074,9 +5952,9 @@ void mesh_plan_buffer_line(float x, float y, float z, const float e, float feed_
#if defined(DELTA) || defined(SCARA)
inline bool prepare_move_delta() {
inline bool prepare_move_delta(float target[NUM_AXIS]) {
float difference[NUM_AXIS];
for (int8_t i=0; i < NUM_AXIS; i++) difference[i] = destination[i] - current_position[i];
for (int8_t i=0; i < NUM_AXIS; i++) difference[i] = target[i] - current_position[i];
float cartesian_mm = sqrt(sq(difference[X_AXIS]) + sq(difference[Y_AXIS]) + sq(difference[Z_AXIS]));
if (cartesian_mm < 0.000001) cartesian_mm = abs(difference[E_AXIS]);
@ -6093,22 +5971,22 @@ void mesh_plan_buffer_line(float x, float y, float z, const float e, float feed_
float fraction = float(s) / float(steps);
for (int8_t i = 0; i < NUM_AXIS; i++)
destination[i] = current_position[i] + difference[i] * fraction;
target[i] = current_position[i] + difference[i] * fraction;
calculate_delta(destination);
calculate_delta(target);
#ifdef ENABLE_AUTO_BED_LEVELING
adjust_delta(destination);
adjust_delta(target);
#endif
//SERIAL_ECHOPGM("destination[X_AXIS]="); SERIAL_ECHOLN(destination[X_AXIS]);
//SERIAL_ECHOPGM("destination[Y_AXIS]="); SERIAL_ECHOLN(destination[Y_AXIS]);
//SERIAL_ECHOPGM("destination[Z_AXIS]="); SERIAL_ECHOLN(destination[Z_AXIS]);
//SERIAL_ECHOPGM("target[X_AXIS]="); SERIAL_ECHOLN(target[X_AXIS]);
//SERIAL_ECHOPGM("target[Y_AXIS]="); SERIAL_ECHOLN(target[Y_AXIS]);
//SERIAL_ECHOPGM("target[Z_AXIS]="); SERIAL_ECHOLN(target[Z_AXIS]);
//SERIAL_ECHOPGM("delta[X_AXIS]="); SERIAL_ECHOLN(delta[X_AXIS]);
//SERIAL_ECHOPGM("delta[Y_AXIS]="); SERIAL_ECHOLN(delta[Y_AXIS]);
//SERIAL_ECHOPGM("delta[Z_AXIS]="); SERIAL_ECHOLN(delta[Z_AXIS]);
plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], feedrate/60*feedrate_multiplier/100.0, active_extruder);
plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], target[E_AXIS], feedrate/60*feedrate_multiplier/100.0, active_extruder);
}
return true;
}
@ -6116,7 +5994,7 @@ void mesh_plan_buffer_line(float x, float y, float z, const float e, float feed_
#endif // DELTA || SCARA
#ifdef SCARA
inline bool prepare_move_scara() { return prepare_move_delta(); }
inline bool prepare_move_scara(float target[NUM_AXIS]) { return prepare_move_delta(target); }
#endif
#ifdef DUAL_X_CARRIAGE
@ -6193,9 +6071,9 @@ void prepare_move() {
#endif
#ifdef SCARA
if (!prepare_move_scara()) return;
if (!prepare_move_scara(destination)) return;
#elif defined(DELTA)
if (!prepare_move_delta()) return;
if (!prepare_move_delta(destination)) return;
#endif
#ifdef DUAL_X_CARRIAGE
@ -6209,6 +6087,148 @@ void prepare_move() {
set_current_to_destination();
}
/**
* Plan an arc in 2 dimensions
*
* The arc is approximated by generating many small linear segments.
* The length of each segment is configured in MM_PER_ARC_SEGMENT (Default 1mm)
* Arcs should only be made relatively large (over 5mm), as larger arcs with
* larger segments will tend to be more efficient. Your slicer should have
* options for G2/G3 arc generation. In future these options may be GCode tunable.
*/
void plan_arc(
float target[NUM_AXIS], // Destination position
float *offset, // Center of rotation relative to current_position
uint8_t clockwise // Clockwise?
) {
float radius = hypot(offset[X_AXIS], offset[Y_AXIS]),
center_axis0 = current_position[X_AXIS] + offset[X_AXIS],
center_axis1 = current_position[Y_AXIS] + offset[Y_AXIS],
linear_travel = target[Z_AXIS] - current_position[Z_AXIS],
extruder_travel = target[E_AXIS] - current_position[E_AXIS],
r_axis0 = -offset[X_AXIS], // Radius vector from center to current location
r_axis1 = -offset[Y_AXIS],
rt_axis0 = target[X_AXIS] - center_axis0,
rt_axis1 = target[Y_AXIS] - center_axis1;
// CCW angle of rotation between position and target from the circle center. Only one atan2() trig computation required.
float angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1);
if (angular_travel < 0) { angular_travel += RADIANS(360); }
if (clockwise) { angular_travel -= RADIANS(360); }
// Make a circle if the angular rotation is 0
if (current_position[X_AXIS] == target[X_AXIS] && current_position[Y_AXIS] == target[Y_AXIS] && angular_travel == 0)
angular_travel += RADIANS(360);
float mm_of_travel = hypot(angular_travel*radius, fabs(linear_travel));
if (mm_of_travel < 0.001) { return; }
uint16_t segments = floor(mm_of_travel / MM_PER_ARC_SEGMENT);
if (segments == 0) segments = 1;
float theta_per_segment = angular_travel/segments;
float linear_per_segment = linear_travel/segments;
float extruder_per_segment = extruder_travel/segments;
/* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
r_T = [cos(phi) -sin(phi);
sin(phi) cos(phi] * r ;
For arc generation, the center of the circle is the axis of rotation and the radius vector is
defined from the circle center to the initial position. Each line segment is formed by successive
vector rotations. This requires only two cos() and sin() computations to form the rotation
matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
all double numbers are single precision on the Arduino. (True double precision will not have
round off issues for CNC applications.) Single precision error can accumulate to be greater than
tool precision in some cases. Therefore, arc path correction is implemented.
Small angle approximation may be used to reduce computation overhead further. This approximation
holds for everything, but very small circles and large MM_PER_ARC_SEGMENT values. In other words,
theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
issue for CNC machines with the single precision Arduino calculations.
This approximation also allows plan_arc to immediately insert a line segment into the planner
without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
a correction, the planner should have caught up to the lag caused by the initial plan_arc overhead.
This is important when there are successive arc motions.
*/
// Vector rotation matrix values
float cos_T = 1-0.5*theta_per_segment*theta_per_segment; // Small angle approximation
float sin_T = theta_per_segment;
float arc_target[NUM_AXIS];
float sin_Ti;
float cos_Ti;
float r_axisi;
uint16_t i;
int8_t count = 0;
// Initialize the linear axis
arc_target[Z_AXIS] = current_position[Z_AXIS];
// Initialize the extruder axis
arc_target[E_AXIS] = current_position[E_AXIS];
float feed_rate = feedrate*feedrate_multiplier/60/100.0;
for (i = 1; i < segments; i++) { // Increment (segments-1)
if (count < N_ARC_CORRECTION) {
// Apply vector rotation matrix to previous r_axis0 / 1
r_axisi = r_axis0*sin_T + r_axis1*cos_T;
r_axis0 = r_axis0*cos_T - r_axis1*sin_T;
r_axis1 = r_axisi;
count++;
}
else {
// Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
// Compute exact location by applying transformation matrix from initial radius vector(=-offset).
cos_Ti = cos(i*theta_per_segment);
sin_Ti = sin(i*theta_per_segment);
r_axis0 = -offset[X_AXIS]*cos_Ti + offset[Y_AXIS]*sin_Ti;
r_axis1 = -offset[X_AXIS]*sin_Ti - offset[Y_AXIS]*cos_Ti;
count = 0;
}
// Update arc_target location
arc_target[X_AXIS] = center_axis0 + r_axis0;
arc_target[Y_AXIS] = center_axis1 + r_axis1;
arc_target[Z_AXIS] += linear_per_segment;
arc_target[E_AXIS] += extruder_per_segment;
clamp_to_software_endstops(arc_target);
#if defined(DELTA) || defined(SCARA)
calculate_delta(arc_target);
#ifdef ENABLE_AUTO_BED_LEVELING
adjust_delta(arc_target);
#endif
plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], arc_target[E_AXIS], feed_rate, active_extruder);
#else
plan_buffer_line(arc_target[X_AXIS], arc_target[Y_AXIS], arc_target[Z_AXIS], arc_target[E_AXIS], feed_rate, active_extruder);
#endif
}
// Ensure last segment arrives at target location.
#if defined(DELTA) || defined(SCARA)
calculate_delta(target);
#ifdef ENABLE_AUTO_BED_LEVELING
adjust_delta(target);
#endif
plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], target[E_AXIS], feed_rate, active_extruder);
#else
plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], feed_rate, active_extruder);
#endif
// 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
// in any intermediate location.
set_current_to_destination();
}
#if HAS_CONTROLLERFAN
void controllerFan() {