FTM: add laser feature

Marlin/src/inc/SanityCheck.h

@@ -4756,8 +4756,6 @@ static_assert(_PLUS_TEST(3), "DEFAULT_MAX_ACCELERATION values must be positive."
       #error "DIRECT_STEPPING is not yet available in FT_MOTION. Disable NO_STANDARD_MOTION if you require it."
     #elif ENABLED(DIFFERENTIAL_EXTRUDER)
       #error "DIFFERENTIAL_EXTRUDER is not yet available in FT_MOTION. Disable NO_STANDARD_MOTION if you require it."
-    #elif ENABLED(LASER_FEATURE)
-      #error "LASER_FEATURE is not yet available in FT_MOTION. Disable NO_STANDARD_MOTION if you require it."
     #elif ENABLED(Z_LATE_ENABLE)
       #error "Z_LATE_ENABLE is not yet available in FT_MOTION. Disable NO_STANDARD_MOTION if you require it."
     #endif

Marlin/src/module/ft_motion.cpp

@@ -53,6 +53,10 @@
   #include "../feature/runout.h"
 #endif
 
+#if HAS_CUTTER
+  #include "../feature/spindle_laser.h"
+#endif
+
 FTMotion ftMotion;
 
 void ft_config_t::prep_for_shaper_change() { ftMotion.prep_for_shaper_change(); }
@@ -373,6 +377,14 @@ bool FTMotion::plan_next_block() {
 
     // There was never a block? Run out the plan and bail.
     if (!current_block) {
+      #if ENABLED(LASER_FEATURE)
+      // If no movement, turn laser/cutter off
+        cutter.apply_power(0);
+      #endif
+      #if HAS_CUTTER
+        // Turn off non-laser cutter when no movement
+        cutter.apply_power(0);
+      #endif
       currentGenerator->planRunout(0);
       return false;
     }
@@ -382,13 +394,41 @@ bool FTMotion::plan_next_block() {
 
     // Handle sync blocks and skip others
     if (current_block->is_sync()) {
+      // Set laser power
+      #if ENABLED(LASER_POWER_SYNC)
+        if (cutter.cutter_mode == CUTTER_MODE_CONTINUOUS) {
+          if (current_block->is_sync_pwr()) {
+            planner.laser_inline.status.isSyncPower = true;
+            cutter.apply_power(current_block->laser.power);
+          }
+        }
+      #endif
+      // Set "fan speeds" for a laser module
+      #if ENABLED(LASER_SYNCHRONOUS_M106_M107)
+        if (current_block->is_sync_fan()) planner.sync_fan_speeds(current_block->fan_speed);
+      #endif
       if (current_block->is_sync_pos()) stepper._set_position(current_block->position);
       continue;
     }
 
     // Keep extruder position within float precision
     ensure_extruder_float_precision();
 
+    #if ENABLED(LASER_FEATURE)
+      // Apply standard laser power for non-sync blocks
+      if (cutter.cutter_mode == CUTTER_MODE_STANDARD) {
+        cutter.apply_power(current_block->laser.power);
+      }
+      // Note: CUTTER_MODE_DYNAMIC est géré dans fill_stepper_plan_buffer()
+    #endif
+
+    // For non-inline cutter, grossly apply power
+    #if HAS_CUTTER
+      if (cutter.cutter_mode == CUTTER_MODE_STANDARD) {
+        cutter.apply_power(current_block->cutter_power);
+      }
+    #endif
+
     #if ENABLED(POWER_LOSS_RECOVERY)
       recovery.info.sdpos = current_block->sdpos;
       recovery.info.current_position = current_block->start_position;
@@ -642,6 +682,42 @@ void FTMotion::fill_stepper_plan_buffer() {
     }
     tau += FTM_TS; // (s) Time since start of block
 
+    #if ENABLED(LASER_FEATURE)
+    /**
+    * CUTTER_MODE_DYNAMIC: Apply laser power based on feed velocity
+    * This replaces the block_phase_isr() dynamic mode handling
+    */
+      if (cutter.cutter_mode == CUTTER_MODE_DYNAMIC
+      && planner.laser_inline.status.isPowered
+      && stepper.current_block
+      && cutter.last_block_power != stepper.current_block->laser.power
+      ) {
+        cutter.apply_power(stepper.current_block->laser.power);
+        cutter.last_block_power = stepper.current_block->laser.power;
+      }
+
+    /**
+     * LASER_POWER_TRAP: Adjust laser power during acceleration/deceleration
+     * Power is ramped up during acceleration and down during deceleration.
+     * Delegates to trajectory generator for velocity calculation 
+     */
+      #if ENABLED(LASER_POWER_TRAP)
+        if (cutter.cutter_mode == CUTTER_MODE_CONTINUOUS) {
+          if (planner.laser_inline.status.isPowered && planner.laser_inline.status.isEnabled) {
+            // Get power ratio from trajectory generator (handles all trajectory types)
+            const float power_ratio = currentGenerator->getPowerRatioAtTime(tau);
+            stepper.current_block->laser.trap_ramp_active_pwr = 
+              stepper.current_block->laser.power * power_ratio;
+            cutter.apply_power(stepper.current_block->laser.trap_ramp_active_pwr);
+          }
+          // Not a powered move
+          else {
+            cutter.apply_power(0);
+          }
+        }
+      #endif
+    #endif
+
     // Get distance from trajectory generator
     xyze_float_t traj_coords = calc_traj_point(currentGenerator->getDistanceAtTime(tau));
     if (fastForwardUntilMotion && traj_coords == last_target_traj) {

Marlin/src/module/ft_motion/trajectory_generator.h

@@ -58,6 +58,36 @@ class TrajectoryGenerator {
    */
   virtual float getTotalDuration() const = 0;
 
+  #if ENABLED(LASER_FEATURE)
+    /**
+     * Get the velocity at time t.
+     * @param t Time since start of trajectory [s]
+     * @return Velocity [mm/s]
+     */
+    virtual float getVelocityAtTime(const float t) const = 0;
+
+    /**
+     * Get the laser power ratio at time t (relative to nominal power).
+     * Used for LASER_POWER_TRAP: interpolates power based on velocity during accel/decel phases.
+     * Returns 1.0 during cruise, < 1.0 during accel/decel phases.
+     * @param t Time since start of trajectory [s]
+     * @return Power ratio [0.0 to 1.0]
+     */
+    virtual float getPowerRatioAtTime(const float t) const {
+      const float velocity = getVelocityAtTime(t);
+      const float nominal_speed = getNominalSpeed();
+      if (nominal_speed <= 0.0f) return 0.0f;
+      const float ratio = velocity / nominal_speed;
+      return ratio > 1.0f ? 1.0f : ratio;
+    }
+
+    /**
+     * Get the nominal speed of the trajectory.
+     * @return Nominal speed [mm/s]
+     */
+    virtual float getNominalSpeed() const = 0;
+  #endif
+
   /**
    * Reset the trajectory generator to initial state.
    */
@@ -67,6 +97,10 @@ class TrajectoryGenerator {
   // Protected constructor to prevent direct instantiation
   TrajectoryGenerator() = default;
   virtual ~TrajectoryGenerator() = default;
+
+  // Member variable available for all trajectory generators
+  // Used by getNominalSpeed() in implementations
+  float nominal_speed = 0.0f;    // Peak feedrate [mm/s]
 };
 
 /**

Marlin/src/module/ft_motion/trajectory_poly5.h

@@ -83,6 +83,41 @@ class Poly5TrajectoryGenerator : public TrapezoidalTrajectoryGenerator {
     return pos_after_coast + tau * (dec_c1 + sq(tau) * (dec_c3 + tau * (dec_c4 + tau * dec_c5)));
   }
 
+  #if ENABLED(LASER_FEATURE)
+    /**
+     * Get velocity at time t for Poly5.
+     * Velocity is the derivative of position.
+     * Phase 1 (accel): d/dt[t * (c1 + t²*(c3 + t*(c4 + t*c5)))]
+     * = c1 + 3*t²*c3 + 4*t³*c4 + 5*t⁴*c5 + 2*t*(c3 + t*(c4 + t*c5))
+     */
+    float getVelocityAtTime(const float t) const override {
+      if (t < T1) {
+        // Acceleration phase: polynomial derivative
+        const float t2 = sq(t);
+        const float t3 = t2 * t;
+        const float t4 = t3 * t;
+        return acc_c1 + 2.0f * t * (acc_c3 + t * (acc_c4 + t * acc_c5))
+                      + t2 * (3.0f * acc_c3 + t * (4.0f * acc_c4 + 5.0f * t * acc_c5));
+      }
+      else if (t <= T1_plus_T2) {
+        // Coasting phase: constant velocity
+        return nominal_speed;
+      }
+      // Deceleration phase
+      const float tau = t - T1_plus_T2;
+      const float tau2 = sq(tau);
+      return dec_c1 + 2.0f * tau * (dec_c3 + tau * (dec_c4 + tau * dec_c5))
+                    + tau2 * (3.0f * dec_c3 + tau * (4.0f * dec_c4 + 5.0f * tau * dec_c5));
+    }
+
+    /**
+     * Get nominal speed for power ratio calculation
+     */
+    float getNominalSpeed() const override {
+      return nominal_speed;
+    }
+  #endif
+
   void reset() override {
     acc_c1 = acc_c3 = acc_c4 = acc_c5 = 0.0f;
     dec_c1 = dec_c3 = dec_c4 = dec_c5 = 0.0f;

Marlin/src/module/ft_motion/trajectory_poly6.cpp

@@ -107,6 +107,45 @@ float Poly6TrajectoryGenerator::getDistanceAtTime(const float t) const {
        + dec_c6 * K_u(pos_after_coast, nominal_speed, T3, u);
 }
 
+/**
+ * Helper functions for velocity calculation (derivatives)
+ * s5'(u) = v0 + 3*c3*u² + 4*c4*u³ + 5*c5*u⁴
+ * K'(u) = 3*u²*(1-u)³ - 3*u³*(1-u)² = 3*u²*(1-u)²*((1-u) - u) = 3*u²*(1-u)²*(1-2u)
+ */
+static inline float s5p_u(const float v0, const float c3, const float c4, const float c5, const float u) {
+  const float u2 = sq(u), u3 = u2 * u, u4 = u3 * u;
+  return v0 + 3.0f * c3 * u2 + 4.0f * c4 * u3 + 5.0f * c5 * u4;
+}
+
+static inline float Kp_u(const float u) {
+  const float um1 = 1.0f - u;
+  // K'(u) = d/du[u³(1-u)³] = 3u²(1-u)³ - 3u³(1-u)²
+  //       = 3u²(1-u)²[(1-u) - u] = 3u²(1-u)²(1-2u)
+  return 3.0f * sq(u) * sq(um1) * (1.0f - 2.0f * u);
+}
+
+#if ENABLED(LASER_FEATURE)
+  float Poly6TrajectoryGenerator::getVelocityAtTime(const float t) const {
+    if (t < T1) {
+      // Acceleration phase: velocity = s5'(u)/Ts + c6*K'(u)/Ts (chain rule for u=t/T1)
+      const float u = t / T1;
+      const float v_poly = s5p_u(initial_speed, acc_c3, acc_c4, acc_c5, u);
+      const float v_shape = acc_c6 * Kp_u(u);
+      return (v_poly + v_shape) / T1;
+    }
+    else if (t <= T1_plus_T2) {
+      // Coast phase: constant velocity
+      return nominal_speed;
+    }
+    // Deceleration phase
+    const float tau = t - T1_plus_T2;
+    const float u = tau / T3;
+    const float v_poly = s5p_u(nominal_speed, dec_c3, dec_c4, dec_c5, u);
+    const float v_shape = dec_c6 * Kp_u(u);
+    return (v_poly + v_shape) / T3;
+  }
+#endif
+
 void Poly6TrajectoryGenerator::reset() {
   // Reset polynomial coefficients
   acc_c3 = acc_c4 = acc_c5 = 0.0f;

Marlin/src/module/ft_motion/trajectory_poly6.h

@@ -39,6 +39,23 @@ class Poly6TrajectoryGenerator : public TrapezoidalTrajectoryGenerator {
 
   float getDistanceAtTime(const float t) const override;
 
+
+  #if ENABLED(LASER_FEATURE)
+    /**
+     * Get velocity at time t for Poly6.
+     * Velocity is derivative of position: s5'(u) + c6 * K'(u)
+     * where u = t/Tphase and K'(u) = 3*u²(1-u)³ - 3*u³(1-u)²
+     */
+  float getVelocityAtTime(const float t) const override;
+
+    /**
+     * Get nominal speed for power ratio calculation
+     */
+    float getNominalSpeed() const override {
+      return nominal_speed;
+    }
+  #endif
+
   void reset() override;
 
 private:

Marlin/src/module/ft_motion/trajectory_trapezoidal.h

@@ -84,6 +84,34 @@ class TrapezoidalTrajectoryGenerator : public TrajectoryGenerator {
     return total_duration;
   }
 
+  #if ENABLED(LASER_FEATURE)
+    /**
+     * Get velocity at time t based on trapezoidal profile
+     */
+    float getVelocityAtTime(const float t) const override {
+      if (t <= T1) {
+        // Acceleration phase: v = v0 + a*t
+        return initial_speed + acceleration * t;
+      }
+      else if (t <= T1_plus_T2) {
+        // Cruise phase: v = v_nominal
+        return nominal_speed;
+      }
+      else if (t <= total_duration) {
+        // Deceleration phase: v = v_nominal - a*(t - T1_plus_T2)
+        return nominal_speed - acceleration * (t - T1_plus_T2);
+      }
+      return 0.0f;
+    }
+
+    /**
+     * Get nominal speed for power ratio calculation
+     */
+    float getNominalSpeed() const override {
+      return nominal_speed;
+    }
+  #endif
+
   void planRunout(const float duration) override {
     reset();
     T2 = T1_plus_T2 = total_duration = duration; // Coast at zero speed for the entire duration
@@ -103,7 +131,7 @@ class TrapezoidalTrajectoryGenerator : public TrajectoryGenerator {
   float T1_plus_T2 = 0.0f;       // Cached sum of T1 + T2 for performance
   float total_duration = 0.0f;    // Cached total duration T1 + T2 + T3
   float initial_speed = 0.0f;    // Starting feedrate [mm/s]
-  float nominal_speed = 0.0f;    // Peak feedrate [mm/s]
+  // nominal_speed inherited from TrajectoryGenerator base class
   float acceleration = 0.0f;     // Acceleration [mm/s²]
   float pos_before_coast = 0.0f; // Position after acceleration phase [mm]
   float pos_after_coast = 0.0f;  // Position after acceleration and coasting phase [mm]