Browse Source

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 6 years ago
parent
commit
2deff0d9b9

+ 0
- 1
Marlin/Configuration_adv.h View File

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

+ 0
- 1
Marlin/src/config/default/Configuration_adv.h View File

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

+ 3
- 1
Marlin/src/inc/SanityCheck.h View File

@@ -278,7 +278,9 @@
278 278
 #elif defined(JUNCTION_DEVIATION_FACTOR)
279 279
   #error "JUNCTION_DEVIATION_FACTOR is now JUNCTION_DEVIATION_MM. Please update your configuration."
280 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 284
 #endif
283 285
 
284 286
 #define BOARD_MKS_13     -47

+ 16
- 5
Marlin/src/module/planner.cpp View File

@@ -2166,11 +2166,22 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
2166 2166
       }
2167 2167
       else {
2168 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 2185
         if (block->millimeters < 1.0) {
2175 2186
 
2176 2187
           // Fast acos approximation, minus the error bar to be safe
@@ -2178,7 +2189,7 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
2178 2189
 
2179 2190
           // If angle is greater than 135 degrees (octagon), find speed for approximate arc
2180 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 2193
             NOMORE(vmax_junction_sqr, limit_sqr);
2183 2194
           }
2184 2195
         }

+ 23
- 0
Marlin/src/module/planner.h View File

@@ -802,6 +802,29 @@ class Planner {
802 802
 
803 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 830
 #define PLANNER_XY_FEEDRATE() (MIN(planner.max_feedrate_mm_s[X_AXIS], planner.max_feedrate_mm_s[Y_AXIS]))

Loading…
Cancel
Save