|
@@ -7284,26 +7284,26 @@ void plan_arc(
|
7284
|
7284
|
) {
|
7285
|
7285
|
|
7286
|
7286
|
float radius = hypot(offset[X_AXIS], offset[Y_AXIS]),
|
7287
|
|
- center_axis0 = current_position[X_AXIS] + offset[X_AXIS],
|
7288
|
|
- center_axis1 = current_position[Y_AXIS] + offset[Y_AXIS],
|
|
7287
|
+ center_X = current_position[X_AXIS] + offset[X_AXIS],
|
|
7288
|
+ center_Y = current_position[Y_AXIS] + offset[Y_AXIS],
|
7289
|
7289
|
linear_travel = target[Z_AXIS] - current_position[Z_AXIS],
|
7290
|
7290
|
extruder_travel = target[E_AXIS] - current_position[E_AXIS],
|
7291
|
|
- r_axis0 = -offset[X_AXIS], // Radius vector from center to current location
|
7292
|
|
- r_axis1 = -offset[Y_AXIS],
|
7293
|
|
- rt_axis0 = target[X_AXIS] - center_axis0,
|
7294
|
|
- rt_axis1 = target[Y_AXIS] - center_axis1;
|
|
7291
|
+ r_X = -offset[X_AXIS], // Radius vector from center to current location
|
|
7292
|
+ r_Y = -offset[Y_AXIS],
|
|
7293
|
+ rt_X = target[X_AXIS] - center_X,
|
|
7294
|
+ rt_Y = target[Y_AXIS] - center_Y;
|
7295
|
7295
|
|
7296
|
7296
|
// CCW angle of rotation between position and target from the circle center. Only one atan2() trig computation required.
|
7297
|
|
- float angular_travel = atan2(r_axis0 * rt_axis1 - r_axis1 * rt_axis0, r_axis0 * rt_axis0 + r_axis1 * rt_axis1);
|
7298
|
|
- if (angular_travel < 0) angular_travel += RADIANS(360);
|
7299
|
|
- if (clockwise) angular_travel -= RADIANS(360);
|
|
7297
|
+ float angular_travel = atan2(r_X * rt_Y - r_Y * rt_X, r_X * rt_X + r_Y * rt_Y);
|
|
7298
|
+ if (angular_travel < 0) angular_travel += RADIANS(360);
|
|
7299
|
+ if (clockwise) angular_travel -= RADIANS(360);
|
7300
|
7300
|
|
7301
|
7301
|
// Make a circle if the angular rotation is 0
|
7302
|
|
- if (current_position[X_AXIS] == target[X_AXIS] && current_position[Y_AXIS] == target[Y_AXIS] && angular_travel == 0)
|
7303
|
|
- angular_travel += RADIANS(360);
|
|
7302
|
+ if (angular_travel == 0 && current_position[X_AXIS] == target[X_AXIS] && current_position[Y_AXIS] == target[Y_AXIS])
|
|
7303
|
+ angular_travel == RADIANS(360);
|
7304
|
7304
|
|
7305
|
7305
|
float mm_of_travel = hypot(angular_travel * radius, fabs(linear_travel));
|
7306
|
|
- if (mm_of_travel < 0.001) return;
|
|
7306
|
+ if (mm_of_travel < 0.001) return;
|
7307
|
7307
|
uint16_t segments = floor(mm_of_travel / (MM_PER_ARC_SEGMENT));
|
7308
|
7308
|
if (segments == 0) segments = 1;
|
7309
|
7309
|
|
|
@@ -7342,9 +7342,7 @@ void plan_arc(
|
7342
|
7342
|
float sin_T = theta_per_segment;
|
7343
|
7343
|
|
7344
|
7344
|
float arc_target[NUM_AXIS];
|
7345
|
|
- float sin_Ti;
|
7346
|
|
- float cos_Ti;
|
7347
|
|
- float r_axisi;
|
|
7345
|
+ float sin_Ti, cos_Ti, r_new_Y;
|
7348
|
7346
|
uint16_t i;
|
7349
|
7347
|
int8_t count = 0;
|
7350
|
7348
|
|
|
@@ -7356,28 +7354,29 @@ void plan_arc(
|
7356
|
7354
|
|
7357
|
7355
|
float feed_rate = feedrate * feedrate_multiplier / 60 / 100.0;
|
7358
|
7356
|
|
7359
|
|
- for (i = 1; i < segments; i++) { // Increment (segments-1)
|
|
7357
|
+ for (i = 1; i < segments; i++) { // Iterate (segments-1) times
|
7360
|
7358
|
|
7361
|
|
- if (count < N_ARC_CORRECTION) {
|
7362
|
|
- // Apply vector rotation matrix to previous r_axis0 / 1
|
7363
|
|
- r_axisi = r_axis0 * sin_T + r_axis1 * cos_T;
|
7364
|
|
- r_axis0 = r_axis0 * cos_T - r_axis1 * sin_T;
|
7365
|
|
- r_axis1 = r_axisi;
|
7366
|
|
- count++;
|
|
7359
|
+ if (++count < N_ARC_CORRECTION) {
|
|
7360
|
+ // Apply vector rotation matrix to previous r_X / 1
|
|
7361
|
+ r_new_Y = r_X * sin_T + r_Y * cos_T;
|
|
7362
|
+ r_X = r_X * cos_T - r_Y * sin_T;
|
|
7363
|
+ r_Y = r_new_Y;
|
7367
|
7364
|
}
|
7368
|
7365
|
else {
|
7369
|
7366
|
// Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
|
7370
|
7367
|
// Compute exact location by applying transformation matrix from initial radius vector(=-offset).
|
|
7368
|
+ // To reduce stuttering, the sin and cos could be computed at different times.
|
|
7369
|
+ // For now, compute both at the same time.
|
7371
|
7370
|
cos_Ti = cos(i * theta_per_segment);
|
7372
|
7371
|
sin_Ti = sin(i * theta_per_segment);
|
7373
|
|
- r_axis0 = -offset[X_AXIS] * cos_Ti + offset[Y_AXIS] * sin_Ti;
|
7374
|
|
- r_axis1 = -offset[X_AXIS] * sin_Ti - offset[Y_AXIS] * cos_Ti;
|
|
7372
|
+ r_X = -offset[X_AXIS] * cos_Ti + offset[Y_AXIS] * sin_Ti;
|
|
7373
|
+ r_Y = -offset[X_AXIS] * sin_Ti - offset[Y_AXIS] * cos_Ti;
|
7375
|
7374
|
count = 0;
|
7376
|
7375
|
}
|
7377
|
7376
|
|
7378
|
7377
|
// Update arc_target location
|
7379
|
|
- arc_target[X_AXIS] = center_axis0 + r_axis0;
|
7380
|
|
- arc_target[Y_AXIS] = center_axis1 + r_axis1;
|
|
7378
|
+ arc_target[X_AXIS] = center_X + r_X;
|
|
7379
|
+ arc_target[Y_AXIS] = center_Y + r_Y;
|
7381
|
7380
|
arc_target[Z_AXIS] += linear_per_segment;
|
7382
|
7381
|
arc_target[E_AXIS] += extruder_per_segment;
|
7383
|
7382
|
|