Browse Source

Fix planner leveling and rename arguments

Use lx, ly, lz for “logical” positions
Scott Lahteine 7 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
 
523
 
524
 #if PLANNER_LEVELING
524
 #if PLANNER_LEVELING
525
 
525
 
526
-  void Planner::apply_leveling(
526
+  void Planner::apply_leveling(float &lx, float &ly, float &lz) {
527
     #if ENABLED(MESH_BED_LEVELING)
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
     #endif
544
     #endif
532
-    , float &z
533
-  ) {
545
+  }
546
+
547
+  void Planner::unapply_leveling(float &lx, float &ly, float &lz) {
534
     #if ENABLED(MESH_BED_LEVELING)
548
     #if ENABLED(MESH_BED_LEVELING)
535
 
549
 
536
       if (mbl.active())
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
     #endif
565
     #endif
552
   }
566
   }
562
  *  fr_mm_s   - (target) speed of the move
576
  *  fr_mm_s   - (target) speed of the move
563
  *  extruder  - target extruder
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
   // Calculate the buffer head after we push this byte
580
   // Calculate the buffer head after we push this byte
575
   int next_buffer_head = next_block_index(block_buffer_head);
581
   int next_buffer_head = next_block_index(block_buffer_head);
576
 
582
 
578
   // Rest here until there is room in the buffer.
584
   // Rest here until there is room in the buffer.
579
   while (block_buffer_tail == next_buffer_head) idle();
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
   #endif
589
   #endif
584
 
590
 
585
   // The target position of the tool in absolute steps
591
   // The target position of the tool in absolute steps
586
   // Calculate target position in absolute steps
592
   // Calculate target position in absolute steps
587
   //this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
593
   //this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
588
   long target[NUM_AXIS] = {
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
     lround(e * axis_steps_per_mm[E_AXIS])
598
     lround(e * axis_steps_per_mm[E_AXIS])
593
   };
599
   };
594
 
600
 
598
 
604
 
599
   /*
605
   /*
600
   SERIAL_ECHO_START;
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
   SERIAL_ECHOPAIR(" (", dy);
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
   SERIAL_ECHOPAIR(" (", dz);
623
   SERIAL_ECHOPAIR(" (", dz);
607
   SERIAL_ECHOLNPGM(")");
624
   SERIAL_ECHOLNPGM(")");
608
   //*/
625
   //*/
671
   // For a mixing extruder, get a magnified step_event_count for each
688
   // For a mixing extruder, get a magnified step_event_count for each
672
   #if ENABLED(MIXING_EXTRUDER)
689
   #if ENABLED(MIXING_EXTRUDER)
673
     for (uint8_t i = 0; i < MIXING_STEPPERS; i++)
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
   #endif
692
   #endif
676
 
693
 
677
   #if FAN_COUNT > 0
694
   #if FAN_COUNT > 0
1124
       block->advance_rate = acc_dist ? advance / (float)acc_dist : 0;
1141
       block->advance_rate = acc_dist ? advance / (float)acc_dist : 0;
1125
     }
1142
     }
1126
     /**
1143
     /**
1127
-      SERIAL_ECHO_START;
1144
+     SERIAL_ECHO_START;
1128
      SERIAL_ECHOPGM("advance :");
1145
      SERIAL_ECHOPGM("advance :");
1129
      SERIAL_ECHO(block->advance/256.0);
1146
      SERIAL_ECHO(block->advance/256.0);
1130
      SERIAL_ECHOPGM("advance rate :");
1147
      SERIAL_ECHOPGM("advance rate :");
1152
  *
1169
  *
1153
  * On CORE machines stepper ABC will be translated from the given XYZ.
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
   #endif
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
        ne = position[E_AXIS] = lround(e * axis_steps_per_mm[E_AXIS]);
1181
        ne = position[E_AXIS] = lround(e * axis_steps_per_mm[E_AXIS]);
1172
   stepper.set_position(nx, ny, nz, ne);
1182
   stepper.set_position(nx, ny, nz, ne);
1173
   previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
1183
   previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.

+ 32
- 26
Marlin/planner.h View File

202
     static bool is_full() { return (block_buffer_tail == BLOCK_MOD(block_buffer_head + 1)); }
202
     static bool is_full() { return (block_buffer_tail == BLOCK_MOD(block_buffer_head + 1)); }
203
 
203
 
204
     #if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
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
      * Set the E position (mm) of the planner (and the E stepper)
246
      * Set the E position (mm) of the planner (and the E stepper)

Loading…
Cancel
Save