Browse Source

Fix planner leveling and rename arguments

Use lx, ly, lz for “logical” positions
Scott Lahteine 8 years ago
parent
commit
c109399bf6
2 changed files with 90 additions and 74 deletions
  1. 58
    48
      Marlin/planner.cpp
  2. 32
    26
      Marlin/planner.h

+ 58
- 48
Marlin/planner.cpp View File

@@ -523,30 +523,44 @@ void Planner::check_axes_activity() {
523 523
 
524 524
 #if PLANNER_LEVELING
525 525
 
526
-  void Planner::apply_leveling(
526
+  void Planner::apply_leveling(float &lx, float &ly, float &lz) {
527 527
     #if ENABLED(MESH_BED_LEVELING)
528
-      const float &x, const float &y
529
-    #else
530
-      float &x, float &y
528
+
529
+      if (mbl.active())
530
+        lz += mbl.get_z(RAW_X_POSITION(lx), RAW_Y_POSITION(ly));
531
+
532
+    #elif ENABLED(AUTO_BED_LEVELING_LINEAR)
533
+
534
+      float dx = RAW_X_POSITION(lx) - (X_TILT_FULCRUM),
535
+            dy = RAW_Y_POSITION(ly) - (Y_TILT_FULCRUM),
536
+            dz = RAW_Z_POSITION(lz);
537
+
538
+      apply_rotation_xyz(bed_level_matrix, dx, dy, dz);
539
+
540
+      lx = LOGICAL_X_POSITION(dx + X_TILT_FULCRUM);
541
+      ly = LOGICAL_Y_POSITION(dy + Y_TILT_FULCRUM);
542
+      lz = LOGICAL_Z_POSITION(dz);
543
+
531 544
     #endif
532
-    , float &z
533
-  ) {
545
+  }
546
+
547
+  void Planner::unapply_leveling(float &lx, float &ly, float &lz) {
534 548
     #if ENABLED(MESH_BED_LEVELING)
535 549
 
536 550
       if (mbl.active())
537
-        z += mbl.get_z(RAW_X_POSITION(x), RAW_Y_POSITION(y));
551
+        lz -= mbl.get_z(RAW_X_POSITION(lx), RAW_Y_POSITION(ly));
552
+
553
+    #elif ENABLED(AUTO_BED_LEVELING_LINEAR)
538 554
 
539
-    #elif ENABLED(AUTO_BED_LEVELING_FEATURE)
555
+      matrix_3x3 inverse = matrix_3x3::transpose(bed_level_matrix);
540 556
 
541
-      float tx = RAW_X_POSITION(x) - (X_TILT_FULCRUM),
542
-            ty = RAW_Y_POSITION(y) - (Y_TILT_FULCRUM),
543
-            tz = RAW_Z_POSITION(z);
557
+      float dx = lx - (X_TILT_FULCRUM), dy = ly - (Y_TILT_FULCRUM), dz = lz;
544 558
 
545
-      apply_rotation_xyz(bed_level_matrix, tx, ty, tz);
559
+      apply_rotation_xyz(inverse, dx, dy, dz);
546 560
 
547
-      x = LOGICAL_X_POSITION(tx + X_TILT_FULCRUM);
548
-      y = LOGICAL_Y_POSITION(ty + Y_TILT_FULCRUM);
549
-      z = LOGICAL_Z_POSITION(tz);
561
+      lx = LOGICAL_X_POSITION(dx + X_TILT_FULCRUM);
562
+      ly = LOGICAL_Y_POSITION(dy + Y_TILT_FULCRUM);
563
+      lz = LOGICAL_Z_POSITION(dz);
550 564
 
551 565
     #endif
552 566
   }
@@ -562,15 +576,7 @@ void Planner::check_axes_activity() {
562 576
  *  fr_mm_s   - (target) speed of the move
563 577
  *  extruder  - target extruder
564 578
  */
565
-
566
-void Planner::buffer_line(
567
-  #if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
568
-    float x, float y, float z
569
-  #else
570
-    const float& x, const float& y, const float& z
571
-  #endif
572
-  , const float& e, float fr_mm_s, const uint8_t extruder
573
-) {
579
+void Planner::buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, float fr_mm_s, const uint8_t extruder) {
574 580
   // Calculate the buffer head after we push this byte
575 581
   int next_buffer_head = next_block_index(block_buffer_head);
576 582
 
@@ -578,17 +584,17 @@ void Planner::buffer_line(
578 584
   // Rest here until there is room in the buffer.
579 585
   while (block_buffer_tail == next_buffer_head) idle();
580 586
 
581
-  #if ENABLED(MESH_BED_LEVELING) || ENABLED(AUTO_BED_LEVELING_FEATURE)
582
-    apply_leveling(x, y, z);
587
+  #if PLANNER_LEVELING
588
+    apply_leveling(lx, ly, lz);
583 589
   #endif
584 590
 
585 591
   // The target position of the tool in absolute steps
586 592
   // Calculate target position in absolute steps
587 593
   //this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
588 594
   long target[NUM_AXIS] = {
589
-    lround(x * axis_steps_per_mm[X_AXIS]),
590
-    lround(y * axis_steps_per_mm[Y_AXIS]),
591
-    lround(z * axis_steps_per_mm[Z_AXIS]),
595
+    lround(lx * axis_steps_per_mm[X_AXIS]),
596
+    lround(ly * axis_steps_per_mm[Y_AXIS]),
597
+    lround(lz * axis_steps_per_mm[Z_AXIS]),
592 598
     lround(e * axis_steps_per_mm[E_AXIS])
593 599
   };
594 600
 
@@ -598,11 +604,22 @@ void Planner::buffer_line(
598 604
 
599 605
   /*
600 606
   SERIAL_ECHO_START;
601
-  SERIAL_ECHOPAIR("Planner X:", x);
602
-  SERIAL_ECHOPAIR(" (", dx);
603
-  SERIAL_ECHOPAIR(") Y:", y);
607
+  SERIAL_ECHOPGM("Planner ", x);
608
+  #if IS_KINEMATIC
609
+    SERIAL_ECHOPAIR("A:", x);
610
+    SERIAL_ECHOPAIR(" (", dx);
611
+    SERIAL_ECHOPAIR(") B:", y);
612
+  #else
613
+    SERIAL_ECHOPAIR("X:", x);
614
+    SERIAL_ECHOPAIR(" (", dx);
615
+    SERIAL_ECHOPAIR(") Y:", y);
616
+  #endif
604 617
   SERIAL_ECHOPAIR(" (", dy);
605
-  SERIAL_ECHOPAIR(") Z:", z);
618
+  #elif ENABLED(DELTA)
619
+    SERIAL_ECHOPAIR(") C:", z);
620
+  #else
621
+    SERIAL_ECHOPAIR(") Z:", z);
622
+  #endif
606 623
   SERIAL_ECHOPAIR(" (", dz);
607 624
   SERIAL_ECHOLNPGM(")");
608 625
   //*/
@@ -671,7 +688,7 @@ void Planner::buffer_line(
671 688
   // For a mixing extruder, get a magnified step_event_count for each
672 689
   #if ENABLED(MIXING_EXTRUDER)
673 690
     for (uint8_t i = 0; i < MIXING_STEPPERS; i++)
674
-      block->mix_event_count[i] = (mixing_factor[i] < 0.0001) ? 0 : block->step_event_count / mixing_factor[i];
691
+      block->mix_event_count[i] = UNEAR_ZERO(mixing_factor[i]) ? 0 : block->step_event_count / mixing_factor[i];
675 692
   #endif
676 693
 
677 694
   #if FAN_COUNT > 0
@@ -1124,7 +1141,7 @@ void Planner::buffer_line(
1124 1141
       block->advance_rate = acc_dist ? advance / (float)acc_dist : 0;
1125 1142
     }
1126 1143
     /**
1127
-      SERIAL_ECHO_START;
1144
+     SERIAL_ECHO_START;
1128 1145
      SERIAL_ECHOPGM("advance :");
1129 1146
      SERIAL_ECHO(block->advance/256.0);
1130 1147
      SERIAL_ECHOPGM("advance rate :");
@@ -1152,22 +1169,15 @@ void Planner::buffer_line(
1152 1169
  *
1153 1170
  * On CORE machines stepper ABC will be translated from the given XYZ.
1154 1171
  */
1155
-void Planner::set_position_mm(
1156
-  #if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
1157
-    float x, float y, float z
1158
-  #else
1159
-    const float& x, const float& y, const float& z
1160
-  #endif
1161
-  , const float& e
1162
-) {
1172
+void Planner::set_position_mm(ARG_X, ARG_Y, ARG_Z, const float &e) {
1163 1173
 
1164
-  #if ENABLED(MESH_BED_LEVELING) || ENABLED(AUTO_BED_LEVELING_FEATURE)
1165
-    apply_leveling(x, y, z);
1174
+  #if PLANNER_LEVELING
1175
+    apply_leveling(lx, ly, lz);
1166 1176
   #endif
1167 1177
 
1168
-  long nx = position[X_AXIS] = lround(x * axis_steps_per_mm[X_AXIS]),
1169
-       ny = position[Y_AXIS] = lround(y * axis_steps_per_mm[Y_AXIS]),
1170
-       nz = position[Z_AXIS] = lround(z * axis_steps_per_mm[Z_AXIS]),
1178
+  long nx = position[X_AXIS] = lround(lx * axis_steps_per_mm[X_AXIS]),
1179
+       ny = position[Y_AXIS] = lround(ly * axis_steps_per_mm[Y_AXIS]),
1180
+       nz = position[Z_AXIS] = lround(lz * axis_steps_per_mm[Z_AXIS]),
1171 1181
        ne = position[E_AXIS] = lround(e * axis_steps_per_mm[E_AXIS]);
1172 1182
   stepper.set_position(nx, ny, nz, ne);
1173 1183
   previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.

+ 32
- 26
Marlin/planner.h View File

@@ -202,39 +202,45 @@ class Planner {
202 202
     static bool is_full() { return (block_buffer_tail == BLOCK_MOD(block_buffer_head + 1)); }
203 203
 
204 204
     #if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
205
+      #define ARG_X float lx
206
+      #define ARG_Y float ly
207
+      #define ARG_Z float lz
208
+    #else
209
+      #define ARG_X const float &lx
210
+      #define ARG_Y const float &ly
211
+      #define ARG_Z const float &lz
212
+    #endif
205 213
 
206
-      #if ENABLED(MESH_BED_LEVELING)
207
-        static void apply_leveling(const float &x, const float &y, float &z);
208
-      #else
209
-        static void apply_leveling(float &x, float &y, float &z);
210
-      #endif
214
+    #if PLANNER_LEVELING
211 215
 
212 216
       /**
213
-       * Add a new linear movement to the buffer.
214
-       *
215
-       *  x,y,z,e   - target position in mm
216
-       *  fr_mm_s   - (target) speed of the move (mm/s)
217
-       *  extruder  - target extruder
217
+       * Apply leveling to transform a cartesian position
218
+       * as it will be given to the planner and steppers.
218 219
        */
219
-      static void buffer_line(float x, float y, float z, const float& e, float fr_mm_s, const uint8_t extruder);
220
+      static void apply_leveling(float &lx, float &ly, float &lz);
221
+      static void unapply_leveling(float &lx, float &ly, float &lz);
220 222
 
221
-      /**
222
-       * Set the planner.position and individual stepper positions.
223
-       * Used by G92, G28, G29, and other procedures.
224
-       *
225
-       * Multiplies by axis_steps_per_mm[] and does necessary conversion
226
-       * for COREXY / COREXZ / COREYZ to set the corresponding stepper positions.
227
-       *
228
-       * Clears previous speed values.
229
-       */
230
-      static void set_position_mm(float x, float y, float z, const float& e);
231
-
232
-    #else
223
+    #endif
233 224
 
234
-      static void buffer_line(const float& x, const float& y, const float& z, const float& e, float fr_mm_s, const uint8_t extruder);
235
-      static void set_position_mm(const float& x, const float& y, const float& z, const float& e);
225
+    /**
226
+     * Add a new linear movement to the buffer.
227
+     *
228
+     *  x,y,z,e   - target position in mm
229
+     *  fr_mm_s   - (target) speed of the move (mm/s)
230
+     *  extruder  - target extruder
231
+     */
232
+    static void buffer_line(ARG_X, ARG_Y, ARG_Z, const float& e, float fr_mm_s, const uint8_t extruder);
236 233
 
237
-    #endif // AUTO_BED_LEVELING_FEATURE || MESH_BED_LEVELING
234
+    /**
235
+     * Set the planner.position and individual stepper positions.
236
+     * Used by G92, G28, G29, and other procedures.
237
+     *
238
+     * Multiplies by axis_steps_per_mm[] and does necessary conversion
239
+     * for COREXY / COREXZ / COREYZ to set the corresponding stepper positions.
240
+     *
241
+     * Clears previous speed values.
242
+     */
243
+    static void set_position_mm(ARG_X, ARG_Y, ARG_Z, const float& e);
238 244
 
239 245
     /**
240 246
      * Set the E position (mm) of the planner (and the E stepper)

Loading…
Cancel
Save