add comment

[clinton/Smoothieware.git] / src / modules / robot / Robot.cpp

diff --git a/src/modules/robot/Robot.cpp b/src/modules/robot/Robot.cpp
index 4d16a52..c8e3215 100644 (file)
--- a/src/modules/robot/Robot.cpp
+++ b/src/modules/robot/Robot.cpp
@@ -98,7 +98,6 @@
 #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
@@ -1041,6 +1040,9 @@ void Robot::process_move(Gcode *gcode, enum MOTION_MODE_T motion_mode)
             break;
     }
 
+    // needed to act as start of next arc command
+    memcpy(arc_milestone, target, sizeof(arc_milestone));
+
     if(moved) {
         // set machine_position to the calculated target
         memcpy(machine_position, target, n_motors*sizeof(float));
@@ -1453,38 +1455,36 @@ bool Robot::append_arc(Gcode * gcode, const float target[], const float offset[]
         return false;
     }
 
-    // Scary math
-    float center_axis0 = this->machine_position[this->plane_axis_0] + offset[this->plane_axis_0];
-    float center_axis1 = this->machine_position[this->plane_axis_1] + offset[this->plane_axis_1];
-    float linear_travel = target[this->plane_axis_2] - this->machine_position[this->plane_axis_2];
-    float r_axis0 = -offset[this->plane_axis_0]; // Radius vector from center to current location
+    // 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 start position
     float r_axis1 = -offset[this->plane_axis_1];
-    float rt_axis0 = target[this->plane_axis_0] - center_axis0;
-    float rt_axis1 = target[this->plane_axis_1] - center_axis1;
+    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 to see if we have a full circle, and if so, set angualr_travel.   
-    if ((this->machine_position[this->plane_axis_0]==target[this->plane_axis_0]) and
-    (this->machine_position[this->plane_axis_1]==target[this->plane_axis_1])) {
-        if (is_clockwise) {
+    //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 {
+        } else { // set angular_travel to 2pi for a counterclockwise full circle
            angular_travel = (2 * PI);
         }
     } else {
         // 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);
-     
         if (plane_axis_2 == Y_AXIS) { is_clockwise = !is_clockwise; }  //Math for XZ plane is reverse of other 2 planes
-        if (is_clockwise) { // Adjust atan2 output per direction
-           if (angular_travel > 0) { angular_travel -= (2 * PI); }  //Adjust angular_travel to be between 0 and -2Pi
-        } else {
-           if (angular_travel < 0) { angular_travel += (2 * PI); }  //Adjust angular_travel to be between 0 and 2Pi
+        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 {  // adjust angular_travel to be in the range of 0 to 2pi for counterclockwise arcs
+           if (angular_travel < 0) { angular_travel += (2 * PI); }
         }
-    } 
-    
+    }
+
     // Find the distance for this gcode
     float millimeters_of_travel = hypotf(angular_travel * radius, fabsf(linear_travel));
 
@@ -1599,6 +1599,7 @@ bool Robot::compute_arc(Gcode * gcode, const float offset[], const float target[
 
     // Find the radius
     float radius = hypotf(offset[this->plane_axis_0], offset[this->plane_axis_1]);
+
     // Set clockwise/counter-clockwise sign for mc_arc computations
     bool is_clockwise = false;
     if( motion_mode == CW_ARC ) {