Browse Source

Formatted multi-line comments

jbrazio 8 years ago
parent
commit
443e6d26fe
1 changed files with 235 additions and 140 deletions
  1. 235
    140
      Marlin/Marlin_main.cpp

+ 235
- 140
Marlin/Marlin_main.cpp View File

456
   #define KEEPALIVE_STATE(n) ;
456
   #define KEEPALIVE_STATE(n) ;
457
 #endif // HOST_KEEPALIVE_FEATURE
457
 #endif // HOST_KEEPALIVE_FEATURE
458
 
458
 
459
-//===========================================================================
460
-//================================ Functions ================================
461
-//===========================================================================
459
+/**
460
+ * ***************************************************************************
461
+ * ******************************** FUNCTIONS ********************************
462
+ * ***************************************************************************
463
+ */
462
 
464
 
463
 void process_next_command();
465
 void process_next_command();
464
 
466
 
877
     }
879
     }
878
   #endif
880
   #endif
879
 
881
 
880
-  //
881
-  // Loop while serial characters are incoming and the queue is not full
882
-  //
882
+  /**
883
+   * Loop while serial characters are incoming and the queue is not full
884
+   */
883
   while (commands_in_queue < BUFSIZE && MYSERIAL.available() > 0) {
885
   while (commands_in_queue < BUFSIZE && MYSERIAL.available() > 0) {
884
 
886
 
885
     char serial_char = MYSERIAL.read();
887
     char serial_char = MYSERIAL.read();
886
 
888
 
887
-    //
888
-    // If the character ends the line
889
-    //
889
+    /**
890
+     * If the character ends the line
891
+     */
890
     if (serial_char == '\n' || serial_char == '\r') {
892
     if (serial_char == '\n' || serial_char == '\r') {
891
 
893
 
892
       serial_comment_mode = false; // end of line == end of comment
894
       serial_comment_mode = false; // end of line == end of comment
994
 
996
 
995
     if (!card.sdprinting) return;
997
     if (!card.sdprinting) return;
996
 
998
 
997
-    // '#' stops reading from SD to the buffer prematurely, so procedural macro calls are possible
998
-    // if it occurs, stop_buffering is triggered and the buffer is run dry.
999
-    // this character _can_ occur in serial com, due to checksums. however, no checksums are used in SD printing
999
+    /**
1000
+     * '#' stops reading from SD to the buffer prematurely, so procedural
1001
+     * macro calls are possible. If it occurs, stop_buffering is triggered
1002
+     * and the buffer is run dry; this character _can_ occur in serial com
1003
+     * due to checksums, however, no checksums are used in SD printing.
1004
+     */
1000
 
1005
 
1001
     if (commands_in_queue == 0) stop_buffering = false;
1006
     if (commands_in_queue == 0) stop_buffering = false;
1002
 
1007
 
1035
         _commit_command(false);
1040
         _commit_command(false);
1036
       }
1041
       }
1037
       else if (sd_count >= MAX_CMD_SIZE - 1) {
1042
       else if (sd_count >= MAX_CMD_SIZE - 1) {
1038
-        // Keep fetching, but ignore normal characters beyond the max length
1039
-        // The command will be injected when EOL is reached
1043
+        /**
1044
+         * Keep fetching, but ignore normal characters beyond the max length
1045
+         * The command will be injected when EOL is reached
1046
+         */
1040
       }
1047
       }
1041
       else {
1048
       else {
1042
         if (sd_char == ';') sd_comment_mode = true;
1049
         if (sd_char == ';') sd_comment_mode = true;
1110
     if (extruder == 0)
1117
     if (extruder == 0)
1111
       return base_home_pos(X_AXIS) + home_offset[X_AXIS];
1118
       return base_home_pos(X_AXIS) + home_offset[X_AXIS];
1112
     else
1119
     else
1113
-      // In dual carriage mode the extruder offset provides an override of the
1114
-      // second X-carriage offset when homed - otherwise X2_HOME_POS is used.
1115
-      // This allow soft recalibration of the second extruder offset position without firmware reflash
1116
-      // (through the M218 command).
1120
+      /**
1121
+       * In dual carriage mode the extruder offset provides an override of the
1122
+       * second X-carriage offset when homed - otherwise X2_HOME_POS is used.
1123
+       * This allow soft recalibration of the second extruder offset position
1124
+       * without firmware reflash (through the M218 command).
1125
+       */
1117
       return (extruder_offset[X_AXIS][1] > 0) ? extruder_offset[X_AXIS][1] : X2_HOME_POS;
1126
       return (extruder_offset[X_AXIS][1] > 0) ? extruder_offset[X_AXIS][1] : X2_HOME_POS;
1118
   }
1127
   }
1119
 
1128
 
1173
 
1182
 
1174
       // SERIAL_ECHOPGM("homeposition[x]= "); SERIAL_ECHO(homeposition[0]);
1183
       // SERIAL_ECHOPGM("homeposition[x]= "); SERIAL_ECHO(homeposition[0]);
1175
       // SERIAL_ECHOPGM("homeposition[y]= "); SERIAL_ECHOLN(homeposition[1]);
1184
       // SERIAL_ECHOPGM("homeposition[y]= "); SERIAL_ECHOLN(homeposition[1]);
1176
-      // Works out real Homeposition angles using inverse kinematics,
1177
-      // and calculates homing offset using forward kinematics
1185
+
1186
+      /**
1187
+       * Works out real Homeposition angles using inverse kinematics,
1188
+       * and calculates homing offset using forward kinematics
1189
+       */
1178
       calculate_delta(homeposition);
1190
       calculate_delta(homeposition);
1179
 
1191
 
1180
       // SERIAL_ECHOPGM("base Theta= "); SERIAL_ECHO(delta[X_AXIS]);
1192
       // SERIAL_ECHOPGM("base Theta= "); SERIAL_ECHO(delta[X_AXIS]);
1194
 
1206
 
1195
       current_position[axis] = delta[axis];
1207
       current_position[axis] = delta[axis];
1196
 
1208
 
1197
-      // SCARA home positions are based on configuration since the actual limits are determined by the
1198
-      // inverse kinematic transform.
1209
+      /**
1210
+       * SCARA home positions are based on configuration since the actual
1211
+       * limits are determined by the inverse kinematic transform.
1212
+       */
1199
       min_pos[axis] = base_min_pos(axis); // + (delta[axis] - base_home_pos(axis));
1213
       min_pos[axis] = base_min_pos(axis); // + (delta[axis] - base_home_pos(axis));
1200
       max_pos[axis] = base_max_pos(axis); // + (delta[axis] - base_home_pos(axis));
1214
       max_pos[axis] = base_max_pos(axis); // + (delta[axis] - base_home_pos(axis));
1201
     }
1215
     }
1357
 
1371
 
1358
   static void run_z_probe() {
1372
   static void run_z_probe() {
1359
 
1373
 
1360
-    refresh_cmd_timeout(); // to prevent stepper_inactive_time from running out and EXTRUDER_RUNOUT_PREVENT from extruding
1374
+    /**
1375
+     * To prevent stepper_inactive_time from running out and
1376
+     * EXTRUDER_RUNOUT_PREVENT from extruding
1377
+     */
1378
+    refresh_cmd_timeout();
1361
 
1379
 
1362
     #if ENABLED(DELTA)
1380
     #if ENABLED(DELTA)
1363
 
1381
 
1377
       st_synchronize();
1395
       st_synchronize();
1378
       endstops_hit_on_purpose(); // clear endstop hit flags
1396
       endstops_hit_on_purpose(); // clear endstop hit flags
1379
 
1397
 
1380
-      // we have to let the planner know where we are right now as it is not where we said to go.
1398
+      /**
1399
+       * We have to let the planner know where we are right now as it
1400
+       * is not where we said to go.
1401
+       */
1381
       long stop_steps = st_get_position(Z_AXIS);
1402
       long stop_steps = st_get_position(Z_AXIS);
1382
       float mm = start_z - float(start_steps - stop_steps) / axis_steps_per_unit[Z_AXIS];
1403
       float mm = start_z - float(start_steps - stop_steps) / axis_steps_per_unit[Z_AXIS];
1383
       current_position[Z_AXIS] = mm;
1404
       current_position[Z_AXIS] = mm;
1402
 
1423
 
1403
       // Tell the planner where we ended up - Get this from the stepper handler
1424
       // Tell the planner where we ended up - Get this from the stepper handler
1404
       zPosition = st_get_axis_position_mm(Z_AXIS);
1425
       zPosition = st_get_axis_position_mm(Z_AXIS);
1405
-      plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS]);
1426
+      plan_set_position(
1427
+        current_position[X_AXIS], current_position[Y_AXIS], zPosition,
1428
+        current_position[E_AXIS]
1429
+      );
1406
 
1430
 
1407
       // move up the retract distance
1431
       // move up the retract distance
1408
       zPosition += home_bump_mm(Z_AXIS);
1432
       zPosition += home_bump_mm(Z_AXIS);
1474
     feedrate = oldFeedRate;
1498
     feedrate = oldFeedRate;
1475
   }
1499
   }
1476
 
1500
 
1477
-  inline void do_blocking_move_to_xy(float x, float y) { do_blocking_move_to(x, y, current_position[Z_AXIS]); }
1478
-  inline void do_blocking_move_to_x(float x) { do_blocking_move_to(x, current_position[Y_AXIS], current_position[Z_AXIS]); }
1479
-  inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], z); }
1480
-  inline void raise_z_after_probing() { do_blocking_move_to_z(current_position[Z_AXIS] + Z_RAISE_AFTER_PROBING); }
1501
+  inline void do_blocking_move_to_xy(float x, float y) {
1502
+    do_blocking_move_to(x, y, current_position[Z_AXIS]);
1503
+  }
1504
+
1505
+  inline void do_blocking_move_to_x(float x) {
1506
+    do_blocking_move_to(x, current_position[Y_AXIS], current_position[Z_AXIS]);
1507
+  }
1508
+
1509
+  inline void do_blocking_move_to_z(float z) {
1510
+    do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], z);
1511
+  }
1512
+
1513
+  inline void raise_z_after_probing() {
1514
+    do_blocking_move_to_z(current_position[Z_AXIS] + Z_RAISE_AFTER_PROBING);
1515
+  }
1481
 
1516
 
1482
   static void clean_up_after_endstop_move() {
1517
   static void clean_up_after_endstop_move() {
1483
     #if ENABLED(DEBUG_LEVELING_FEATURE)
1518
     #if ENABLED(DEBUG_LEVELING_FEATURE)
1729
       }
1764
       }
1730
     #endif
1765
     #endif
1731
 
1766
 
1732
-    do_blocking_move_to_xy(x - (X_PROBE_OFFSET_FROM_EXTRUDER), y - (Y_PROBE_OFFSET_FROM_EXTRUDER)); // this also updates current_position
1767
+    // this also updates current_position
1768
+    do_blocking_move_to_xy(x - (X_PROBE_OFFSET_FROM_EXTRUDER), y - (Y_PROBE_OFFSET_FROM_EXTRUDER));
1733
 
1769
 
1734
     #if DISABLED(Z_PROBE_SLED) && DISABLED(Z_PROBE_ALLEN_KEY)
1770
     #if DISABLED(Z_PROBE_SLED) && DISABLED(Z_PROBE_ALLEN_KEY)
1735
       if (probe_action & ProbeDeploy) {
1771
       if (probe_action & ProbeDeploy) {
1780
     /**
1816
     /**
1781
      * All DELTA leveling in the Marlin uses NONLINEAR_BED_LEVELING
1817
      * All DELTA leveling in the Marlin uses NONLINEAR_BED_LEVELING
1782
      */
1818
      */
1783
-
1784
     static void extrapolate_one_point(int x, int y, int xdir, int ydir) {
1819
     static void extrapolate_one_point(int x, int y, int xdir, int ydir) {
1785
       if (bed_level[x][y] != 0.0) {
1820
       if (bed_level[x][y] != 0.0) {
1786
         return;  // Don't overwrite good values.
1821
         return;  // Don't overwrite good values.
1800
       bed_level[x][y] = median;
1835
       bed_level[x][y] = median;
1801
     }
1836
     }
1802
 
1837
 
1803
-    // Fill in the unprobed points (corners of circular print surface)
1804
-    // using linear extrapolation, away from the center.
1838
+    /**
1839
+     * Fill in the unprobed points (corners of circular print surface)
1840
+     * using linear extrapolation, away from the center.
1841
+     */
1805
     static void extrapolate_unprobed_bed_level() {
1842
     static void extrapolate_unprobed_bed_level() {
1806
       int half = (AUTO_BED_LEVELING_GRID_POINTS - 1) / 2;
1843
       int half = (AUTO_BED_LEVELING_GRID_POINTS - 1) / 2;
1807
       for (int y = 0; y <= half; y++) {
1844
       for (int y = 0; y <= half; y++) {
1815
       }
1852
       }
1816
     }
1853
     }
1817
 
1854
 
1818
-    // Print calibration results for plotting or manual frame adjustment.
1855
+    /**
1856
+     * Print calibration results for plotting or manual frame adjustment.
1857
+     */
1819
     static void print_bed_level() {
1858
     static void print_bed_level() {
1820
       for (int y = 0; y < AUTO_BED_LEVELING_GRID_POINTS; y++) {
1859
       for (int y = 0; y < AUTO_BED_LEVELING_GRID_POINTS; y++) {
1821
         for (int x = 0; x < AUTO_BED_LEVELING_GRID_POINTS; x++) {
1860
         for (int x = 0; x < AUTO_BED_LEVELING_GRID_POINTS; x++) {
1826
       }
1865
       }
1827
     }
1866
     }
1828
 
1867
 
1829
-    // Reset calibration results to zero.
1868
+    /**
1869
+     * Reset calibration results to zero.
1870
+     */
1830
     void reset_bed_level() {
1871
     void reset_bed_level() {
1831
       #if ENABLED(DEBUG_LEVELING_FEATURE)
1872
       #if ENABLED(DEBUG_LEVELING_FEATURE)
1832
         if (marlin_debug_flags & DEBUG_LEVELING) {
1873
         if (marlin_debug_flags & DEBUG_LEVELING) {
1846
 
1887
 
1847
     void raise_z_for_servo() {
1888
     void raise_z_for_servo() {
1848
       float zpos = current_position[Z_AXIS], z_dest = Z_RAISE_BEFORE_PROBING;
1889
       float zpos = current_position[Z_AXIS], z_dest = Z_RAISE_BEFORE_PROBING;
1849
-      // The zprobe_zoffset is negative any switch below the nozzle, so
1850
-      // multiply by Z_HOME_DIR (-1) to move enough away from bed for the probe
1890
+      /**
1891
+       * The zprobe_zoffset is negative any switch below the nozzle, so
1892
+       * multiply by Z_HOME_DIR (-1) to move enough away from bed for the probe
1893
+       */
1851
       z_dest += axis_homed[Z_AXIS] ? zprobe_zoffset * Z_HOME_DIR : zpos;
1894
       z_dest += axis_homed[Z_AXIS] ? zprobe_zoffset * Z_HOME_DIR : zpos;
1852
       if (zpos < z_dest) do_blocking_move_to_z(z_dest); // also updates current_position
1895
       if (zpos < z_dest) do_blocking_move_to_z(z_dest); // also updates current_position
1853
     }
1896
     }
1894
       #if Z_RAISE_AFTER_PROBING > 0
1937
       #if Z_RAISE_AFTER_PROBING > 0
1895
         raise_z_after_probing(); // raise Z
1938
         raise_z_after_probing(); // raise Z
1896
       #endif
1939
       #endif
1897
-      do_blocking_move_to_x(X_MAX_POS + SLED_DOCKING_OFFSET + offset - 1);  // Dock sled a bit closer to ensure proper capturing
1940
+      // Dock sled a bit closer to ensure proper capturing
1941
+      do_blocking_move_to_x(X_MAX_POS + SLED_DOCKING_OFFSET + offset - 1);
1898
       digitalWrite(SLED_PIN, LOW); // turn off magnet
1942
       digitalWrite(SLED_PIN, LOW); // turn off magnet
1899
     }
1943
     }
1900
     else {
1944
     else {
2190
 #endif // FWRETRACT
2234
 #endif // FWRETRACT
2191
 
2235
 
2192
 /**
2236
 /**
2193
- *
2194
- * G-Code Handler functions
2195
- *
2237
+ * ***************************************************************************
2238
+ * ***************************** G-CODE HANDLING *****************************
2239
+ * ***************************************************************************
2196
  */
2240
  */
2197
 
2241
 
2198
 /**
2242
 /**
2383
     #endif
2427
     #endif
2384
   #endif
2428
   #endif
2385
 
2429
 
2386
-  // For mesh bed leveling deactivate the mesh calculations, will be turned on again when homing all axis
2430
+  /**
2431
+   * For mesh bed leveling deactivate the mesh calculations, will be turned
2432
+   * on again when homing all axis
2433
+   */
2387
   #if ENABLED(MESH_BED_LEVELING)
2434
   #if ENABLED(MESH_BED_LEVELING)
2388
     uint8_t mbl_was_active = mbl.active;
2435
     uint8_t mbl_was_active = mbl.active;
2389
     mbl.active = 0;
2436
     mbl.active = 0;
2391
 
2438
 
2392
   setup_for_endstop_move();
2439
   setup_for_endstop_move();
2393
 
2440
 
2394
-  set_destination_to_current(); // Directly after a reset this is all 0. Later we get a hint if we have to raise z or not.
2441
+  /**
2442
+   * Directly after a reset this is all 0. Later we get a hint if we have
2443
+   * to raise z or not.
2444
+   */
2445
+  set_destination_to_current();
2395
 
2446
 
2396
   feedrate = 0.0;
2447
   feedrate = 0.0;
2397
 
2448
 
2398
   #if ENABLED(DELTA)
2449
   #if ENABLED(DELTA)
2399
-    // A delta can only safely home all axis at the same time
2400
-    // all axis have to home at the same time
2450
+    /**
2451
+     * A delta can only safely home all axis at the same time
2452
+     * all axis have to home at the same time
2453
+     */
2401
 
2454
 
2402
     // Pretend the current position is 0,0,0
2455
     // Pretend the current position is 0,0,0
2403
     for (int i = X_AXIS; i <= Z_AXIS; i++) current_position[i] = 0;
2456
     for (int i = X_AXIS; i <= Z_AXIS; i++) current_position[i] = 0;
2462
         line_to_destination();
2515
         line_to_destination();
2463
         st_synchronize();
2516
         st_synchronize();
2464
 
2517
 
2465
-        // Update the current Z position even if it currently not real from Z-home
2466
-        // otherwise each call to line_to_destination() will want to move Z-axis
2467
-        // by MIN_Z_HEIGHT_FOR_HOMING.
2518
+        /**
2519
+         * Update the current Z position even if it currently not real from
2520
+         * Z-home otherwise each call to line_to_destination() will want to
2521
+         * move Z-axis by MIN_Z_HEIGHT_FOR_HOMING.
2522
+         */
2468
         current_position[Z_AXIS] = destination[Z_AXIS];
2523
         current_position[Z_AXIS] = destination[Z_AXIS];
2469
       }
2524
       }
2470
     #endif
2525
     #endif
2581
 
2636
 
2582
           if (home_all_axis) {
2637
           if (home_all_axis) {
2583
 
2638
 
2584
-            // At this point we already have Z at MIN_Z_HEIGHT_FOR_HOMING height
2585
-            // No need to move Z any more as this height should already be safe
2586
-            // enough to reach Z_SAFE_HOMING XY positions.
2587
-            // Just make sure the planner is in sync.
2639
+            /**
2640
+             * At this point we already have Z at MIN_Z_HEIGHT_FOR_HOMING height
2641
+             * No need to move Z any more as this height should already be safe
2642
+             * enough to reach Z_SAFE_HOMING XY positions.
2643
+             * Just make sure the planner is in sync.
2644
+             */
2588
             sync_plan_position();
2645
             sync_plan_position();
2589
 
2646
 
2590
-            //
2591
-            // Set the Z probe (or just the nozzle) destination to the safe homing point
2592
-            //
2647
+            /**
2648
+             * Set the Z probe (or just the nozzle) destination to the safe
2649
+             *  homing point
2650
+             */
2593
             destination[X_AXIS] = round(Z_SAFE_HOMING_X_POINT - (X_PROBE_OFFSET_FROM_EXTRUDER));
2651
             destination[X_AXIS] = round(Z_SAFE_HOMING_X_POINT - (X_PROBE_OFFSET_FROM_EXTRUDER));
2594
             destination[Y_AXIS] = round(Z_SAFE_HOMING_Y_POINT - (Y_PROBE_OFFSET_FROM_EXTRUDER));
2652
             destination[Y_AXIS] = round(Z_SAFE_HOMING_Y_POINT - (Y_PROBE_OFFSET_FROM_EXTRUDER));
2595
             destination[Z_AXIS] = current_position[Z_AXIS]; //z is already at the right height
2653
             destination[Z_AXIS] = current_position[Z_AXIS]; //z is already at the right height
2606
             line_to_destination();
2664
             line_to_destination();
2607
             st_synchronize();
2665
             st_synchronize();
2608
 
2666
 
2609
-            // Update the current positions for XY, Z is still at least at
2610
-            // MIN_Z_HEIGHT_FOR_HOMING height, no changes there.
2667
+            /**
2668
+             * Update the current positions for XY, Z is still at least at
2669
+             * MIN_Z_HEIGHT_FOR_HOMING height, no changes there.
2670
+             */
2611
             current_position[X_AXIS] = destination[X_AXIS];
2671
             current_position[X_AXIS] = destination[X_AXIS];
2612
             current_position[Y_AXIS] = destination[Y_AXIS];
2672
             current_position[Y_AXIS] = destination[Y_AXIS];
2613
 
2673
 
2620
             // Let's see if X and Y are homed
2680
             // Let's see if X and Y are homed
2621
             if (axis_homed[X_AXIS] && axis_homed[Y_AXIS]) {
2681
             if (axis_homed[X_AXIS] && axis_homed[Y_AXIS]) {
2622
 
2682
 
2623
-              // Make sure the Z probe is within the physical limits
2624
-              // NOTE: This doesn't necessarily ensure the Z probe is also within the bed!
2683
+              /**
2684
+               * Make sure the Z probe is within the physical limits
2685
+               * NOTE: This doesn't necessarily ensure the Z probe is also
2686
+               * within the bed!
2687
+               */
2625
               float cpx = current_position[X_AXIS], cpy = current_position[Y_AXIS];
2688
               float cpx = current_position[X_AXIS], cpy = current_position[Y_AXIS];
2626
               if (   cpx >= X_MIN_POS - (X_PROBE_OFFSET_FROM_EXTRUDER)
2689
               if (   cpx >= X_MIN_POS - (X_PROBE_OFFSET_FROM_EXTRUDER)
2627
                   && cpx <= X_MAX_POS - (X_PROBE_OFFSET_FROM_EXTRUDER)
2690
                   && cpx <= X_MAX_POS - (X_PROBE_OFFSET_FROM_EXTRUDER)
2858
       case MeshSetZOffset:
2921
       case MeshSetZOffset:
2859
         if (code_seen('Z')) {
2922
         if (code_seen('Z')) {
2860
           z = code_value();
2923
           z = code_value();
2861
-        } 
2924
+        }
2862
         else {
2925
         else {
2863
           SERIAL_PROTOCOLPGM("Z not entered.\n");
2926
           SERIAL_PROTOCOLPGM("Z not entered.\n");
2864
           return;
2927
           return;
3038
         float z_offset = zprobe_zoffset;
3101
         float z_offset = zprobe_zoffset;
3039
         if (code_seen(axis_codes[Z_AXIS])) z_offset += code_value();
3102
         if (code_seen(axis_codes[Z_AXIS])) z_offset += code_value();
3040
       #else // !DELTA
3103
       #else // !DELTA
3041
-        // solve the plane equation ax + by + d = z
3042
-        // A is the matrix with rows [x y 1] for all the probed points
3043
-        // B is the vector of the Z positions
3044
-        // the normal vector to the plane is formed by the coefficients of the plane equation in the standard form, which is Vx*x+Vy*y+Vz*z+d = 0
3045
-        // so Vx = -a Vy = -b Vz = 1 (we want the vector facing towards positive Z
3104
+        /**
3105
+         * solve the plane equation ax + by + d = z
3106
+         * A is the matrix with rows [x y 1] for all the probed points
3107
+         * B is the vector of the Z positions
3108
+         * the normal vector to the plane is formed by the coefficients of the
3109
+         * plane equation in the standard form, which is Vx*x+Vy*y+Vz*z+d = 0
3110
+         * so Vx = -a Vy = -b Vz = 1 (we want the vector facing towards positive Z
3111
+         */
3046
 
3112
 
3047
         int abl2 = auto_bed_leveling_grid_points * auto_bed_leveling_grid_points;
3113
         int abl2 = auto_bed_leveling_grid_points * auto_bed_leveling_grid_points;
3048
 
3114
 
3273
         plan_bed_level_matrix.debug(" \n\nBed Level Correction Matrix:");
3339
         plan_bed_level_matrix.debug(" \n\nBed Level Correction Matrix:");
3274
 
3340
 
3275
       if (!dryrun) {
3341
       if (!dryrun) {
3276
-        // Correct the Z height difference from Z probe position and nozzle tip position.
3277
-        // The Z height on homing is measured by Z probe, but the Z probe is quite far from the nozzle.
3278
-        // When the bed is uneven, this height must be corrected.
3342
+        /**
3343
+         * Correct the Z height difference from Z probe position and nozzle tip position.
3344
+         * The Z height on homing is measured by Z probe, but the Z probe is quite far
3345
+         * from the nozzle. When the bed is uneven, this height must be corrected.
3346
+         */
3279
         float x_tmp = current_position[X_AXIS] + X_PROBE_OFFSET_FROM_EXTRUDER,
3347
         float x_tmp = current_position[X_AXIS] + X_PROBE_OFFSET_FROM_EXTRUDER,
3280
               y_tmp = current_position[Y_AXIS] + Y_PROBE_OFFSET_FROM_EXTRUDER,
3348
               y_tmp = current_position[Y_AXIS] + Y_PROBE_OFFSET_FROM_EXTRUDER,
3281
               z_tmp = current_position[Z_AXIS],
3349
               z_tmp = current_position[Z_AXIS],
3290
           }
3358
           }
3291
         #endif
3359
         #endif
3292
 
3360
 
3293
-        apply_rotation_xyz(plan_bed_level_matrix, x_tmp, y_tmp, z_tmp); // Apply the correction sending the Z probe offset
3294
-
3295
-        // Get the current Z position and send it to the planner.
3296
-        //
3297
-        // >> (z_tmp - real_z) : The rotated current Z minus the uncorrected Z (most recent plan_set_position/sync_plan_position)
3298
-        //
3299
-        // >> zprobe_zoffset : Z distance from nozzle to Z probe (set by default, M851, EEPROM, or Menu)
3300
-        //
3301
-        // >> Z_RAISE_AFTER_PROBING : The distance the Z probe will have lifted after the last probe
3302
-        //
3303
-        // >> Should home_offset[Z_AXIS] be included?
3304
-        //
3305
-        //      Discussion: home_offset[Z_AXIS] was applied in G28 to set the starting Z.
3306
-        //      If Z is not tweaked in G29 -and- the Z probe in G29 is not actually "homing" Z...
3307
-        //      then perhaps it should not be included here. The purpose of home_offset[] is to
3308
-        //      adjust for inaccurate endstops, not for reasonably accurate probes. If it were
3309
-        //      added here, it could be seen as a compensating factor for the Z probe.
3310
-        //
3361
+        // Apply the correction sending the Z probe offset
3362
+        apply_rotation_xyz(plan_bed_level_matrix, x_tmp, y_tmp, z_tmp);
3363
+
3364
+        /*
3365
+         * Get the current Z position and send it to the planner.
3366
+         *
3367
+         * >> (z_tmp - real_z) : The rotated current Z minus the uncorrected Z
3368
+         * (most recent plan_set_position/sync_plan_position)
3369
+         *
3370
+         * >> zprobe_zoffset : Z distance from nozzle to Z probe
3371
+         * (set by default, M851, EEPROM, or Menu)
3372
+         *
3373
+         * >> Z_RAISE_AFTER_PROBING : The distance the Z probe will have lifted
3374
+         * after the last probe
3375
+         *
3376
+         * >> Should home_offset[Z_AXIS] be included?
3377
+         *
3378
+         *
3379
+         *   Discussion: home_offset[Z_AXIS] was applied in G28 to set the
3380
+         *   starting Z. If Z is not tweaked in G29 -and- the Z probe in G29 is
3381
+         *   not actually "homing" Z... then perhaps it should not be included
3382
+         *   here. The purpose of home_offset[] is to adjust for inaccurate
3383
+         *   endstops, not for reasonably accurate probes. If it were added
3384
+         *   here, it could be seen as a compensating factor for the Z probe.
3385
+         */
3311
         #if ENABLED(DEBUG_LEVELING_FEATURE)
3386
         #if ENABLED(DEBUG_LEVELING_FEATURE)
3312
           if (marlin_debug_flags & DEBUG_LEVELING) {
3387
           if (marlin_debug_flags & DEBUG_LEVELING) {
3313
             SERIAL_ECHOPAIR("> AFTER apply_rotation_xyz > z_tmp  = ", z_tmp);
3388
             SERIAL_ECHOPAIR("> AFTER apply_rotation_xyz > z_tmp  = ", z_tmp);
3697
 
3772
 
3698
 #if ENABLED(AUTO_BED_LEVELING_FEATURE) && ENABLED(Z_MIN_PROBE_REPEATABILITY_TEST)
3773
 #if ENABLED(AUTO_BED_LEVELING_FEATURE) && ENABLED(Z_MIN_PROBE_REPEATABILITY_TEST)
3699
 
3774
 
3700
-  // This is redundant since the SanityCheck.h already checks for a valid Z_MIN_PROBE_PIN, but here for clarity.
3775
+  /**
3776
+   * This is redundant since the SanityCheck.h already checks for a valid
3777
+   *  Z_MIN_PROBE_PIN, but here for clarity.
3778
+   */
3701
   #if ENABLED(Z_MIN_PROBE_ENDSTOP)
3779
   #if ENABLED(Z_MIN_PROBE_ENDSTOP)
3702
     #if !HAS_Z_PROBE
3780
     #if !HAS_Z_PROBE
3703
       #error You must define Z_MIN_PROBE_PIN to enable Z probe repeatability calculation.
3781
       #error You must define Z_MIN_PROBE_PIN to enable Z probe repeatability calculation.
3804
       if (!seen_L) n_legs = 7;
3882
       if (!seen_L) n_legs = 7;
3805
     }
3883
     }
3806
 
3884
 
3807
-    // Now get everything to the specified probe point So we can safely do a probe to
3808
-    // get us close to the bed.  If the Z-Axis is far from the bed, we don't want to
3809
-    // use that as a starting point for each probe.
3810
-    //
3885
+    /**
3886
+     * Now get everything to the specified probe point So we can safely do a
3887
+     * probe to get us close to the bed.  If the Z-Axis is far from the bed,
3888
+     * we don't want to use that as a starting point for each probe.
3889
+     */
3811
     if (verbose_level > 2)
3890
     if (verbose_level > 2)
3812
       SERIAL_PROTOCOLPGM("Positioning the probe...\n");
3891
       SERIAL_PROTOCOLPGM("Positioning the probe...\n");
3813
 
3892
 
3814
     #if ENABLED(DELTA)
3893
     #if ENABLED(DELTA)
3815
-      reset_bed_level();    // we don't do bed level correction in M48 because we want the raw data when we probe
3894
+      // we don't do bed level correction in M48 because we want the raw data when we probe
3895
+      reset_bed_level();
3816
     #else
3896
     #else
3817
-      plan_bed_level_matrix.set_to_identity();  // we don't do bed level correction in M48 because we wantthe raw data when we probe
3897
+      // we don't do bed level correction in M48 because we want the raw data when we probe
3898
+      plan_bed_level_matrix.set_to_identity();
3818
     #endif
3899
     #endif
3819
 
3900
 
3820
     if (Z_start_location < Z_RAISE_BEFORE_PROBING * 2.0)
3901
     if (Z_start_location < Z_RAISE_BEFORE_PROBING * 2.0)
3822
 
3903
 
3823
     do_blocking_move_to_xy(X_probe_location - X_PROBE_OFFSET_FROM_EXTRUDER, Y_probe_location - Y_PROBE_OFFSET_FROM_EXTRUDER);
3904
     do_blocking_move_to_xy(X_probe_location - X_PROBE_OFFSET_FROM_EXTRUDER, Y_probe_location - Y_PROBE_OFFSET_FROM_EXTRUDER);
3824
 
3905
 
3825
-    //
3826
-    // OK, do the initial probe to get us close to the bed.
3827
-    // Then retrace the right amount and use that in subsequent probes
3828
-    //
3906
+    /**
3907
+     * OK, do the initial probe to get us close to the bed.
3908
+     * Then retrace the right amount and use that in subsequent probes
3909
+     */
3829
     setup_for_endstop_move();
3910
     setup_for_endstop_move();
3830
 
3911
 
3831
     probe_pt(X_probe_location, Y_probe_location, Z_RAISE_BEFORE_PROBING,
3912
     probe_pt(X_probe_location, Y_probe_location, Z_RAISE_BEFORE_PROBING,
3862
 
3943
 
3863
         for (uint8_t l = 0; l < n_legs - 1; l++) {
3944
         for (uint8_t l = 0; l < n_legs - 1; l++) {
3864
           double delta_angle;
3945
           double delta_angle;
3946
+
3865
           if (schizoid_flag)
3947
           if (schizoid_flag)
3866
-            delta_angle = dir * 2.0 * 72.0;   // The points of a 5 point star are 72 degrees apart.  We need to
3867
-          // skip a point and go to the next one on the star.
3948
+            // The points of a 5 point star are 72 degrees apart.  We need to
3949
+            // skip a point and go to the next one on the star.
3950
+            delta_angle = dir * 2.0 * 72.0;
3951
+
3868
           else
3952
           else
3869
-            delta_angle = dir * (float) random(25, 45);   // If we do this line, we are just trying to move further
3870
-          // around the circle.
3953
+            // If we do this line, we are just trying to move further
3954
+            // around the circle.
3955
+            delta_angle = dir * (float) random(25, 45);
3956
+
3871
           angle += delta_angle;
3957
           angle += delta_angle;
3958
+
3872
           while (angle > 360.0)   // We probably do not need to keep the angle between 0 and 2*PI, but the
3959
           while (angle > 360.0)   // We probably do not need to keep the angle between 0 and 2*PI, but the
3873
             angle -= 360.0;       // Arduino documentation says the trig functions should not be given values
3960
             angle -= 360.0;       // Arduino documentation says the trig functions should not be given values
3874
           while (angle < 0.0)     // outside of this range.   It looks like they behave correctly with
3961
           while (angle < 0.0)     // outside of this range.   It looks like they behave correctly with
3875
             angle += 360.0;       // numbers outside of the range, but just to be safe we clamp them.
3962
             angle += 360.0;       // numbers outside of the range, but just to be safe we clamp them.
3963
+
3876
           X_current = X_probe_location - X_PROBE_OFFSET_FROM_EXTRUDER + cos(RADIANS(angle)) * radius;
3964
           X_current = X_probe_location - X_PROBE_OFFSET_FROM_EXTRUDER + cos(RADIANS(angle)) * radius;
3877
           Y_current = Y_probe_location - Y_PROBE_OFFSET_FROM_EXTRUDER + sin(RADIANS(angle)) * radius;
3965
           Y_current = Y_probe_location - Y_PROBE_OFFSET_FROM_EXTRUDER + sin(RADIANS(angle)) * radius;
3966
+
3878
           #if DISABLED(DELTA)
3967
           #if DISABLED(DELTA)
3879
             X_current = constrain(X_current, X_MIN_POS, X_MAX_POS);
3968
             X_current = constrain(X_current, X_MIN_POS, X_MAX_POS);
3880
             Y_current = constrain(Y_current, Y_MIN_POS, Y_MAX_POS);
3969
             Y_current = constrain(Y_current, Y_MIN_POS, Y_MAX_POS);
3904
         } // n_legs loop
3993
         } // n_legs loop
3905
       } // n_legs
3994
       } // n_legs
3906
 
3995
 
3907
-      // We don't really have to do this move, but if we don't we can see a funny shift in the Z Height
3908
-      // Because the user might not have the Z_RAISE_BEFORE_PROBING height identical to the
3909
-      // Z_RAISE_BETWEEN_PROBING height.  This gets us back to the probe location at the same height that
3910
-      // we have been running around the circle at.
3996
+      /**
3997
+       * We don't really have to do this move, but if we don't we can see a
3998
+       * funny shift in the Z Height because the user might not have the
3999
+       * Z_RAISE_BEFORE_PROBING height identical to the Z_RAISE_BETWEEN_PROBING
4000
+       * height. This gets us back to the probe location at the same height that
4001
+       * we have been running around the circle at.
4002
+       */
3911
       do_blocking_move_to_xy(X_probe_location - X_PROBE_OFFSET_FROM_EXTRUDER, Y_probe_location - Y_PROBE_OFFSET_FROM_EXTRUDER);
4003
       do_blocking_move_to_xy(X_probe_location - X_PROBE_OFFSET_FROM_EXTRUDER, Y_probe_location - Y_PROBE_OFFSET_FROM_EXTRUDER);
3912
       if (deploy_probe_for_each_reading)
4004
       if (deploy_probe_for_each_reading)
3913
         sample_set[n] = probe_pt(X_probe_location, Y_probe_location, Z_RAISE_BEFORE_PROBING, ProbeDeployAndStow, verbose_level);
4005
         sample_set[n] = probe_pt(X_probe_location, Y_probe_location, Z_RAISE_BEFORE_PROBING, ProbeDeployAndStow, verbose_level);
3917
           sample_set[n] = probe_pt(X_probe_location, Y_probe_location, Z_RAISE_BEFORE_PROBING, ProbeStay, verbose_level);
4009
           sample_set[n] = probe_pt(X_probe_location, Y_probe_location, Z_RAISE_BEFORE_PROBING, ProbeStay, verbose_level);
3918
       }
4010
       }
3919
 
4011
 
3920
-      //
3921
-      // Get the current mean for the data points we have so far
3922
-      //
4012
+      /**
4013
+       * Get the current mean for the data points we have so far
4014
+       */
3923
       sum = 0.0;
4015
       sum = 0.0;
3924
       for (uint8_t j = 0; j <= n; j++) sum += sample_set[j];
4016
       for (uint8_t j = 0; j <= n; j++) sum += sample_set[j];
3925
       mean = sum / (n + 1);
4017
       mean = sum / (n + 1);
3926
 
4018
 
3927
-      //
3928
-      // Now, use that mean to calculate the standard deviation for the
3929
-      // data points we have so far
3930
-      //
4019
+      /**
4020
+       * Now, use that mean to calculate the standard deviation for the
4021
+       * data points we have so far
4022
+       */
3931
       sum = 0.0;
4023
       sum = 0.0;
3932
       for (uint8_t j = 0; j <= n; j++) {
4024
       for (uint8_t j = 0; j <= n; j++) {
3933
         float ss = sample_set[j] - mean;
4025
         float ss = sample_set[j] - mean;
4367
   inline void gcode_M80() {
4459
   inline void gcode_M80() {
4368
     OUT_WRITE(PS_ON_PIN, PS_ON_AWAKE); //GND
4460
     OUT_WRITE(PS_ON_PIN, PS_ON_AWAKE); //GND
4369
 
4461
 
4370
-    // If you have a switch on suicide pin, this is useful
4371
-    // if you want to start another print with suicide feature after
4372
-    // a print without suicide...
4462
+    /**
4463
+     * If you have a switch on suicide pin, this is useful
4464
+     * if you want to start another print with suicide feature after
4465
+     * a print without suicide...
4466
+     */
4373
     #if HAS_SUICIDE
4467
     #if HAS_SUICIDE
4374
       OUT_WRITE(SUICIDE_PIN, HIGH);
4468
       OUT_WRITE(SUICIDE_PIN, HIGH);
4375
     #endif
4469
     #endif
6973
   float linear_per_segment = linear_travel / segments;
7067
   float linear_per_segment = linear_travel / segments;
6974
   float extruder_per_segment = extruder_travel / segments;
7068
   float extruder_per_segment = extruder_travel / segments;
6975
 
7069
 
6976
-  /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
6977
-     and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
6978
-         r_T = [cos(phi) -sin(phi);
6979
-                sin(phi)  cos(phi] * r ;
6980
-
6981
-     For arc generation, the center of the circle is the axis of rotation and the radius vector is
6982
-     defined from the circle center to the initial position. Each line segment is formed by successive
6983
-     vector rotations. This requires only two cos() and sin() computations to form the rotation
6984
-     matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
6985
-     all double numbers are single precision on the Arduino. (True double precision will not have
6986
-     round off issues for CNC applications.) Single precision error can accumulate to be greater than
6987
-     tool precision in some cases. Therefore, arc path correction is implemented.
6988
-
6989
-     Small angle approximation may be used to reduce computation overhead further. This approximation
6990
-     holds for everything, but very small circles and large MM_PER_ARC_SEGMENT values. In other words,
6991
-     theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
6992
-     to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
6993
-     numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
6994
-     issue for CNC machines with the single precision Arduino calculations.
6995
-
6996
-     This approximation also allows plan_arc to immediately insert a line segment into the planner
6997
-     without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
6998
-     a correction, the planner should have caught up to the lag caused by the initial plan_arc overhead.
6999
-     This is important when there are successive arc motions.
7000
-  */
7070
+  /**
7071
+   * Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
7072
+   * and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
7073
+   *     r_T = [cos(phi) -sin(phi);
7074
+   *            sin(phi)  cos(phi] * r ;
7075
+   *
7076
+   * For arc generation, the center of the circle is the axis of rotation and the radius vector is
7077
+   * defined from the circle center to the initial position. Each line segment is formed by successive
7078
+   * vector rotations. This requires only two cos() and sin() computations to form the rotation
7079
+   * matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
7080
+   * all double numbers are single precision on the Arduino. (True double precision will not have
7081
+   * round off issues for CNC applications.) Single precision error can accumulate to be greater than
7082
+   * tool precision in some cases. Therefore, arc path correction is implemented.
7083
+   *
7084
+   * Small angle approximation may be used to reduce computation overhead further. This approximation
7085
+   * holds for everything, but very small circles and large MM_PER_ARC_SEGMENT values. In other words,
7086
+   * theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
7087
+   * to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
7088
+   * numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
7089
+   * issue for CNC machines with the single precision Arduino calculations.
7090
+   *
7091
+   * This approximation also allows plan_arc to immediately insert a line segment into the planner
7092
+   * without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
7093
+   * a correction, the planner should have caught up to the lag caused by the initial plan_arc overhead.
7094
+   * This is important when there are successive arc motions.
7095
+   */
7001
   // Vector rotation matrix values
7096
   // Vector rotation matrix values
7002
   float cos_T = 1 - 0.5 * theta_per_segment * theta_per_segment; // Small angle approximation
7097
   float cos_T = 1 - 0.5 * theta_per_segment * theta_per_segment; // Small angle approximation
7003
   float sin_T = theta_per_segment;
7098
   float sin_T = theta_per_segment;

Loading…
Cancel
Save