- float rt_axis0 = target[this->plane_axis_0] - center_axis0;
- float rt_axis1 = target[this->plane_axis_1] - center_axis1;
-
- // Patch from GRBL Firmware - Christoph Baumann 04072015
- // CCW angle between position and target from circle center. Only one atan2() trig computation required.
-
- float angular_travel = atan2f(r_axis0 * rt_axis1 - r_axis1 * rt_axis0, r_axis0 * rt_axis0 + r_axis1 * rt_axis1);
- gcode->stream->printf("Mpos Plane_Axis_0: %8.34f\r\n", machine_position[this->plane_axis_0]);
- gcode->stream->printf("Mpos Plane_Axis_1: %8.34f\r\n", machine_position[this->plane_axis_1]);
- gcode->stream->printf("Offset Plane_Axis_0: %8.34f\r\n", offset[this->plane_axis_0]);
- gcode->stream->printf("Offset Plane_Axis_1: %8.34f\r\n", offset[this->plane_axis_1]);
- gcode->stream->printf("Target Plane_Axis_0: %8.34f\r\n", target[this->plane_axis_0]);
- gcode->stream->printf("Target Plane_Axis_1: %8.34f\r\n", target[this->plane_axis_1]);
- gcode->stream->printf("center_axis0: %8.34f\r\n", center_axis0);
- gcode->stream->printf("center_axis1: %8.34f\r\n", center_axis1);
- gcode->stream->printf("Radius:%8.34f\r\n",radius);
- gcode->stream->printf("r_axis0:%8.34f\r\n",r_axis0);
- gcode->stream->printf("rt_axis0:%8.34f\r\n",rt_axis0);
- gcode->stream->printf("r_axis1:%8.34f\r\n",r_axis1);
- gcode->stream->printf("rt_axis1:%8.34f\r\n",rt_axis1);
- gcode->stream->printf("ARC_ANGULAR_TRAVEL_EPSILON:%8.64f\r\n",ARC_ANGULAR_TRAVEL_EPSILON);
- gcode->stream->printf("angular_travel1:%8.34f\r\n",angular_travel);
- if (plane_axis_2 == Y_AXIS) { is_clockwise = !is_clockwise; } //Math for XZ plane is reverse of other 2 planes
- if (is_clockwise) { // Correct atan2 output per direction
- if (angular_travel >= -ARC_ANGULAR_TRAVEL_EPSILON) { angular_travel -= (2 * PI); }
+ float rt_axis0 = target[this->plane_axis_0] - this->arc_milestone[this->plane_axis_0] - offset[this->plane_axis_0]; // Radius vector from center to target position
+ float rt_axis1 = target[this->plane_axis_1] - this->arc_milestone[this->plane_axis_1] - offset[this->plane_axis_1];
+ float angular_travel = 0;
+ //check for condition where atan2 formula will fail due to everything canceling out exactly
+ if((this->arc_milestone[this->plane_axis_0]==target[this->plane_axis_0]) && (this->arc_milestone[this->plane_axis_1]==target[this->plane_axis_1])) {
+ if (is_clockwise) { // set angular_travel to -2pi for a clockwise full circle
+ angular_travel = (-2 * PI);
+ } else { // set angular_travel to 2pi for a counterclockwise full circle
+ angular_travel = (2 * PI);
+ }