#define ymax_checksum CHECKSUM("y_max")
#define zmax_checksum CHECKSUM("z_max")
-#define ARC_ANGULAR_TRAVEL_EPSILON 5E-9F // Float (radians)
#define PI 3.14159265358979323846F // force to be float, do not use M_PI
// The Robot converts GCodes into actual movements, and then adds them to the Planner, which passes them to the Conveyor so they can be added to the queue
return false;
}
- // Scary math
+ // Scary math.
+ // We need to use arc_milestone here to get accurate arcs as previous machine_position may have been skipped due to small movements
float center_axis0 = this->arc_milestone[this->plane_axis_0] + offset[this->plane_axis_0];
float center_axis1 = this->arc_milestone[this->plane_axis_1] + offset[this->plane_axis_1];
float linear_travel = target[this->plane_axis_2] - this->arc_milestone[this->plane_axis_2];
- float r_axis0 = -offset[this->plane_axis_0]; // Radius vector from center to current location
+ float r_axis0 = -offset[this->plane_axis_0]; // Radius vector from center to start position
float r_axis1 = -offset[this->plane_axis_1];
- float rt_axis0 = target[this->plane_axis_0] - this->arc_milestone[this->plane_axis_0] - offset[this->plane_axis_0];
+ 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;
-
- gcode->stream->printf("Machine_Position Plane_Axis_0: %8.34f\r\n", machine_position[this->plane_axis_0]);
- gcode->stream->printf("Machine_Position Plane_Axis_1: %8.34f\r\n", machine_position[this->plane_axis_1]);
- gcode->stream->printf("Arc Milestone Plane_Axis_0: %8.34f\r\n", arc_milestone[this->plane_axis_0]);
- gcode->stream->printf("Arc_Milestone Plane_Axis_1: %8.34f\r\n", arc_milestone[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("r_axis1: %8.34f\r\n",r_axis1);
- gcode->stream->printf("rt2_axis0: %8.34f\r\n",rt_axis0);
- gcode->stream->printf("rt2_axis1: %8.34f\r\n",rt_axis1);
- gcode->stream->printf("Old rt_axis0: %8.34f\r\n",target[this->plane_axis_0] - center_axis0);
- gcode->stream->printf("Old rt_axis1: %8.34f\r\n",target[this->plane_axis_1] - center_axis1);
- gcode->stream->printf("ARC_ANGULAR_TRAVEL_EPSILON: %8.64f\r\n",ARC_ANGULAR_TRAVEL_EPSILON);
-
+ //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])) {
- gcode->stream->printf("Full Circle: True\r\n");
- if (is_clockwise) {
+ if (is_clockwise) { // set angular_travel to -2pi for a clockwise full circle
angular_travel = (-2 * PI);
- } else {
+ } else { // set angular_travel to 2pi for a counterclockwise full circle
angular_travel = (2 * PI);
}
- gcode->stream->printf("Full Circle angular_travel: %8.34f\r\n",angular_travel);
} else {
- gcode->stream->printf("Full Circle: False\r\n");
-
// Patch from GRBL Firmware - Christoph Baumann 04072015
// CCW angle between position and target from circle center. Only one atan2() trig computation required.
- // Only run if not a full circle
+ // Only run if not a full circle or angular travel will incorrectly result in 0.0f
angular_travel = atan2f(r_axis0 * rt_axis1 - r_axis1 * rt_axis0, r_axis0 * rt_axis0 + r_axis1 * rt_axis1);
- gcode->stream->printf("initial 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); }
- } else {
- if (angular_travel <= ARC_ANGULAR_TRAVEL_EPSILON) { angular_travel += (2 * PI); }
- }
- gcode->stream->printf("old angular_travel2: %8.34f\r\n",angular_travel);
-
- angular_travel = atan2f(r_axis0 * rt_axis1 - r_axis1 * rt_axis0, r_axis0 * rt_axis0 + r_axis1 * rt_axis1);
-
- 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 (is_clockwise) { // adjust angular_travel to be in the range of -2pi to 0 for clockwise arcs
if (angular_travel > 0) { angular_travel -= (2 * PI); }
- } else {
+ } else { // adjust angular_travel to be in the range of 0 to 2pi for counterclockwise arcs
if (angular_travel < 0) { angular_travel += (2 * PI); }
}
}
- gcode->stream->printf("new angular_travel2: %8.34f\r\n",angular_travel);
-
// Find the distance for this gcode
float millimeters_of_travel = hypotf(angular_travel * radius, fabsf(linear_travel));
- gcode->stream->printf("millimeters_of_travel:%8.34f\r\n",millimeters_of_travel);
// We don't care about non-XYZ moves ( for example the extruder produces some of those )
if( millimeters_of_travel < 0.000001F ) {
HCoop / clinton / Smoothieware.git / blobdiff