Merge pull request #741 from bdring/Devt

Devt

FluidNC/src/GCode.cpp

@@ -331,7 +331,7 @@ Error gc_execute_line(char* line, Channel& channel) {
                                 gc_block.modal.motion = Motion::ProbeAway;
                                 break;
                             case 50:
-                                gc_block.modal.motion = Motion::ProbeAway;
+                                gc_block.modal.motion = Motion::ProbeAwayNoError;
                                 break;
                             default:
                                 FAIL(Error::GcodeUnsupportedCommand);

FluidNC/src/Kinematics/Cartesian.cpp

@@ -36,11 +36,14 @@ namespace Kinematics {
         // the mask whose limits have been reached.
         clear_bits(motorMask, limited);
 
+        auto oldAxisMask = axisMask;
+
         // Set axisMask according to the motors that are still running.
         axisMask = Machine::Axes::motors_to_axes(motorMask);
 
-        // We do not have to stop until all motors have reached their limits
-        return !axisMask;
+        // Return true when an axis drops out of the mask, causing replan
+        // on any remaining axes.
+        return axisMask != oldAxisMask;
     }
 
     void Cartesian::releaseMotors(AxisMask axisMask, MotorMask motors) {

FluidNC/src/Kinematics/WallPlotter.cpp

@@ -24,9 +24,12 @@ namespace Kinematics {
         // The motors assume they start from (0, 0, 0).
         // So we need to derive the zero lengths to satisfy the kinematic equations.
         xy_to_lengths(0, 0, zero_left, zero_right);
-        last_left  = zero_left;
-        last_right = zero_right;
-        last_z     = 0;
+        last_motor_segment_end[0] = zero_left;
+        last_motor_segment_end[1] = zero_right;
+        auto n_axis               = config->_axes->_numberAxis;
+        for (size_t axis = Z_AXIS; axis < n_axis; axis++) {
+            last_motor_segment_end[axis] = 0.0;
+        }
     }
 
     void WallPlotter::transform_cartesian_to_motors(float* cartesian, float* motors) {
@@ -39,70 +42,93 @@ namespace Kinematics {
       All linear motions pass through cartesian_to_motors() to be planned as mc_move_motors operations.
 
       Parameters:
-        target = an MAX_N_AXIS array of target positions (where the move is supposed to go)
+        target = an n_axis array of target positions (where the move is supposed to go)
         pl_data = planner data (see the definition of this type to see what it is)
-        position = an MAX_N_AXIS array of where the machine is starting from for this move
+        position = an n_axis array of where the machine is starting from for this move
     */
     bool WallPlotter::cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) {
         float    dx, dy, dz;     // segment distances in each cartesian axis
         uint32_t segment_count;  // number of segments the move will be broken in to.
 
+        auto n_axis = config->_axes->_numberAxis;
+
+        float total_cartesian_distance = vector_distance(position, target, n_axis);
+        if (total_cartesian_distance == 0) {
+            mc_move_motors(target, pl_data);
+            return true;
+        }
+
+        float cartesian_feed_rate = pl_data->feed_rate;
+
         // calculate the total X,Y axis move distance
         // Z axis is the same in both coord systems, so it does not undergo conversion
-        float dist = vector_distance(target, position, 2); // Only compute distance for both axes. X and Y
+        float xydist = vector_distance(target, position, 2);  // Only compute distance for both axes. X and Y
         // Segment our G1 and G0 moves based on yaml file. If we choose a small enough _segment_length we can hide the nonlinearity
-        segment_count = dist / _segment_length;
+        segment_count = xydist / _segment_length;
         if (segment_count < 1) {  // Make sure there is at least one segment, even if there is no movement
             // We need to do this to make sure other things like S and M codes get updated properly by
             // the planner even if there is no movement??
             segment_count = 1;
         }
-        // Calc distance of individual cartesian segments
-        dx = (target[X_AXIS] - position[X_AXIS])/segment_count; 
-        dy = (target[Y_AXIS] - position[Y_AXIS])/segment_count; 
-        dz = (target[Z_AXIS] - position[Z_AXIS])/segment_count;
-        // Current cartesian end point of the segment        
-        float seg_x = position[X_AXIS];  
-        float seg_y = position[Y_AXIS];
-        float seg_z = position[Z_AXIS];
+        float cartesian_segment_length = total_cartesian_distance / segment_count;
+
+        // Calc length of each cartesian segment - the same for all segments
+        float cartesian_segment_components[n_axis];
+        for (size_t axis = X_AXIS; axis < n_axis; axis++) {
+            cartesian_segment_components[axis] = (target[axis] - position[axis]) / segment_count;
+        }
+
+        float cartesian_segment_end[n_axis];
+        copyAxes(cartesian_segment_end, position);
+
         // Calculate desired cartesian feedrate distance ratio. Same for each seg.
-        float vdcart_ratio = pl_data->feed_rate/(dist/segment_count);
         for (uint32_t segment = 1; segment <= segment_count; segment++) {
-            // calc next cartesian end point of the next segment
-            seg_x += dx;
-            seg_y += dy;
-            seg_z += dz;
+            // calculate the cartesian end point of the next segment
+            for (size_t axis = X_AXIS; axis < n_axis; axis++) {
+                cartesian_segment_end[axis] += cartesian_segment_components[axis];
+            }
+
             // Convert cartesian space coords to motor space
-            float seg_left, seg_right; 
-            xy_to_lengths(seg_x, seg_y, seg_left, seg_right);
+            float motor_segment_end[n_axis];
+            xy_to_lengths(cartesian_segment_end[X_AXIS], cartesian_segment_end[Y_AXIS], motor_segment_end[0], motor_segment_end[1]);
+            for (size_t axis = Z_AXIS; axis < n_axis; axis++) {
+                motor_segment_end[axis] = cartesian_segment_end[axis];
+            }
+
 #ifdef USE_CHECKED_KINEMATICS
             // Check the inverse computation.
             float cx, cy;
-            // These are absolute.
-            lengths_to_xy(seg_left, seg_right, cx, cy);
+            lengths_to_xy(motor_segment_end[0], motor_segment_end[1], cx, cy);
 
-            if (abs(seg_x - cx) > 0.1 || abs(seg_y - cy) > 0.1) {
+            if (abs(cartesian_segment_end[X_AXIS] - cx) > 0.1 || abs(cartesian_segment_end[Y_AXIS] - cy) > 0.1) {
                 // FIX: Produce an alarm state?
             }
-#endif  // end USE_CHECKED_KINEMATICS
-            // Set interpolated feedrate in motor space for this segment to
-            // get desired feedrate in cartesian space
-            if (!pl_data->motion.rapidMotion) { // Rapid motions ignore feedrate. Don't convert.
-              // T=D/V, Tcart=Tmotor, Dcart/Vcart=Dmotor/Vmotor
-              // Vmotor = Dmotor*(Vcart/Dcart)
-              pl_data->feed_rate = hypot_f(last_left-seg_left,last_right-seg_right)*vdcart_ratio; 
+#endif
+            // Adjust feedrate by the ratio of the segment lengths in motor and cartesian spaces,
+            // accounting for all axes
+            if (!pl_data->motion.rapidMotion) {  // Rapid motions ignore feedrate. Don't convert.
+                                                 // T=D/V, Tcart=Tmotor, Dcart/Vcart=Dmotor/Vmotor
+                                                 // Vmotor = Dmotor*(Vcart/Dcart)
+                float motor_segment_length = vector_distance(last_motor_segment_end, motor_segment_end, n_axis);
+                pl_data->feed_rate         = cartesian_feed_rate * motor_segment_length / cartesian_segment_length;
             }
-            // TODO: G93 pl_data->motion.inverseTime logic?? Does this even make sense for wallplotter? 
-            // Remember absolute motor lengths for next time
-            last_left  = seg_left;
-            last_right = seg_right;
-            last_z     = seg_z;
+
+            // TODO: G93 pl_data->motion.inverseTime logic?? Does this even make sense for wallplotter?
+
+            // Remember the last motor position so the length can be computed the next time
+            copyAxes(last_motor_segment_end, motor_segment_end);
+
             // Initiate motor movement with converted feedrate and converted position
             // mc_move_motors() returns false if a jog is cancelled.
             // In that case we stop sending segments to the planner.
             // Note that the left motor runs backward.
             // TODO: It might be better to adjust motor direction in .yaml file by inverting direction pin??
-            float cables[MAX_N_AXIS] = { 0 - (seg_left - zero_left), 0 + (seg_right - zero_right), seg_z };
+            float cables[n_axis];
+            cables[0] = 0 - (motor_segment_end[0] - zero_left);
+            cables[1] = 0 + (motor_segment_end[1] - zero_right);
+            for (size_t axis = Z_AXIS; axis < n_axis; axis++) {
+                cables[axis] = cartesian_segment_end[axis];
+            }
             if (!mc_move_motors(cables, pl_data)) {
                 // TODO fixup last_left last_right?? What is position state when jog is cancelled?
                 return false;
@@ -115,19 +141,21 @@ namespace Kinematics {
       The status command uses motors_to_cartesian() to convert
       your motor positions to cartesian X,Y,Z... coordinates.
 
-      Convert the MAX_N_AXIS array of motor positions to cartesian in your code.
+      Convert the n_axis array of motor positions to cartesian in your code.
     */
     void WallPlotter::motors_to_cartesian(float* cartesian, float* motors, int n_axis) {
         // The motors start at zero, but effectively at zero_left, so we need to correct for the computation.
         // Note that the left motor runs backward.
         // TODO: It might be better to adjust motor direction in .yaml file by inverting direction pin??
+
         float absolute_x, absolute_y;
         lengths_to_xy((0 - motors[_left_axis]) + zero_left, (0 + motors[_right_axis]) + zero_right, absolute_x, absolute_y);
 
         cartesian[X_AXIS] = absolute_x;
         cartesian[Y_AXIS] = absolute_y;
-        cartesian[Z_AXIS] = motors[Z_AXIS];
-
+        for (size_t axis = Z_AXIS; axis < n_axis; axis++) {
+            cartesian[axis] = motors[axis];
+        }
         // Now we have numbers that if fed back into the system should produce the same values.
     }
 

FluidNC/src/Kinematics/WallPlotter.h

@@ -45,9 +45,7 @@ namespace Kinematics {
         // State
         float zero_left;   //  The left cord offset corresponding to cartesian (0, 0).
         float zero_right;  //  The right cord offset corresponding to cartesian (0, 0).
-        float last_left;   //  The last produced left cord length.
-        float last_right;  //  The last produced right cord length.
-        float last_z;      //  The last produced z value.
+        float last_motor_segment_end[MAX_N_AXIS];
 
         // Parameters
         int   _left_axis     = 0;

FluidNC/src/Logging.cpp

@@ -20,14 +20,29 @@ bool atMsgLevel(MsgLevel level) {
 #endif
 
 DebugStream::DebugStream(const char* name) {
-    DEBUG_OUT << "[MSG:" << name << ": ";
+    _charcnt = 0;
+    print("[MSG:");
+    if (*name) {
+        print(name);
+        print(": ");
+    } else {
+        print(" ");
+    }
 }
 
 size_t DebugStream::write(uint8_t c) {
-    DEBUG_OUT << static_cast<char>(c);
-    return 1;
+    if (_charcnt < MAXLINE - 2) {
+        // Leave room for ]\n\0
+        _outline[_charcnt++] = (char)c;
+        return 1;
+    }
+    return 0;
 }
 
 DebugStream::~DebugStream() {
-    DEBUG_OUT << "]\n";
+    // write() leaves space for three characters at the end
+    _outline[_charcnt++] = ']';
+    _outline[_charcnt++] = '\n';
+    _outline[_charcnt++] = '\0';
+    DEBUG_OUT << _outline;
 }

FluidNC/src/Logging.h

@@ -30,6 +30,14 @@ enum MsgLevel {
 #include "MyIOStream.h"
 
 class DebugStream : public Print {
+    // Log lines are collected in a buffer and sent to the output stream
+    // when the line is complete, thus avoiding the possibility of interleaving
+    // output from multiple cores.
+    static const int MAXLINE = 256;
+
+    char _outline[MAXLINE];
+    int  _charcnt = 0;
+
 public:
     DebugStream(const char* name);
     size_t write(uint8_t c) override;

FluidNC/src/Machine/Homing.cpp

@@ -113,9 +113,8 @@ namespace Machine {
 
         log_debug("Starting from " << target[0] << "," << target[1] << "," << target[2]);
 
-        float maxSeekTime  = 0.0;
-        float limitingRate = 0.0;
-        float ratesq       = 0.0;
+        float maxSeekTime = 0.0;
+        float ratesq      = 0.0;
 
         settle_ms = 0;
 
@@ -215,23 +214,28 @@ namespace Machine {
 
             auto seekTime = travel / axis_rate;
             if (seekTime > maxSeekTime) {
-                maxSeekTime  = seekTime;
-                limitingRate = axis_rate;
+                maxSeekTime = seekTime;
             }
         }
-        // Scale the distance array, currently in units of time, back to positions
+
         // When approaching add a fudge factor (scaler) to ensure that the limit is reached -
         // but no fudge factor when pulling off.
+        // For fast approach, scale the distance array according to the axis that will
+        // take the longest time to reach its max range at its seek rate, preserving
+        // the speeds of the axes.
+
         for (int axis = 0; axis < n_axis; axis++) {
             if (bitnum_is_true(axesMask, axis)) {
+                if (phase == Machine::Homing::Phase::FastApproach) {
+                    // For fast approach the vector direction is determined by the rates
+                    float absDistance = maxSeekTime * rates[axis];
+                    distance[axis]    = distance[axis] >= 0 ? absDistance : -absDistance;
+                }
+
                 auto paxis  = axes->_axis[axis];
                 auto homing = paxis->_homing;
                 auto scaler = approach ? (seeking ? homing->_seek_scaler : homing->_feed_scaler) : 1.0;
                 distance[axis] *= scaler;
-                if (phase == Machine::Homing::Phase::FastApproach) {
-                    // For fast approach the vector direction is determined by the rates
-                    distance[axis] *= rates[axis] / limitingRate;
-                }
                 target[axis] += distance[axis];
             }
         }

FluidNC/src/ProcessSettings.cpp

@@ -411,7 +411,7 @@ static Error show_limits(const char* value, WebUI::AuthenticationLevel auth_leve
     out << "Limit  Axes: ";
     write_limit_set(Machine::Axes::limitMask, out);
     out << '\n';
-    out << "  PosLimitPins NegLimitPins\n";
+    out << "  PosLimitPins NegLimitPins Probe\n";
     const TickType_t interval = 500;
     TickType_t       limit    = xTaskGetTickCount();
     runLimitLoop              = true;
@@ -422,6 +422,9 @@ static Error show_limits(const char* value, WebUI::AuthenticationLevel auth_leve
             write_limit_set(Machine::Axes::posLimitMask, out);
             out << ' ';
             write_limit_set(Machine::Axes::negLimitMask, out);
+            if (config->_probe->get_state()) {
+                out << " P";
+            }
             out << '\n';
             limit = thisTime + interval;
         }