Browse Source

Initial cleaning up of arc code

Scott Lahteine 9 years ago
parent
commit
5cfb2533d6
1 changed files with 25 additions and 26 deletions
  1. 25
    26
      Marlin/Marlin_main.cpp

+ 25
- 26
Marlin/Marlin_main.cpp View File

7284
 ) {
7284
 ) {
7285
 
7285
 
7286
   float radius = hypot(offset[X_AXIS], offset[Y_AXIS]),
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
         linear_travel = target[Z_AXIS] - current_position[Z_AXIS],
7289
         linear_travel = target[Z_AXIS] - current_position[Z_AXIS],
7290
         extruder_travel = target[E_AXIS] - current_position[E_AXIS],
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
   // CCW angle of rotation between position and target from the circle center. Only one atan2() trig computation required.
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
   // Make a circle if the angular rotation is 0
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
   float mm_of_travel = hypot(angular_travel * radius, fabs(linear_travel));
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
   uint16_t segments = floor(mm_of_travel / (MM_PER_ARC_SEGMENT));
7307
   uint16_t segments = floor(mm_of_travel / (MM_PER_ARC_SEGMENT));
7308
   if (segments == 0) segments = 1;
7308
   if (segments == 0) segments = 1;
7309
 
7309
 
7342
   float sin_T = theta_per_segment;
7342
   float sin_T = theta_per_segment;
7343
 
7343
 
7344
   float arc_target[NUM_AXIS];
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
   uint16_t i;
7346
   uint16_t i;
7349
   int8_t count = 0;
7347
   int8_t count = 0;
7350
 
7348
 
7356
 
7354
 
7357
   float feed_rate = feedrate * feedrate_multiplier / 60 / 100.0;
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
     else {
7365
     else {
7369
       // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
7366
       // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
7370
       // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
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
       cos_Ti = cos(i * theta_per_segment);
7370
       cos_Ti = cos(i * theta_per_segment);
7372
       sin_Ti = sin(i * theta_per_segment);
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
       count = 0;
7374
       count = 0;
7376
     }
7375
     }
7377
 
7376
 
7378
     // Update arc_target location
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
     arc_target[Z_AXIS] += linear_per_segment;
7380
     arc_target[Z_AXIS] += linear_per_segment;
7382
     arc_target[E_AXIS] += extruder_per_segment;
7381
     arc_target[E_AXIS] += extruder_per_segment;
7383
 
7382
 

Loading…
Cancel
Save