Selaa lähdekoodia

Fix/Improve junction deviation

- Respect axis max acceleration limits instead of forcing a fixed acceleration value.
- The `junction_unit_vec` ensures proper handling of entry and exit speeds even when the axes involved have different limits.
Scott Lahteine 7 vuotta sitten
vanhempi
commit
2deff0d9b9

+ 0
- 1
Marlin/Configuration_adv.h Näytä tiedosto

447
 //#define JUNCTION_DEVIATION
447
 //#define JUNCTION_DEVIATION
448
 #if ENABLED(JUNCTION_DEVIATION)
448
 #if ENABLED(JUNCTION_DEVIATION)
449
   #define JUNCTION_DEVIATION_MM 0.02  // (mm) Distance from real junction edge
449
   #define JUNCTION_DEVIATION_MM 0.02  // (mm) Distance from real junction edge
450
-  #define JUNCTION_ACCELERATION 1000  // (mm/s²) Maximum centripetal acceleration
451
   //#define JUNCTION_DEVIATION_INCLUDE_E
450
   //#define JUNCTION_DEVIATION_INCLUDE_E
452
 #endif
451
 #endif
453
 
452
 

+ 0
- 1
Marlin/src/config/default/Configuration_adv.h Näytä tiedosto

447
 //#define JUNCTION_DEVIATION
447
 //#define JUNCTION_DEVIATION
448
 #if ENABLED(JUNCTION_DEVIATION)
448
 #if ENABLED(JUNCTION_DEVIATION)
449
   #define JUNCTION_DEVIATION_MM 0.02  // (mm) Distance from real junction edge
449
   #define JUNCTION_DEVIATION_MM 0.02  // (mm) Distance from real junction edge
450
-  #define JUNCTION_ACCELERATION 1000  // (mm/s²) Maximum centripetal acceleration
451
   //#define JUNCTION_DEVIATION_INCLUDE_E
450
   //#define JUNCTION_DEVIATION_INCLUDE_E
452
 #endif
451
 #endif
453
 
452
 

+ 3
- 1
Marlin/src/inc/SanityCheck.h Näytä tiedosto

278
 #elif defined(JUNCTION_DEVIATION_FACTOR)
278
 #elif defined(JUNCTION_DEVIATION_FACTOR)
279
   #error "JUNCTION_DEVIATION_FACTOR is now JUNCTION_DEVIATION_MM. Please update your configuration."
279
   #error "JUNCTION_DEVIATION_FACTOR is now JUNCTION_DEVIATION_MM. Please update your configuration."
280
 #elif defined(JUNCTION_ACCELERATION_FACTOR)
280
 #elif defined(JUNCTION_ACCELERATION_FACTOR)
281
-  #error "JUNCTION_ACCELERATION_FACTOR is now JUNCTION_ACCELERATION. Please update your configuration."
281
+  #error "JUNCTION_ACCELERATION_FACTOR is obsolete. Delete it from Configuration_adv.h."
282
+#elif defined(JUNCTION_ACCELERATION)
283
+  #error "JUNCTION_ACCELERATION is obsolete. Delete it from Configuration_adv.h."
282
 #endif
284
 #endif
283
 
285
 
284
 #define BOARD_MKS_13     -47
286
 #define BOARD_MKS_13     -47

+ 16
- 5
Marlin/src/module/planner.cpp Näytä tiedosto

2166
       }
2166
       }
2167
       else {
2167
       else {
2168
         NOLESS(junction_cos_theta, -0.999999); // Check for numerical round-off to avoid divide by zero.
2168
         NOLESS(junction_cos_theta, -0.999999); // Check for numerical round-off to avoid divide by zero.
2169
-        const float sin_theta_d2 = SQRT(0.5 * (1.0 - junction_cos_theta)); // Trig half angle identity. Always positive.
2170
 
2169
 
2171
-        // TODO: Technically, the acceleration used in calculation needs to be limited by the minimum of the
2172
-        // two junctions. However, this shouldn't be a significant problem except in extreme circumstances.
2173
-        vmax_junction_sqr = (JUNCTION_ACCELERATION * JUNCTION_DEVIATION_MM * sin_theta_d2) / (1.0 - sin_theta_d2);
2170
+        float junction_unit_vec[JD_AXES] = {
2171
+          unit_vec[X_AXIS] - previous_unit_vec[X_AXIS],
2172
+          unit_vec[Y_AXIS] - previous_unit_vec[Y_AXIS],
2173
+          unit_vec[Z_AXIS] - previous_unit_vec[Z_AXIS]
2174
+          #if ENABLED(JUNCTION_DEVIATION_INCLUDE_E)
2175
+            , unit_vec[E_AXIS] - previous_unit_vec[E_AXIS]
2176
+          #endif
2177
+        };
2178
+        // Convert delta vector to unit vector
2179
+        normalize_junction_vector(junction_unit_vec);
2180
+
2181
+        const float junction_acceleration = limit_value_by_axis_maximum(block->acceleration, junction_unit_vec),
2182
+                    sin_theta_d2 = SQRT(0.5 * (1.0 - junction_cos_theta)); // Trig half angle identity. Always positive.
2183
+
2184
+        vmax_junction_sqr = (junction_acceleration * JUNCTION_DEVIATION_MM * sin_theta_d2) / (1.0 - sin_theta_d2);
2174
         if (block->millimeters < 1.0) {
2185
         if (block->millimeters < 1.0) {
2175
 
2186
 
2176
           // Fast acos approximation, minus the error bar to be safe
2187
           // Fast acos approximation, minus the error bar to be safe
2178
 
2189
 
2179
           // If angle is greater than 135 degrees (octagon), find speed for approximate arc
2190
           // If angle is greater than 135 degrees (octagon), find speed for approximate arc
2180
           if (junction_theta > RADIANS(135)) {
2191
           if (junction_theta > RADIANS(135)) {
2181
-            const float limit_sqr = block->millimeters / (RADIANS(180) - junction_theta) * JUNCTION_ACCELERATION;
2192
+            const float limit_sqr = block->millimeters / (RADIANS(180) - junction_theta) * junction_acceleration;
2182
             NOMORE(vmax_junction_sqr, limit_sqr);
2193
             NOMORE(vmax_junction_sqr, limit_sqr);
2183
           }
2194
           }
2184
         }
2195
         }

+ 23
- 0
Marlin/src/module/planner.h Näytä tiedosto

802
 
802
 
803
     static void recalculate();
803
     static void recalculate();
804
 
804
 
805
+    #if ENABLED(JUNCTION_DEVIATION)
806
+
807
+      #if ENABLED(JUNCTION_DEVIATION_INCLUDE_E)
808
+        #define JD_AXES XYZE
809
+      #else
810
+        #define JD_AXES XYZ
811
+      #endif
812
+
813
+      FORCE_INLINE static void normalize_junction_vector(float (&vector)[JD_AXES]) {
814
+        float magnitude_sq = 0.0;
815
+        for (uint8_t idx = 0; idx < JD_AXES; idx++) if (vector[idx]) magnitude_sq += sq(vector[idx]);
816
+        const float inv_magnitude = 1.0 / SQRT(magnitude_sq);
817
+        for (uint8_t idx = 0; idx < JD_AXES; idx++) vector[idx] *= inv_magnitude;
818
+      }
819
+
820
+      FORCE_INLINE static float limit_value_by_axis_maximum(const float &max_value, float (&unit_vec)[JD_AXES]) {
821
+        float limit_value = max_value;
822
+        for (uint8_t idx = 0; idx < JD_AXES; idx++) if (unit_vec[idx]) // Avoid divide by zero
823
+          NOMORE(limit_value, ABS(max_acceleration_mm_per_s2[idx] / unit_vec[idx]));
824
+        return limit_value;
825
+      }
826
+
827
+    #endif // JUNCTION_DEVIATION
805
 };
828
 };
806
 
829
 
807
 #define PLANNER_XY_FEEDRATE() (MIN(planner.max_feedrate_mm_s[X_AXIS], planner.max_feedrate_mm_s[Y_AXIS]))
830
 #define PLANNER_XY_FEEDRATE() (MIN(planner.max_feedrate_mm_s[X_AXIS], planner.max_feedrate_mm_s[Y_AXIS]))

Loading…
Peruuta
Tallenna