Browse Source

Updated M48 friendly for DELTA

Partial implementation with reference to MarlinFirmware/Marlin#3011
Scott Lahteine 9 years ago
parent
commit
68085ca855
2 changed files with 125 additions and 109 deletions
  1. 125
    105
      Marlin/Marlin_main.cpp
  2. 0
    4
      Marlin/SanityCheck.h

+ 125
- 105
Marlin/Marlin_main.cpp View File

@@ -3642,6 +3642,7 @@ inline void gcode_M42() {
3642 3642
    *     V = Verbose level (0-4, default=1)
3643 3643
    *     E = Engage Z probe for each reading
3644 3644
    *     L = Number of legs of movement before probe
3645
+   *     S = Schizoid (Or Star if you prefer)
3645 3646
    *
3646 3647
    * This function assumes the bed has been homed.  Specifically, that a G28 command
3647 3648
    * as been issued prior to invoking the M48 Z probe repeatability measurement function.
@@ -3651,7 +3652,7 @@ inline void gcode_M42() {
3651 3652
   inline void gcode_M48() {
3652 3653
 
3653 3654
     double sum = 0.0, mean = 0.0, sigma = 0.0, sample_set[50];
3654
-    uint8_t verbose_level = 1, n_samples = 10, n_legs = 0;
3655
+    uint8_t verbose_level = 1, n_samples = 10, n_legs = 0, schizoid_flag = 0;
3655 3656
 
3656 3657
     if (code_seen('V')) {
3657 3658
       verbose_level = code_value_short();
@@ -3672,50 +3673,57 @@ inline void gcode_M42() {
3672 3673
       }
3673 3674
     }
3674 3675
 
3675
-    double X_current = st_get_axis_position_mm(X_AXIS),
3676
-           Y_current = st_get_axis_position_mm(Y_AXIS),
3677
-           Z_current = st_get_axis_position_mm(Z_AXIS),
3678
-           E_current = st_get_axis_position_mm(E_AXIS),
3679
-           X_probe_location = X_current, Y_probe_location = Y_current,
3676
+    float  X_current = current_position[X_AXIS],
3677
+           Y_current = current_position[Y_AXIS],
3678
+           Z_current = current_position[Z_AXIS],
3679
+           X_probe_location = X_current + X_PROBE_OFFSET_FROM_EXTRUDER,
3680
+           Y_probe_location = Y_current + Y_PROBE_OFFSET_FROM_EXTRUDER,
3680 3681
            Z_start_location = Z_current + Z_RAISE_BEFORE_PROBING;
3681
-
3682 3682
     bool deploy_probe_for_each_reading = code_seen('E');
3683 3683
 
3684 3684
     if (code_seen('X')) {
3685
-      X_probe_location = code_value() - (X_PROBE_OFFSET_FROM_EXTRUDER);
3686
-      if (X_probe_location < X_MIN_POS || X_probe_location > X_MAX_POS) {
3687
-        out_of_range_error(PSTR("X"));
3688
-        return;
3689
-      }
3685
+      X_probe_location = code_value();
3686
+      #if DISABLED(DELTA)
3687
+        if (X_probe_location < MIN_PROBE_X || X_probe_location > MAX_PROBE_X) {
3688
+          out_of_range_error(PSTR("X"));
3689
+          return;
3690
+        }
3691
+      #endif
3690 3692
     }
3691 3693
 
3692 3694
     if (code_seen('Y')) {
3693
-      Y_probe_location = code_value() -  Y_PROBE_OFFSET_FROM_EXTRUDER;
3694
-      if (Y_probe_location < Y_MIN_POS || Y_probe_location > Y_MAX_POS) {
3695
-        out_of_range_error(PSTR("Y"));
3695
+      Y_probe_location = code_value();
3696
+      #if DISABLED(DELTA)
3697
+        if (Y_probe_location < MIN_PROBE_Y || Y_probe_location > MAX_PROBE_Y) {
3698
+          out_of_range_error(PSTR("Y"));
3699
+          return;
3700
+        }
3701
+      #endif
3702
+    }
3703
+
3704
+    #if ENABLED(DELTA)
3705
+      if (sqrt(X_probe_location * X_probe_location + Y_probe_location * Y_probe_location) > DELTA_PROBEABLE_RADIUS) {
3706
+        SERIAL_PROTOCOLPGM("? (X,Y) location outside of probeable radius.\n");
3696 3707
         return;
3697 3708
       }
3698
-    }
3709
+    #endif
3710
+
3711
+    bool seen_L = code_seen('L');
3699 3712
 
3700
-    if (code_seen('L')) {
3713
+    if (seen_L) {
3701 3714
       n_legs = code_value_short();
3702
-      if (n_legs == 1) n_legs = 2;
3703 3715
       if (n_legs < 0 || n_legs > 15) {
3704 3716
         SERIAL_PROTOCOLPGM("?Number of legs in movement not plausible (0-15).\n");
3705 3717
         return;
3706 3718
       }
3719
+      if (n_legs == 1) n_legs = 2;
3707 3720
     }
3708 3721
 
3709
-    //
3710
-    // Do all the preliminary setup work.   First raise the Z probe.
3711
-    //
3712
-
3713
-    st_synchronize();
3714
-    plan_bed_level_matrix.set_to_identity();
3715
-    plan_buffer_line(X_current, Y_current, Z_start_location, E_current, homing_feedrate[Z_AXIS] / 60, active_extruder);
3716
-    st_synchronize();
3722
+    if (code_seen('S')) {
3723
+      schizoid_flag++;
3724
+      if (!seen_L) n_legs = 7;
3725
+    }
3717 3726
 
3718
-    //
3719 3727
     // Now get everything to the specified probe point So we can safely do a probe to
3720 3728
     // get us close to the bed.  If the Z-Axis is far from the bed, we don't want to
3721 3729
     // use that as a starting point for each probe.
@@ -3723,90 +3731,112 @@ inline void gcode_M42() {
3723 3731
     if (verbose_level > 2)
3724 3732
       SERIAL_PROTOCOLPGM("Positioning the probe...\n");
3725 3733
 
3726
-    plan_buffer_line(X_probe_location, Y_probe_location, Z_start_location,
3727
-                     E_current,
3728
-                     homing_feedrate[X_AXIS] / 60,
3729
-                     active_extruder);
3730
-    st_synchronize();
3734
+    #if ENABLED(DELTA)
3735
+      reset_bed_level();    // we don't do bed level correction in M48 because we want the raw data when we probe
3736
+    #else
3737
+      plan_bed_level_matrix.set_to_identity();  // we don't do bed level correction in M48 because we wantthe raw data when we probe
3738
+    #endif
3739
+
3740
+    if (Z_start_location < Z_RAISE_BEFORE_PROBING * 2.0)
3741
+      do_blocking_move_to_z(Z_start_location);
3731 3742
 
3732
-    current_position[X_AXIS] = X_current = st_get_axis_position_mm(X_AXIS);
3733
-    current_position[Y_AXIS] = Y_current = st_get_axis_position_mm(Y_AXIS);
3734
-    current_position[Z_AXIS] = Z_current = st_get_axis_position_mm(Z_AXIS);
3735
-    current_position[E_AXIS] = E_current = st_get_axis_position_mm(E_AXIS);
3743
+    do_blocking_move_to_xy(X_probe_location - X_PROBE_OFFSET_FROM_EXTRUDER, Y_probe_location - Y_PROBE_OFFSET_FROM_EXTRUDER);
3736 3744
 
3737 3745
     //
3738 3746
     // OK, do the initial probe to get us close to the bed.
3739 3747
     // Then retrace the right amount and use that in subsequent probes
3740 3748
     //
3741
-
3742
-    deploy_z_probe();
3743
-
3744 3749
     setup_for_endstop_move();
3745
-    run_z_probe();
3746 3750
 
3747
-    Z_current = current_position[Z_AXIS] = st_get_axis_position_mm(Z_AXIS);
3748
-    Z_start_location = Z_current + Z_RAISE_BEFORE_PROBING;
3751
+    probe_pt(X_probe_location, Y_probe_location, Z_RAISE_BEFORE_PROBING,
3752
+      deploy_probe_for_each_reading ? ProbeDeployAndStow : ProbeDeploy,
3753
+      verbose_level);
3749 3754
 
3750
-    plan_buffer_line(X_probe_location, Y_probe_location, Z_start_location,
3751
-                     E_current,
3752
-                     homing_feedrate[X_AXIS] / 60,
3753
-                     active_extruder);
3754
-    st_synchronize();
3755
-    Z_current = current_position[Z_AXIS] = st_get_axis_position_mm(Z_AXIS);
3756
-
3757
-    if (deploy_probe_for_each_reading) stow_z_probe();
3755
+    raise_z_after_probing();
3758 3756
 
3759 3757
     for (uint8_t n = 0; n < n_samples; n++) {
3760
-      // Make sure we are at the probe location
3761
-      do_blocking_move_to(X_probe_location, Y_probe_location, Z_start_location); // this also updates current_position
3762
-
3758
+      randomSeed(millis());
3759
+      delay(500);
3763 3760
       if (n_legs) {
3764
-        millis_t ms = millis();
3765
-        double radius = ms % ((X_MAX_LENGTH) / 4),       // limit how far out to go
3766
-               theta = RADIANS(ms % 360L);
3767
-        float dir = (ms & 0x0001) ? 1 : -1;            // clockwise or counter clockwise
3761
+        float radius, angle = random(0.0, 360.0);
3762
+        int dir = (random(0, 10) > 5.0) ? -1 : 1;  // clockwise or counter clockwise
3768 3763
 
3769
-        //SERIAL_ECHOPAIR("starting radius: ",radius);
3770
-        //SERIAL_ECHOPAIR("   theta: ",theta);
3771
-        //SERIAL_ECHOPAIR("   direction: ",dir);
3772
-        //SERIAL_EOL;
3764
+        radius = random(
3765
+          #if ENABLED(DELTA)
3766
+            DELTA_PROBEABLE_RADIUS / 8, DELTA_PROBEABLE_RADIUS / 3
3767
+          #else
3768
+            5, X_MAX_LENGTH / 8
3769
+          #endif
3770
+        );
3771
+
3772
+        if (verbose_level > 3) {
3773
+          SERIAL_ECHOPAIR("Starting radius: ", radius);
3774
+          SERIAL_ECHOPAIR("   angle: ", angle);
3775
+          delay(100);
3776
+          if (dir > 0)
3777
+            SERIAL_ECHO(" Direction: Counter Clockwise \n");
3778
+          else
3779
+            SERIAL_ECHO(" Direction: Clockwise \n");
3780
+          delay(100);
3781
+        }
3773 3782
 
3774 3783
         for (uint8_t l = 0; l < n_legs - 1; l++) {
3775
-          ms = millis();
3776
-          theta += RADIANS(dir * (ms % 20L));
3777
-          radius += (ms % 10L) - 5L;
3778
-          if (radius < 0.0) radius = -radius;
3779
-
3780
-          X_current = X_probe_location + cos(theta) * radius;
3781
-          X_current = constrain(X_current, X_MIN_POS, X_MAX_POS);
3782
-          Y_current = Y_probe_location + sin(theta) * radius;
3783
-          Y_current = constrain(Y_current, Y_MIN_POS, Y_MAX_POS);
3784
-
3784
+          double delta_angle;
3785
+          if (schizoid_flag)
3786
+            delta_angle = dir * 2.0 * 72.0;   // The points of a 5 point star are 72 degrees apart.  We need to
3787
+          // skip a point and go to the next one on the star.
3788
+          else
3789
+            delta_angle = dir * (float) random(25, 45);   // If we do this line, we are just trying to move further
3790
+          // around the circle.
3791
+          angle += delta_angle;
3792
+          while (angle > 360.0)   // We probably do not need to keep the angle between 0 and 2*PI, but the
3793
+            angle -= 360.0;       // Arduino documentation says the trig functions should not be given values
3794
+          while (angle < 0.0)     // outside of this range.   It looks like they behave correctly with
3795
+            angle += 360.0;       // numbers outside of the range, but just to be safe we clamp them.
3796
+          X_current = X_probe_location - X_PROBE_OFFSET_FROM_EXTRUDER + cos(RADIANS(angle)) * radius;
3797
+          Y_current = Y_probe_location - Y_PROBE_OFFSET_FROM_EXTRUDER + sin(RADIANS(angle)) * radius;
3798
+          #if DISABLED(DELTA)
3799
+            X_current = constrain(X_current, X_MIN_POS, X_MAX_POS);
3800
+            Y_current = constrain(Y_current, Y_MIN_POS, Y_MAX_POS);
3801
+          #else
3802
+            // If we have gone out too far, we can do a simple fix and scale the numbers
3803
+            // back in closer to the origin.
3804
+            while (sqrt(X_current * X_current + Y_current * Y_current) > DELTA_PROBEABLE_RADIUS) {
3805
+              X_current /= 1.25;
3806
+              Y_current /= 1.25;
3807
+              if (verbose_level > 3) {
3808
+                SERIAL_ECHOPAIR("Pulling point towards center:", X_current);
3809
+                SERIAL_ECHOPAIR(", ", Y_current);
3810
+                SERIAL_EOL;
3811
+                delay(50);
3812
+              }
3813
+            }
3814
+          #endif
3785 3815
           if (verbose_level > 3) {
3816
+            SERIAL_PROTOCOL("Going to:");
3786 3817
             SERIAL_ECHOPAIR("x: ", X_current);
3787 3818
             SERIAL_ECHOPAIR("y: ", Y_current);
3819
+            SERIAL_ECHOPAIR("  z: ", current_position[Z_AXIS]);
3788 3820
             SERIAL_EOL;
3821
+            delay(55);
3789 3822
           }
3790
-
3791
-          do_blocking_move_to(X_current, Y_current, Z_current); // this also updates current_position
3792
-
3823
+          do_blocking_move_to_xy(X_current, Y_current);
3793 3824
         } // n_legs loop
3794
-
3795
-        // Go back to the probe location
3796
-        do_blocking_move_to(X_probe_location, Y_probe_location, Z_start_location); // this also updates current_position
3797
-
3798 3825
       } // n_legs
3799 3826
 
3800
-      if (deploy_probe_for_each_reading)  {
3801
-        deploy_z_probe();
3802
-        delay(1000);
3827
+      // We don't really have to do this move, but if we don't we can see a funny shift in the Z Height
3828
+      // Because the user might not have the Z_RAISE_BEFORE_PROBING height identical to the
3829
+      // Z_RAISE_BETWEEN_PROBING height.  This gets us back to the probe location at the same height that
3830
+      // we have been running around the circle at.
3831
+      do_blocking_move_to_xy(X_probe_location - X_PROBE_OFFSET_FROM_EXTRUDER, Y_probe_location - Y_PROBE_OFFSET_FROM_EXTRUDER);
3832
+      if (deploy_probe_for_each_reading)
3833
+        sample_set[n] = probe_pt(X_probe_location, Y_probe_location, Z_RAISE_BEFORE_PROBING, ProbeDeployAndStow, verbose_level);
3834
+      else {
3835
+        if (n == n_samples - 1)
3836
+          sample_set[n] = probe_pt(X_probe_location, Y_probe_location, Z_RAISE_BEFORE_PROBING, ProbeStow, verbose_level); else
3837
+          sample_set[n] = probe_pt(X_probe_location, Y_probe_location, Z_RAISE_BEFORE_PROBING, ProbeStay, verbose_level);
3803 3838
       }
3804 3839
 
3805
-      setup_for_endstop_move();
3806
-      run_z_probe();
3807
-
3808
-      sample_set[n] = current_position[Z_AXIS];
3809
-
3810 3840
       //
3811 3841
       // Get the current mean for the data points we have so far
3812 3842
       //
@@ -3824,13 +3854,13 @@ inline void gcode_M42() {
3824 3854
         sum += ss * ss;
3825 3855
       }
3826 3856
       sigma = sqrt(sum / (n + 1));
3827
-
3828 3857
       if (verbose_level > 1) {
3829 3858
         SERIAL_PROTOCOL(n + 1);
3830 3859
         SERIAL_PROTOCOLPGM(" of ");
3831 3860
         SERIAL_PROTOCOL((int)n_samples);
3832 3861
         SERIAL_PROTOCOLPGM("   z: ");
3833 3862
         SERIAL_PROTOCOL_F(current_position[Z_AXIS], 6);
3863
+        delay(50);
3834 3864
         if (verbose_level > 2) {
3835 3865
           SERIAL_PROTOCOLPGM(" mean: ");
3836 3866
           SERIAL_PROTOCOL_F(mean, 6);
@@ -3838,36 +3868,26 @@ inline void gcode_M42() {
3838 3868
           SERIAL_PROTOCOL_F(sigma, 6);
3839 3869
         }
3840 3870
       }
3841
-
3842 3871
       if (verbose_level > 0) SERIAL_EOL;
3872
+      delay(50);
3873
+      do_blocking_move_to_z(current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS);
3874
+    }  // End of probe loop code
3843 3875
 
3844
-      plan_buffer_line(X_probe_location, Y_probe_location, Z_start_location, current_position[E_AXIS], homing_feedrate[Z_AXIS] / 60, active_extruder);
3845
-      st_synchronize();
3846
-
3847
-      // Stow between
3848
-      if (deploy_probe_for_each_reading) {
3849
-        stow_z_probe();
3850
-        delay(1000);
3851
-      }
3852
-    }
3853
-
3854
-    // Stow after
3855
-    if (!deploy_probe_for_each_reading) {
3856
-      stow_z_probe();
3857
-      delay(1000);
3858
-    }
3859
-
3860
-    clean_up_after_endstop_move();
3876
+    // raise_z_after_probing();
3861 3877
 
3862 3878
     if (verbose_level > 0) {
3863 3879
       SERIAL_PROTOCOLPGM("Mean: ");
3864 3880
       SERIAL_PROTOCOL_F(mean, 6);
3865 3881
       SERIAL_EOL;
3882
+      delay(25);
3866 3883
     }
3867 3884
 
3868 3885
     SERIAL_PROTOCOLPGM("Standard Deviation: ");
3869 3886
     SERIAL_PROTOCOL_F(sigma, 6);
3870 3887
     SERIAL_EOL; SERIAL_EOL;
3888
+    delay(25);
3889
+
3890
+    clean_up_after_endstop_move();
3871 3891
   }
3872 3892
 
3873 3893
 #endif // AUTO_BED_LEVELING_FEATURE && Z_MIN_PROBE_REPEATABILITY_TEST

+ 0
- 4
Marlin/SanityCheck.h View File

@@ -231,10 +231,6 @@
231 231
       #error You cannot use Z_PROBE_SLED with DELTA.
232 232
     #endif
233 233
 
234
-    #if ENABLED(Z_MIN_PROBE_REPEATABILITY_TEST)
235
-      #error Z_MIN_PROBE_REPEATABILITY_TEST is not supported with DELTA yet.
236
-    #endif
237
-
238 234
   #endif
239 235
 
240 236
 #endif

Loading…
Cancel
Save