+ if(segments > 1) {
+ float theta_per_segment = angular_travel / segments;
+ float linear_per_segment = linear_travel / segments;
+
+ /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
+ and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
+ r_T = [cos(phi) -sin(phi);
+ sin(phi) cos(phi] * r ;
+ For arc generation, the center of the circle is the axis of rotation and the radius vector is
+ defined from the circle center to the initial position. Each line segment is formed by successive
+ vector rotations. This requires only two cos() and sin() computations to form the rotation
+ matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
+ all float numbers are single precision on the Arduino. (True float precision will not have
+ round off issues for CNC applications.) Single precision error can accumulate to be greater than
+ tool precision in some cases. Therefore, arc path correction is implemented.
+
+ Small angle approximation may be used to reduce computation overhead further. This approximation
+ holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
+ theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
+ to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
+ numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
+ issue for CNC machines with the single precision Arduino calculations.
+ This approximation also allows mc_arc to immediately insert a line segment into the planner
+ without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
+ a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
+ This is important when there are successive arc motions.
+ */
+ // Vector rotation matrix values
+ float cos_T = 1 - 0.5F * theta_per_segment * theta_per_segment; // Small angle approximation
+ float sin_T = theta_per_segment;
+
+ // TODO we need to handle the ABC axis here by segmenting them
+ float arc_target[n_motors];
+ float sin_Ti;
+ float cos_Ti;
+ float r_axisi;
+ uint16_t i;
+ int8_t count = 0;
+
+ // init array for all axis
+ memcpy(arc_target, machine_position, n_motors*sizeof(float));
+
+ // Initialize the linear axis
+ arc_target[this->plane_axis_2] = this->machine_position[this->plane_axis_2];
+
+ for (i = 1; i < segments; i++) { // Increment (segments-1)
+ if(THEKERNEL->is_halted()) return false; // don't queue any more segments