grbl 中mc_arc()圆弧插补


  1 #ifdef USE_LINE_NUMBERS
  2 void mc_arc(float *position, float *target, float *offset, float radius, float feed_rate, 
  3 uint8_t invert_feed_rate, uint8_t axis_0, uint8_t axis_1, uint8_t axis_linear, uint8_t is_clockwise_arc, int32_t line_number)
  4 #else
  5 void mc_arc(float *position, float *target, float *offset, float radius, float feed_rate,
  6 uint8_t invert_feed_rate, uint8_t axis_0, uint8_t axis_1, uint8_t axis_linear, uint8_t is_clockwise_arc)
  7 #endif
  8 {
  9 
 10 //第一步。确定圆心坐标,圆心->起始点的向量,圆心->终止点的向量
 11 float center_axis0 = position[axis_0] + offset[axis_0];
 12 float center_axis1 = position[axis_1] + offset[axis_1];
 13 float r_axis0 = -offset[axis_0]; // Radius vector from center to current location
 14 float r_axis1 = -offset[axis_1];
 15 float rt_axis0 = target[axis_0] - center_axis0;
 16 float rt_axis1 = target[axis_1] - center_axis1;
 17 
 18 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
 19 
 20 //第二步。计算两向量的夹角(弧度)atan2()是math.h中的函数,向量a(x1,y1),向量b(x2,y2),则他们的叉乘x1y2-x2y1;点积x1x2+y1y2
 21 
 22 //atan2(叉乘,点积)可以求出两向量夹角的弧度。
 23 float angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1);
 24 if (is_clockwise_arc) { // Correct atan2 output per direction
 25 if (angular_travel >= -ARC_ANGULAR_TRAVEL_EPSILON) { angular_travel -= 2*M_PI; }
 26 } else {
 27 if (angular_travel <= ARC_ANGULAR_TRAVEL_EPSILON) { angular_travel += 2*M_PI; }
 28 }
 29 
 30 // NOTE: Segment end points are on the arc, which can lead to the arc diameter being smaller by up to
 31 // (2x) settings.arc_tolerance. For 99% of users, this is just fine. If a different arc segment fit
 32 // is desired, i.e. least-squares, midpoint on arc, just change the mm_per_arc_segment calculation.
 33 // For the intended uses of Grbl, this value shouldn't exceed 2000 for the strictest of cases.
 34 
 35 //第三步计算圆弧可以分为多少条线段arc_tolerance小线段距离圆弧的最大距离。radius半径小线段的一半是0.5*sqrt(r^2-(2r-a)^2)=0.5*sqrt(a(2r-a))
 36 uint16_t segments = floor(fabs(0.5*angular_travel*radius)/
 37 sqrt(settings.arc_tolerance*(2*radius - settings.arc_tolerance)) );
 38 
 39 if (segments) { 
 40 // Multiply inverse feed_rate to compensate for the fact that this movement is approximated
 41 // by a number of discrete segments. The inverse feed_rate should be correct for the sum of 
 42 // all segments.
 43 if (invert_feed_rate) { feed_rate *= segments; }
 44 
 45 float theta_per_segment = angular_travel/segments;
 46 float linear_per_segment = (target[axis_linear] - position[axis_linear])/segments;
 47 
 48 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
 49 and phi is the angle of rotation. Solution approach by Jens Geisler.
 50 r_T = [cos(phi) -sin(phi);
 51 sin(phi) cos(phi] * r ;
 52 
 53 For arc generation, the center of the circle is the axis of rotation and the radius vector is 
 54 defined from the circle center to the initial position. Each line segment is formed by successive
 55 vector rotations. Single precision values can accumulate error greater than tool precision in rare
 56 cases. So, exact arc path correction is implemented. This approach avoids the problem of too many very
 57 expensive trig operations [sin(),cos(),tan()] which can take 100-200 usec each to compute.
 58 
 59 Small angle approximation may be used to reduce computation overhead further. A third-order approximation
 60 (second order sin() has too much error) holds for most, if not, all CNC applications. Note that this 
 61 approximation will begin to accumulate a numerical drift error when theta_per_segment is greater than 
 62 ~0.25 rad(14 deg) AND the approximation is successively used without correction several dozen times. This
 63 scenario is extremely unlikely, since segment lengths and theta_per_segment are automatically generated
 64 and scaled by the arc tolerance setting. Only a very large arc tolerance setting, unrealistic for CNC 
 65 applications, would cause this numerical drift error. However, it is best to set N_ARC_CORRECTION from a
 66 low of ~4 to a high of ~20 or so to avoid trig operations while keeping arc generation accurate.
 67 
 68 This approximation also allows mc_arc to immediately insert a line segment into the planner 
 69 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
 70 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead. 
 71 This is important when there are successive arc motions. 
 72 */
 73 // Computes: cos_T = 1 - theta_per_segment^2/2, sin_T = theta_per_segment - theta_per_segment^3/6) in ~52usec
 74 
 75 //第四步,求半径和每条小线段的夹角T的sinT,cosT,这里用的近似算法https://wenku.baidu.com/view/34498178ba0d4a7303763a33.html
 76 
 77 //这里用到三阶,即节约运算时间又比较精确。
 78 float cos_T = 2.0 - theta_per_segment*theta_per_segment;
 79 float sin_T = theta_per_segment*0.16666667*(cos_T + 4.0);
 80 cos_T *= 0.5;
 81 
 82 float sin_Ti;
 83 float cos_Ti;
 84 float r_axisi;
 85 uint16_t i;
 86 uint8_t count = 0;
 87 
 88 for (i = 1; i<segments; i++) { // Increment (segments-1).
 89 
 90 if (count < N_ARC_CORRECTION) {
 91 // Apply vector rotation matrix. ~40 usec
 92 r_axisi = r_axis0*sin_T + r_axis1*cos_T;
 93 r_axis0 = r_axis0*cos_T - r_axis1*sin_T;
 94 r_axis1 = r_axisi;
 95 count++;
 96 } else { 
 97 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments. ~375 usec
 98 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
 99 
100 //第五步,用极坐标求每一条小线段的终点坐标,起始点与X轴的夹角为a,则终点x坐标rcos(a+T),y坐标rsin(a+T),cos(α+β)=cosαcosβ-sinαsinβ,sin(α+β)=sinαcosβ+cosαsinβ
101 cos_Ti = cos(i*theta_per_segment);
102 sin_Ti = sin(i*theta_per_segment);
103 r_axis0 = -offset[axis_0]*cos_Ti + offset[axis_1]*sin_Ti;
104 r_axis1 = -offset[axis_0]*sin_Ti - offset[axis_1]*cos_Ti;
105 count = 0;
106 }
107 
108 // Update arc_target location
109 position[axis_0] = center_axis0 + r_axis0;
110 position[axis_1] = center_axis1 + r_axis1;
111 position[axis_linear] += linear_per_segment;
112 
113 #ifdef USE_LINE_NUMBERS
114 mc_line(position, feed_rate, invert_feed_rate, line_number);
115 #else
116 mc_line(position, feed_rate, invert_feed_rate);
117 #endif
118 
119 // Bail mid-circle on system abort. Runtime command check already performed by mc_line.
120 if (sys.abort) { return; }
121 }
122 }
123 // Ensure last segment arrives at target location.
124 #ifdef USE_LINE_NUMBERS
125 mc_line(target, feed_rate, invert_feed_rate, line_number);
126 #else
127 mc_line(target, feed_rate, invert_feed_rate);
128 #endif
129 }

 


免责声明!

本站转载的文章为个人学习借鉴使用,本站对版权不负任何法律责任。如果侵犯了您的隐私权益,请联系本站邮箱yoyou2525@163.com删除。



 
粤ICP备18138465号  © 2018-2025 CODEPRJ.COM