Wallplotter feedrate logic

FluidNC/src/Kinematics/WallPlotter.cpp

@@ -49,31 +49,32 @@ namespace Kinematics {
 
         // 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, Y_AXIS);
+        float dist = 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;
         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 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];
+        // 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];
+        // 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;
-            float seg_left, seg_right;
+            // Convert cartesian space coords to motor space
+            float seg_left, seg_right; 
             xy_to_lengths(seg_x, seg_y, seg_left, seg_right);
-            // TODO: Need to adjust cartesian feedrate to motor/plotter space, just leave them alone for now
-
 #ifdef USE_CHECKED_KINEMATICS
             // Check the inverse computation.
             float cx, cy;
@@ -84,18 +85,26 @@ namespace Kinematics {
                 // FIX: Produce an alarm state?
             }
 #endif  // end USE_CHECKED_KINEMATICS
-
-            // mc_move_motors() returns false if a jog is cancelled.
-            // In that case we stop sending segments to the planner.
-
+            // 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; 
+            }
+            // 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;
-
+            // 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 - (last_left - zero_left), 0 + (last_right - zero_right), seg_z };
+            float cables[MAX_N_AXIS] = { 0 - (seg_left - zero_left), 0 + (seg_right - zero_right), seg_z };
             if (!mc_move_motors(cables, pl_data)) {
+                // TODO fixup last_left last_right?? What is position state when jog is cancelled?
                 return false;
             }
         }

FluidNC/src/Spindles/BESCSpindle.cpp

@@ -52,11 +52,9 @@ namespace Spindles {
             shelfSpeeds(4000, 20000);
         }
 
-        // We set the dev_speed scale in the speed map to the full PWM period.
-        // Then, in set_output, we map the dev_speed range of 0..64K to the pulse
-        // length range of ~1ms .. 2ms
-        setupSpeeds(_pwm->period());
-
+        // Use yaml speed_map to setup speed map for "spindle speed" conversion to timer counts used by PWM controller
+        //setupSpeeds(_pulse_span_counts); // Map the counts for just the part of the pulse that changes to keep math inside 32bits later...
+        setupSpeeds(_pwm->period());       // Map the entire pulse width period in counts
         stop();
         config_message();
     }
@@ -73,19 +71,22 @@ namespace Spindles {
 
         _current_pwm_duty = duty;
 
-        // This maps the dev_speed range of 0.._pwm->period() into the pulse length
+        // This maps the dev_speed range of 0..(1<<_pwm_precision) into the pulse length
         // where _min_pulse_counts represents off and (_min_pulse_counts + _pulse_span_counts)
         // represents full on.  Typically the off value is a 1ms pulse length and the
         // full on value is a 2ms pulse.
-        uint32_t pulse_counts = _min_pulse_counts + ((duty * _pulse_span_counts) / _pwm->period());
+        // uint32_t pulse_counts = _min_pulse_counts + (_pulse_span_counts * (uint64_t) duty)/_pwm->period();
+        _pwm->setDuty(_min_pulse_counts + (_pulse_span_counts * (uint64_t) duty)/_pwm->period());
+        // _pwm->setDuty(_min_pulse_counts+duty); // More efficient by keeping math within 32bits??
+        // log_info(name() << " duty:" << duty << " _min_pulse_counts:" << _min_pulse_counts
+        //                 << " _pulse_span_counts:" << _pulse_span_counts << " pulse_counts" << pulse_counts);
 
-        _pwm->setDuty(pulse_counts);
     }
 
     // prints the startup message of the spindle config
     void BESC::config_message() {
         log_info(name() << " Spindle Out:" << _output_pin.name() << " Min:" << _min_pulse_us << "us Max:" << _max_pulse_us
-                        << "us Freq:" << _pwm->frequency() << "Hz Period:" << _pwm->period());
+                        << "us Freq:" << _pwm->frequency() << "Hz Full Period count:" << _pwm->period());
     }
 
     // Configuration registration

FluidNC/src/Spindles/BESCSpindle.h

@@ -34,8 +34,8 @@ namespace Spindles {
         const uint32_t besc_pwm_max_freq = 2000;  // 50 Hz
 
         // Calculated
-        uint16_t _pulse_span_counts;  // In counts of a 16-bit counter
-        uint16_t _min_pulse_counts;   // In counts of a 16-bit counter
+        uint32_t _pulse_span_counts;  // In counts of a 32-bit counter. ESP32 uses up to 20bits
+        uint32_t _min_pulse_counts;   // In counts of a 32-bit counter  ESP32 uses up to 20bits
 
     protected:
         // Configurable

FluidNC/src/Spindles/PWMSpindle.h

@@ -59,7 +59,7 @@ namespace Spindles {
         virtual ~PWM() {}
 
     protected:
-        int32_t _current_pwm_duty = 0;
+        uint32_t _current_pwm_duty = 0;
         PwmPin* _pwm              = nullptr;
 
         // Configurable

example_configs/WallPlotter.yaml

@@ -104,7 +104,7 @@ besc:
   spindown_ms: 2000
   tool_num: 100
   speed_map: 0=0.000% 255=100.000%
-  min_pulse_us: 600
-  max_pulse_us: 2300
+  min_pulse_us: 700
+  max_pulse_us: 2200