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 }