Marlin/motion_control.cpp

   1 /*
   2   motion_control.c - high level interface for issuing motion commands
   3   Part of Grbl
   4
   5   Copyright (c) 2009-2011 Simen Svale Skogsrud
   6   Copyright (c) 2011 Sungeun K. Jeon
   7
   8   Grbl is free software: you can redistribute it and/or modify
   9   it under the terms of the GNU General Public License as published by
  10   the Free Software Foundation, either version 3 of the License, or
  11   (at your option) any later version.
  12
  13   Grbl is distributed in the hope that it will be useful,
  14   but WITHOUT ANY WARRANTY; without even the implied warranty of
  15   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  16   GNU General Public License for more details.
  17
  18   You should have received a copy of the GNU General Public License
  19   along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
  20 */
  21
  22 #include "Marlin.h"
  23 #include "stepper.h"
  24 #include "planner.h"
  25
  26 // The arc is approximated by generating a huge number of tiny, linear segments. The length of each
  27 // segment is configured in settings.mm_per_arc_segment.
  28 void mc_arc(float *position, float *target, float *offset, uint8_t axis_0, uint8_t axis_1,
  29   uint8_t axis_linear, float feed_rate, float radius, uint8_t isclockwise, uint8_t extruder)
  30 {
  31   //   int acceleration_manager_was_enabled = plan_is_acceleration_manager_enabled();
  32   //   plan_set_acceleration_manager_enabled(false); // disable acceleration management for the duration of the arc
  33   float center_axis0 = position[axis_0] + offset[axis_0];
  34   float center_axis1 = position[axis_1] + offset[axis_1];
  35   float linear_travel = target[axis_linear] - position[axis_linear];
  36   float extruder_travel = target[E_AXIS] - position[E_AXIS];
  37   float r_axis0 = -offset[axis_0];  // Radius vector from center to current location
  38   float r_axis1 = -offset[axis_1];
  39   float rt_axis0 = target[axis_0] - center_axis0;
  40   float rt_axis1 = target[axis_1] - center_axis1;
  41
  42   // CCW angle between position and target from circle center. Only one atan2() trig computation required.
  43   float angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1);
  44   if (angular_travel < 0) { angular_travel += 2*M_PI; }
  45   if (isclockwise) { angular_travel -= 2*M_PI; }
  46
  47   float millimeters_of_travel = hypot(angular_travel*radius, fabs(linear_travel));
  48   if (millimeters_of_travel < 0.001) { return; }
  49   uint16_t segments = floor(millimeters_of_travel/MM_PER_ARC_SEGMENT);
  50   if(segments == 0) segments = 1;
  51
  52   /*
  53     // Multiply inverse feed_rate to compensate for the fact that this movement is approximated
  54     // by a number of discrete segments. The inverse feed_rate should be correct for the sum of
  55     // all segments.
  56     if (invert_feed_rate) { feed_rate *= segments; }
  57   */
  58   float theta_per_segment = angular_travel/segments;
  59   float linear_per_segment = linear_travel/segments;
  60   float extruder_per_segment = extruder_travel/segments;
  61
  62   /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
  63      and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
  64          r_T = [cos(phi) -sin(phi);
  65                 sin(phi)  cos(phi] * r ;
  66
  67      For arc generation, the center of the circle is the axis of rotation and the radius vector is
  68      defined from the circle center to the initial position. Each line segment is formed by successive
  69      vector rotations. This requires only two cos() and sin() computations to form the rotation
  70      matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
  71      all double numbers are single precision on the Arduino. (True double precision will not have
  72      round off issues for CNC applications.) Single precision error can accumulate to be greater than
  73      tool precision in some cases. Therefore, arc path correction is implemented.
  74
  75      Small angle approximation may be used to reduce computation overhead further. This approximation
  76      holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
  77      theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
  78      to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
  79      numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
  80      issue for CNC machines with the single precision Arduino calculations.
  81
  82      This approximation also allows mc_arc to immediately insert a line segment into the planner
  83      without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
  84      a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
  85      This is important when there are successive arc motions.
  86   */
  87   // Vector rotation matrix values
  88   float cos_T = 1-0.5*theta_per_segment*theta_per_segment; // Small angle approximation
  89   float sin_T = theta_per_segment;
  90
  91   float arc_target[4];
  92   float sin_Ti;
  93   float cos_Ti;
  94   float r_axisi;
  95   uint16_t i;
  96   int8_t count = 0;
  97
  98   // Initialize the linear axis
  99   arc_target[axis_linear] = position[axis_linear];
 100
 101   // Initialize the extruder axis
 102   arc_target[E_AXIS] = position[E_AXIS];
 103
 104   for (i = 1; i<segments; i++) { // Increment (segments-1)
 105
 106     if (count < N_ARC_CORRECTION) {
 107       // Apply vector rotation matrix
 108       r_axisi = r_axis0*sin_T + r_axis1*cos_T;
 109       r_axis0 = r_axis0*cos_T - r_axis1*sin_T;
 110       r_axis1 = r_axisi;
 111       count++;
 112     } else {
 113       // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
 114       // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
 115       cos_Ti = cos(i*theta_per_segment);
 116       sin_Ti = sin(i*theta_per_segment);
 117       r_axis0 = -offset[axis_0]*cos_Ti + offset[axis_1]*sin_Ti;
 118       r_axis1 = -offset[axis_0]*sin_Ti - offset[axis_1]*cos_Ti;
 119       count = 0;
 120     }
 121
 122     // Update arc_target location
 123     arc_target[axis_0] = center_axis0 + r_axis0;
 124     arc_target[axis_1] = center_axis1 + r_axis1;
 125     arc_target[axis_linear] += linear_per_segment;
 126     arc_target[E_AXIS] += extruder_per_segment;
 127
 128     if (min_software_endstops) {
 129       if (arc_target[X_AXIS] < min_pos[0]) arc_target[X_AXIS] = min_pos[0];
 130       if (arc_target[Y_AXIS] < min_pos[1]) arc_target[Y_AXIS] = min_pos[1];
 131       if (arc_target[Z_AXIS] < min_pos[2]) arc_target[Z_AXIS] = min_pos[2];
 132     }
 133
 134     if (max_software_endstops) {
 135       if (arc_target[X_AXIS] > X_MAX_LENGTH) arc_target[X_AXIS] = X_MAX_LENGTH;
 136       if (arc_target[Y_AXIS] > Y_MAX_LENGTH) arc_target[Y_AXIS] = Y_MAX_LENGTH;
 137       if (arc_target[Z_AXIS] > Z_MAX_LENGTH) arc_target[Z_AXIS] = Z_MAX_LENGTH;
 138     }
 139     plan_buffer_line(arc_target[X_AXIS], arc_target[Y_AXIS], arc_target[Z_AXIS], arc_target[E_AXIS], feed_rate, extruder);
 140
 141   }
 142   // Ensure last segment arrives at target location.
 143   plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], feed_rate, extruder);
 144
 145   //   plan_set_acceleration_manager_enabled(acceleration_manager_was_enabled);
 146 }
 147