FT: Reverse DIR bits, Multi-Axis

Author: thinkyhead

Marlin/src/inc/SanityCheck.h

@@ -4033,8 +4033,12 @@ static_assert(_PLUS_TEST(4), "HOMING_FEEDRATE_MM_M values must be positive.");
 /**
  * Fixed-Time Motion limitations
  */
-#if ENABLED(FT_MOTION) && (NUM_AXES > 3 || E_STEPPERS > 1 || NUM_Z_STEPPERS > 1 || ANY(DUAL_X_CARRIAGE, HAS_DUAL_X_STEPPERS, HAS_DUAL_Y_STEPPERS, HAS_MULTI_EXTRUDER, MIXING_EXTRUDER))
-  #error "FT_MOTION is currently limited to machines with 3 linear axes and a single extruder."
+#if ENABLED(FT_MOTION)
+  #if NUM_AXES > 3
+    #error "FT_MOTION is currently limited to machines with 3 linear axes."
+  #elif ENABLED(MIXING_EXTRUDER)
+    #error "FT_MOTION is incompatible with MIXING_EXTRUDER."
+  #endif
 #endif
 
 // Multi-Stepping Limit

Marlin/src/lcd/extui/ftdi_eve_touch_ui/ftdi_eve_lib/extended/circular_progress.cpp

@@ -27,11 +27,11 @@
 namespace FTDI {
   void draw_circular_progress(CommandProcessor& cmd, int x, int y, int w, int h, float percent, char *text, uint32_t bgcolor, uint32_t fgcolor) {
     const float rim = 0.3;
-    const float a  = percent/100.0*2.0*PI;
-    const float a1 = min(PI/2, a);
-    const float a2 = min(PI/2, a-a1);
-    const float a3 = min(PI/2, a-a1-a2);
-    const float a4 = min(PI/2, a-a1-a2-a3);
+    const float a  = percent/100.0*2.0*M_PI;
+    const float a1 = min(M_PI/2, a);
+    const float a2 = min(M_PI/2, a-a1);
+    const float a3 = min(M_PI/2, a-a1-a2);
+    const float a4 = min(M_PI/2, a-a1-a2-a3);
 
     const int ro  = min(w,h) * 8;
     const int rr = ro * rim;
@@ -69,21 +69,21 @@ namespace FTDI {
     cmd.cmd(VERTEX2F(cx + ro*sin(a1) + 16,cy - ro*cos(a1) + 8));
 
     // Paint lower-right quadrant
-    if (a > PI/2) {
+    if (a > M_PI/2) {
       cmd.cmd(BEGIN(EDGE_STRIP_R));
       cmd.cmd(VERTEX2F(cx, cy));
       cmd.cmd(VERTEX2F(cx + ro*cos(a2),cy + ro*sin(a2) + 16));
     }
 
     // Paint lower-left quadrant
-    if (a > PI) {
+    if (a > M_PI) {
       cmd.cmd(BEGIN(EDGE_STRIP_B));
       cmd.cmd(VERTEX2F(cx, cy));
       cmd.cmd(VERTEX2F(cx - ro*sin(a3) - 8,cy + ro*cos(a3)));
     }
 
     // Paint upper-left quadrant
-    if (a > 1.5*PI) {
+    if (a > 1.5*M_PI) {
       cmd.cmd(BEGIN(EDGE_STRIP_L));
       cmd.cmd(VERTEX2F(cx, cy));
       cmd.cmd(VERTEX2F(cx - ro*cos(a4),cy - ro*sin(a4)));

Marlin/src/module/ft_motion.cpp

@@ -288,7 +288,7 @@ void FxdTiCtrl::loop() {
   // To be called on init or mode or zeta change.
   void FxdTiCtrl::updateShapingA(const_float_t zeta/*=FTM_SHAPING_ZETA*/, const_float_t vtol/*=FTM_SHAPING_V_TOL*/) {
 
-    const float K = exp( -zeta * PI / sqrt(1.0f - sq(zeta)) ),
+    const float K = exp( -zeta * M_PI / sqrt(1.0f - sq(zeta)) ),
                 K2 = sq(K);
 
     switch (cfg_mode) {
@@ -862,40 +862,40 @@ void FxdTiCtrl::convertToSteps(const uint32_t idx) {
         SBI(stepperCmdBuff_ApplyDir[dir_index], dir_bit);
         #if HAS_X_AXIS
           if (x_delta > 0) {
-            stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_DIR_X);
             x_dirState = stepDirState_POS;
           }
           else {
+            stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_DIR_X);
             x_dirState = stepDirState_NEG;
           }
         #endif
 
         #if HAS_Y_AXIS
           if (y_delta > 0) {
-            stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_DIR_Y);
             y_dirState = stepDirState_POS;
           }
           else {
+            stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_DIR_Y);
             y_dirState = stepDirState_NEG;
           }
         #endif
 
         #if HAS_Z_AXIS
           if (z_delta > 0) {
-            stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_DIR_Z);
             z_dirState = stepDirState_POS;
           }
           else {
+            stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_DIR_Z);
             z_dirState = stepDirState_NEG;
           }
         #endif
 
         #if HAS_EXTRUDERS
           if (e_delta > 0) {
-            stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_DIR_E);
             e_dirState = stepDirState_POS;
           }
           else {
+            stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_DIR_E);
             e_dirState = stepDirState_NEG;
           }
         #endif

Marlin/src/module/planner.cpp

@@ -3173,14 +3173,14 @@ bool Planner::buffer_line(const xyze_pos_t &cart, const_feedRate_t fr_mm_s
         if (delta.a <= POLAR_FAST_RADIUS )
           calculated_feedrate = settings.max_feedrate_mm_s[Y_AXIS];
         else {
-            // Normalized vector of movement
-            const float diffBLength = ABS((2.0f * PI * diff.a) * (diff.b / 360.0f)),
-                        diffTheta = DEGREES(ATAN2(diff.a, diffBLength)),
-                        normalizedTheta = 1.0f - (ABS(diffTheta > 90.0f ? 180.0f - diffTheta : diffTheta) / 90.0f);
-
-            // Normalized position along the radius
-            const float radiusRatio = PRINTABLE_RADIUS/delta.a;
-            calculated_feedrate += (fr_mm_s * radiusRatio * normalizedTheta);
+          // Normalized vector of movement
+          const float diffBLength = ABS((2.0f * M_PI * diff.a) * (diff.b / 360.0f)),
+                      diffTheta = DEGREES(ATAN2(diff.a, diffBLength)),
+                      normalizedTheta = 1.0f - (ABS(diffTheta > 90.0f ? 180.0f - diffTheta : diffTheta) / 90.0f);
+
+          // Normalized position along the radius
+          const float radiusRatio = PRINTABLE_RADIUS/delta.a;
+          calculated_feedrate += (fr_mm_s * radiusRatio * normalizedTheta);
         }
       }
       const feedRate_t feedrate = calculated_feedrate;

Marlin/src/module/stepper.cpp

@@ -477,17 +477,20 @@ xyze_int8_t Stepper::count_direction{0};
   #define W_APPLY_STEP(v,Q) W_STEP_WRITE(v)
 #endif
 
-#define E0_APPLY_DIR(REV) do{ (REV) ? FWD_E_DIR(0) : REV_E_DIR(0); }while(0)
-#define E1_APPLY_DIR(REV) do{ (REV) ? FWD_E_DIR(1) : REV_E_DIR(1); }while(0)
-#define E2_APPLY_DIR(REV) do{ (REV) ? FWD_E_DIR(2) : REV_E_DIR(2); }while(0)
-#define E3_APPLY_DIR(REV) do{ (REV) ? FWD_E_DIR(3) : REV_E_DIR(3); }while(0)
-#define E4_APPLY_DIR(REV) do{ (REV) ? FWD_E_DIR(4) : REV_E_DIR(4); }while(0)
-#define E5_APPLY_DIR(REV) do{ (REV) ? FWD_E_DIR(5) : REV_E_DIR(5); }while(0)
-#define E6_APPLY_DIR(REV) do{ (REV) ? FWD_E_DIR(6) : REV_E_DIR(6); }while(0)
-#define E7_APPLY_DIR(REV) do{ (REV) ? FWD_E_DIR(7) : REV_E_DIR(7); }while(0)
-
-#if DISABLED(MIXING_EXTRUDER)
+//#define E0_APPLY_DIR(FWD) do{ (FWD) ? FWD_E_DIR(0) : REV_E_DIR(0); }while(0)
+//#define E1_APPLY_DIR(FWD) do{ (FWD) ? FWD_E_DIR(1) : REV_E_DIR(1); }while(0)
+//#define E2_APPLY_DIR(FWD) do{ (FWD) ? FWD_E_DIR(2) : REV_E_DIR(2); }while(0)
+//#define E3_APPLY_DIR(FWD) do{ (FWD) ? FWD_E_DIR(3) : REV_E_DIR(3); }while(0)
+//#define E4_APPLY_DIR(FWD) do{ (FWD) ? FWD_E_DIR(4) : REV_E_DIR(4); }while(0)
+//#define E5_APPLY_DIR(FWD) do{ (FWD) ? FWD_E_DIR(5) : REV_E_DIR(5); }while(0)
+//#define E6_APPLY_DIR(FWD) do{ (FWD) ? FWD_E_DIR(6) : REV_E_DIR(6); }while(0)
+//#define E7_APPLY_DIR(FWD) do{ (FWD) ? FWD_E_DIR(7) : REV_E_DIR(7); }while(0)
+
+#if ENABLED(MIXING_EXTRUDER)
+  #define E_APPLY_DIR(FWD,Q) do{ if (FWD) { MIXER_STEPPER_LOOP(j) FWD_E_DIR(j); } else { MIXER_STEPPER_LOOP(j) REV_E_DIR(j); } }while(0)
+#else
   #define E_APPLY_STEP(v,Q) E_STEP_WRITE(stepper_extruder, v)
+  #define E_APPLY_DIR(FWD,Q) do{ if (FWD) { FWD_E_DIR(stepper_extruder); } else { REV_E_DIR(stepper_extruder); } }while(0)
 #endif
 
 #define CYCLES_TO_NS(CYC) (1000UL * (CYC) / ((F_CPU) / 1000000))
@@ -621,36 +624,15 @@ void Stepper::disable_all_steppers() {
  *   COREYZ: Y_AXIS=B_AXIS and Z_AXIS=C_AXIS
  */
 void Stepper::apply_directions() {
-
   DIR_WAIT_BEFORE();
 
-  NUM_AXIS_CODE(
+  LOGICAL_AXIS_CODE(
+    SET_STEP_DIR(E),
     SET_STEP_DIR(X), SET_STEP_DIR(Y), SET_STEP_DIR(Z), // ABC
     SET_STEP_DIR(I), SET_STEP_DIR(J), SET_STEP_DIR(K),
     SET_STEP_DIR(U), SET_STEP_DIR(V), SET_STEP_DIR(W)
   );
 
-  #if HAS_EXTRUDERS
-     // Because this is valid for the whole block we don't know
-     // what E steppers will step. Likely all. Set all.
-    if (motor_direction(E_AXIS)) {
-      #if ENABLED(MIXING_EXTRUDER)
-        MIXER_STEPPER_LOOP(j) REV_E_DIR(j);
-      #else
-        REV_E_DIR(stepper_extruder);
-      #endif
-      count_direction.e = -1;
-    }
-    else {
-      #if ENABLED(MIXING_EXTRUDER)
-        MIXER_STEPPER_LOOP(j) FWD_E_DIR(j);
-      #else
-        FWD_E_DIR(stepper_extruder);
-      #endif
-      count_direction.e = 1;
-    }
-  #endif // HAS_EXTRUDERS
-
   DIR_WAIT_AFTER();
 }
 
@@ -2409,29 +2391,16 @@ hal_timer_t Stepper::block_phase_isr() {
           if (la_active) {
             const uint32_t la_step_rate = la_advance_steps > current_block->final_adv_steps ? current_block->la_advance_rate : 0;
             if (la_step_rate != step_rate) {
-              const bool reverse_e = la_step_rate > step_rate;
-              la_interval = calc_timer_interval((reverse_e ? la_step_rate - step_rate : step_rate - la_step_rate) >> current_block->la_scaling);
+              const bool forward_e = la_step_rate <= step_rate;
+              la_interval = calc_timer_interval((forward_e ? step_rate - la_step_rate : la_step_rate - step_rate) >> current_block->la_scaling);
 
-              if (reverse_e != motor_direction(E_AXIS)) {
+              if (forward_e == motor_direction(E_AXIS)) {
                 last_direction_bits.toggle(E_AXIS);
                 count_direction.e = -count_direction.e;
 
                 DIR_WAIT_BEFORE();
 
-                if (reverse_e) {
-                  #if ENABLED(MIXING_EXTRUDER)
-                    MIXER_STEPPER_LOOP(j) REV_E_DIR(j);
-                  #else
-                    REV_E_DIR(stepper_extruder);
-                  #endif
-                }
-                else {
-                  #if ENABLED(MIXING_EXTRUDER)
-                    MIXER_STEPPER_LOOP(j) FWD_E_DIR(j);
-                  #else
-                    FWD_E_DIR(stepper_extruder);
-                  #endif
-                }
+                E_APPLY_DIR(forward_e, false);
 
                 DIR_WAIT_AFTER();
               }
@@ -3448,36 +3417,36 @@ void Stepper::report_positions() {
     #if HAS_Z_AXIS
       // Z is handled differently to update the stepper
       // counts (needed by Marlin for bed level probing).
-      const bool z_dir = TEST(command, FT_BIT_DIR_Z),
+      const bool z_rev = TEST(command, FT_BIT_DIR_Z),
                 z_step = TEST(command, FT_BIT_STEP_Z);
     #endif
 
     if (applyDir) {
-      TERN_(HAS_X_AXIS, X_DIR_WRITE(TEST(command, FT_BIT_DIR_X)));
-      TERN_(HAS_Y_AXIS, Y_DIR_WRITE(TEST(command, FT_BIT_DIR_Y)));
-      TERN_(HAS_Z_AXIS, Z_DIR_WRITE(z_dir));
-      TERN_(HAS_EXTRUDERS, E0_DIR_WRITE(TEST(command, FT_BIT_DIR_E)));
+      TERN_(HAS_X_AXIS, X_APPLY_DIR(!TEST(command, FT_BIT_DIR_X), false));
+      TERN_(HAS_Y_AXIS, Y_APPLY_DIR(!TEST(command, FT_BIT_DIR_Y), false));
+      TERN_(HAS_Z_AXIS, Z_APPLY_DIR(!z_rev, false));
+      TERN_(HAS_EXTRUDERS, E_APPLY_DIR(!TEST(command, FT_BIT_DIR_E), false));
       DIR_WAIT_AFTER();
     }
 
-    TERN_(HAS_X_AXIS, X_STEP_WRITE(TEST(command, FT_BIT_STEP_X)));
-    TERN_(HAS_Y_AXIS, Y_STEP_WRITE(TEST(command, FT_BIT_STEP_Y)));
-    TERN_(HAS_Z_AXIS, Z_STEP_WRITE(z_step));
-    TERN_(HAS_EXTRUDERS, E0_STEP_WRITE(TEST(command, FT_BIT_STEP_E)));
+    TERN_(HAS_X_AXIS, X_APPLY_STEP(TEST(command, FT_BIT_STEP_X)));
+    TERN_(HAS_Y_AXIS, Y_APPLY_STEP(TEST(command, FT_BIT_STEP_Y)));
+    TERN_(HAS_Z_AXIS, Z_APPLY_STEP(z_step));
+    TERN_(HAS_EXTRUDERS, E_APPLY_STEP(TEST(command, FT_BIT_STEP_E)));
 
     START_TIMED_PULSE();
 
     #if HAS_Z_AXIS
       // Update step counts
-      if (z_step) count_position.z += z_dir ? 1 : -1;
+      if (z_step) count_position.z += z_rev ? -1 : 1;
     #endif
 
     AWAIT_HIGH_PULSE();
 
-    X_STEP_WRITE(0);
-    TERN_(HAS_Y_AXIS, Y_STEP_WRITE(0));
-    TERN_(HAS_Z_AXIS, Z_STEP_WRITE(0));
-    TERN_(HAS_EXTRUDERS, E0_STEP_WRITE(0));
+    TERN_(HAS_X_AXIS, X_APPLY_STEP(0, false));
+    TERN_(HAS_Y_AXIS, Y_APPLY_STEP(0, false));
+    TERN_(HAS_Z_AXIS, Z_APPLY_STEP(0, false));
+    TERN_(HAS_EXTRUDERS, E_APPLY_STEP(0, false));
 
   } // Stepper::fxdTiCtrl_stepper