Support Contact probe with bed scanner eddy sensor

Marlin/src/feature/bedlevel/bdl/bdl.cpp

@@ -46,31 +46,34 @@ BDS_Leveling bdl;
 #define DEBUG_OUT ENABLED(DEBUG_OUT_BD)
 #include "../../../core/debug_out.h"
 
-/**
- * M102 S<#> : Set adjustable Z height in 0.1mm units (10ths of a mm)
- *             (e.g., 'M102 S4' enables adjusting for Z <= 0.4mm)
- * M102 S0   : Disable adjustable Z height
- *
- * M102 S-1  : Read BDsensor version
- * M102 S-2  : Read BDsensor distance value
- * M102 S-5  : Read raw Calibration data
- * M102 S-6  : Start Calibration
- */
+// M102 S-5   Read raw Calibrate data
+// M102 S-6   Start Calibrate
+// M102 S4    Set the adjustable Z height value (e.g., 'M102 S4' means it will do adjusting while the Z height <= 0.4mm , disable with 'M102 S0'.)
+// M102 S-1   Read sensor information
 
 #define MAX_BD_HEIGHT                 4.0f
+#define CMD_READ_DATA                 1015
 #define CMD_READ_VERSION              1016
 #define CMD_START_READ_CALIBRATE_DATA 1017
 #define CMD_END_READ_CALIBRATE_DATA   1018
 #define CMD_START_CALIBRATE           1019
+#define CMD_DISTANCE_RAWDATA_TYPE     1020
 #define CMD_END_CALIBRATE             1021
+#define CMD_REBOOT_SENSOR             1022
+#define CMD_SWITCH_MODE               1023
+#define DATA_ERROR                    1024
+
 #define BD_SENSOR_I2C_ADDR            0x3C
 
 I2C_SegmentBED BD_I2C_SENSOR;
 float BDS_Leveling::pos_zero_offset;
-int8_t BDS_Leveling::config_state;
-
+int BDS_Leveling::config_state;
+int BDS_Leveling::sw_mode;
+bool BDS_Leveling::bd_triggered;
+#define move_waiting()  {while(abs(planner.get_axis_position_mm(Z_AXIS)-current_position.z)>0.005) safe_delay(100);}
 void BDS_Leveling::init(uint8_t _sda, uint8_t _scl, uint16_t delay_s) {
   config_state = BDS_IDLE;
+  sw_mode = -1;
   const int ret = BD_I2C_SENSOR.i2c_init(_sda, _scl, BD_SENSOR_I2C_ADDR, delay_s);
   if (ret != 1) SERIAL_ECHOLNPGM("BD Sensor Init Fail (", ret, ")");
   sync_plan_position();
@@ -104,19 +107,183 @@ float BDS_Leveling::interpret(const uint16_t data) {
   return (data & 0x3FF) / 100.0f;
 }
 
-float BDS_Leveling::read() {
+float BDS_Leveling::bd_read() {
   const uint16_t data = BD_I2C_SENSOR.BD_i2c_read();
   return check(data) ? NAN : interpret(data);
 }
 
+float BDS_Leveling::adjust_probe_down(float down_steps){
+  int intr, raw_d;
+  char tmp_1[21];
+  float z_limit = 1.0;
+  move_waiting();
+  intr = raw_d = BD_I2C_SENSOR.BD_i2c_read() & 0x3FF;
+  while (z_limit>0) {
+      z_limit -=  down_steps;
+       sprintf_P(tmp_1, PSTR("G1Z%d.%02d F100"), int(z_limit), int(z_limit * 100) % 100);
+      gcode.process_subcommands_now(tmp_1);
+      //gcode.process_subcommands_now(TS(F("G1Z"), p_float_t(z_limit, 3)));
+      move_waiting();
+      raw_d = BD_I2C_SENSOR.BD_i2c_read() & 0x3FF;
+      SERIAL_ECHOLNPGM("down ",intr,"  ", raw_d);
+      if (((intr - raw_d) < down_steps*100) && (raw_d < 500)){
+          break;
+      }
+      intr = raw_d;
+  }
+  return z_limit;
+}
+
+void BDS_Leveling::adjust_probe_up(float up_steps){
+  int intr0,intr1,intr2;
+  //char tmp_1[21];
+  float z0;
+  BD_I2C_SENSOR.BD_i2c_write(CMD_END_READ_CALIBRATE_DATA);
+  BD_I2C_SENSOR.BD_i2c_write(CMD_DISTANCE_RAWDATA_TYPE);
+  move_waiting();
+ // gcode.process_subcommands_now("G1 Z1");
+  z0 = Z_CLEARANCE_BETWEEN_PROBES;
+  if(Z_CLEARANCE_BETWEEN_PROBES>=2.5){
+      kill(F("Z_CLEARANCE_BETWEEN_PROBES must < 2.5"));
+  }
+  gcode.process_subcommands_now(TS(F("G1Z"), p_float_t(z0, 3)));
+  intr0 = BD_I2C_SENSOR.BD_i2c_read() & 0x3FF;
+  move_waiting();
+  if (intr0 > 720)
+    SERIAL_ECHOLNPGM("warning: triggered in air", intr0);
+  if(adjust_probe_down(0.1)<=0.1){
+    SERIAL_ECHOLNPGM("adjust failed0, homing again");
+    gcode.process_subcommands_now("G1Z4");
+    gcode.process_subcommands_now("G28Z0");
+    return;
+  }
+
+  intr0 = BD_I2C_SENSOR.BD_i2c_read() & 0x3FF;//
+  gcode.process_subcommands_now("G92Z0");
+  z0 = 0;
+  z0 = z0 + 0.4;
+  move_waiting();
+  gcode.process_subcommands_now(TS(F("G1Z"), p_float_t(z0, 3)));
+  planner.synchronize();
+  move_waiting();
+  safe_delay(500);
+  intr1 = BD_I2C_SENSOR.BD_i2c_read() & 0x3FF;//
+  if((intr1-10)<intr0){
+    //config_state = BDS_HOME_END;
+    SERIAL_ECHOLNPGM("adjust failed, homing again");
+    gcode.process_subcommands_now("G28Z0");
+    return;
+  }
+  z0 = z0 + 0.1;
+  gcode.process_subcommands_now(TS(F("G1Z"), p_float_t(z0, 3)));
+  planner.synchronize();
+  move_waiting();
+  safe_delay(500);
+  intr2 = BD_I2C_SENSOR.BD_i2c_read() & 0x3FF;//
+
+  z0 = z0 - (0.5-((intr1 - intr0)*0.1 / ((intr2 - intr1)))) ;
+  SERIAL_ECHOLNPGM("z:", intr0,",", intr1,",", intr2,"; ",z0);
+  //gcode.process_subcommands_now(F("G1Z0.2"));
+  //safe_delay(500);
+  gcode.process_subcommands_now(TS(F("G92Z"), p_float_t(z0, 3)));
+  //planner.synchronize();
+  //safe_delay(100);
+  //intr2 = BD_I2C_SENSOR.BD_i2c_read() & 0x3FF;
+  //SERIAL_ECHOLNPGM("Zend: ", intr2,", z0:", z0);
+  //current_position.z = 4;// z0;
+  //gcode.process_subcommands_now(TS(F("G92Z0")));
+  //sprintf_P(tmp_1, PSTR("G92Z%d.%02d"), int(z0), int(z0 * 100) % 100);
+  z0 = Z_CLEARANCE_BETWEEN_PROBES;//raise after homing
+  gcode.process_subcommands_now(TS(F("G1Z"), p_float_t(z0, 3)));
+  //gcode.process_subcommands_now("G1Z1");//
+  BD_I2C_SENSOR.BD_i2c_write(CMD_END_READ_CALIBRATE_DATA);
+
+}
+
+void BDS_Leveling::prepare_homing() {
+  config_state = BDS_HOMING_Z;
+  if(sw_mode ==-1){
+    read_version(6);
+  }
+  if(sw_mode == 1){
+    BD_I2C_SENSOR.BD_i2c_write(CMD_SWITCH_MODE);
+   // BD_I2C_SENSOR.BD_i2c_write(BD_SENSOR_HOME_Z_POSITION*100);
+    #ifdef BD_SENSOR_CONTACT_PROBE
+      BD_I2C_SENSOR.BD_i2c_write(1);
+    #else
+      BD_I2C_SENSOR.BD_i2c_write((int)(BD_SENSOR_HOME_Z_POSITION*100));
+    #endif
+    pinMode(I2C_BD_SDA_PIN, INPUT);
+    safe_delay(50);
+  }
+}
+
+void BDS_Leveling::end_homing() {
+  BD_I2C_SENSOR.BD_i2c_write(CMD_END_READ_CALIBRATE_DATA);
+  safe_delay(100);         
+  if(sw_mode == 1){
+    #ifdef BD_SENSOR_CONTACT_PROBE
+      current_position.z = Z_CLEARANCE_BETWEEN_PROBES ;
+    #else
+      current_position.z = bd_read();
+    #endif
+    // bdl.end_homing();
+  }
+  else
+    current_position.z = bd_read();
+  config_state = BDS_HOME_END;
+}
+
+bool BDS_Leveling::read_endstop() {
+  if(sw_mode == 1){
+    bd_triggered=digitalRead(I2C_BD_SDA_PIN);
+   // SERIAL_ECHOLNPGM("r", bd_triggered);
+  }
+  return bd_triggered;
+}
+
+void BDS_Leveling::read_version(int len_s) {
+  char tmp_1[21];
+  BD_I2C_SENSOR.BD_i2c_write(CMD_READ_VERSION);
+  safe_delay(50);
+  for (int i = 0; i < len_s; i++) {
+    tmp_1[i] = BD_I2C_SENSOR.BD_i2c_read() & 0xFF;
+    safe_delay(50);
+  }
+  BD_I2C_SENSOR.BD_i2c_write(CMD_END_READ_CALIBRATE_DATA);
+  BD_I2C_SENSOR.BD_i2c_write(CMD_END_READ_CALIBRATE_DATA);
+  //SERIAL_ECHOLNPGM("BD Sensor version:", tmp_1);
+  if (tmp_1[0] != 'V') {
+    SERIAL_ECHOLNPGM("Read Error. Check connection and delay.");
+    sw_mode = -1;
+  }
+  else if (tmp_1[4] != ' '){
+    sw_mode = 1;
+  }
+  else
+    sw_mode = 0;
+  SERIAL_ECHOLNPGM("BD Sensor version:", tmp_1, ",sw_mode:", sw_mode);
+  safe_delay(50);
+}
+
 void BDS_Leveling::process() {
   if (config_state == BDS_IDLE && printingIsActive()) return;
   static millis_t next_check_ms = 0; // starting at T=0
   static float zpos = 0.0f;
   const millis_t ms = millis();
+  if(config_state == BDS_HOMING_Z && sw_mode == 1)
+    return;
+  if(config_state == BDS_HOME_END){
+    config_state = BDS_IDLE;
+#ifdef BD_SENSOR_CONTACT_PROBE
+    if(sw_mode == 1){
+      adjust_probe_up(0);
+    }
+#endif    
+  }
   if (ELAPSED(ms, next_check_ms)) { // timed out (or first run)
     // Check at 1KHz, 5Hz, or 20Hz
-    next_check_ms = ms + (config_state == BDS_HOMING_Z ? 1 : (config_state < BDS_IDLE ? 200 : 50));
+    next_check_ms = ms + (config_state == BDS_HOMING_Z ? 1 : (config_state < BDS_IDLE ? 100 : 50));
 
     uint16_t tmp = 0;
     const float cur_z = planner.get_axis_position_mm(Z_AXIS) - pos_zero_offset;
@@ -141,7 +308,13 @@ void BDS_Leveling::process() {
 
       old_cur_z = cur_z;
       old_buf_z = current_position.z;
-      endstops.bdp_state_update(z_sensor <= BD_SENSOR_HOME_Z_POSITION);
+      if(sw_mode <= 0){
+        if(z_sensor <= BD_SENSOR_HOME_Z_POSITION)
+          bd_triggered = true;
+        else
+          bd_triggered = false;
+      }
+      //endstops.bdp_state_update(z_sensor <= BD_SENSOR_HOME_Z_POSITION);
 
       #if HAS_STATUS_MESSAGE
         static float old_z_sensor = 0;
@@ -170,17 +343,7 @@ void BDS_Leveling::process() {
     // Read version. Usually used as a connection check
     if (config_state == BDS_VERSION) {
       config_state = BDS_IDLE;
-      BD_I2C_SENSOR.BD_i2c_write(CMD_READ_VERSION);
-      safe_delay(100);
-      char tmp_1[21];
-      for (int i = 0; i < 19; i++) {
-        tmp_1[i] = BD_I2C_SENSOR.BD_i2c_read() & 0xFF;
-        safe_delay(50);
-      }
-      BD_I2C_SENSOR.BD_i2c_write(CMD_END_READ_CALIBRATE_DATA);
-      SERIAL_ECHOLNPGM("BD Sensor version:", tmp_1);
-      if (tmp_1[0] != 'V') SERIAL_ECHOLNPGM("Read Error. Check connection and delay.");
-      safe_delay(50);
+      read_version(19);
     }
     // read raw calibrate data
     else if (config_state == BDS_READ_RAW) {
@@ -195,6 +358,7 @@ void BDS_Leveling::process() {
       }
       BD_I2C_SENSOR.BD_i2c_write(CMD_END_READ_CALIBRATE_DATA);
       safe_delay(50);
+      BD_I2C_SENSOR.BD_i2c_write(CMD_END_READ_CALIBRATE_DATA);
       config_state = BDS_IDLE;
     }
     else if (config_state <= BDS_CALIBRATE_START) {   // Start Calibrate
@@ -206,20 +370,21 @@ void BDS_Leveling::process() {
 
         // Move the z axis instead of enabling the Z axis with M17
         // TODO: Use do_blocking_move_to_z for synchronized move.
-        current_position.z = 0;
-        sync_plan_position();
+      //  current_position.z = 0;
+      //  sync_plan_position();
         gcode.process_subcommands_now(F("G1Z0.05"));
         safe_delay(300);
         gcode.process_subcommands_now(F("G1Z0.00"));
         safe_delay(300);
-        current_position.z = 0;
+       // current_position.z = 0;
         sync_plan_position();
+        planner.synchronize();
         //safe_delay(1000);
-
+/*
         while ((planner.get_axis_position_mm(Z_AXIS) - pos_zero_offset) > 0.00001f) {
           safe_delay(200);
           SERIAL_ECHOLNPGM("waiting cur_z:", planner.get_axis_position_mm(Z_AXIS));
-        }
+        }*/
         zpos = 0.00001f;
         safe_delay(100);
         BD_I2C_SENSOR.BD_i2c_write(CMD_START_CALIBRATE); // Begin calibrate

Marlin/src/feature/bedlevel/bdl/bdl.h

@@ -34,16 +34,25 @@ enum BDS_State : int8_t {
   BDS_HOMING_Z        = -3,
   BDS_READ_RAW        = -5,
   BDS_CALIBRATE_START = -6,
-  BDS_CALIBRATING     = -7
+  BDS_CALIBRATING     = -7,
+  BDS_HOME_END        = -8
 };
 
 class BDS_Leveling {
 public:
-  static int8_t config_state;
+  static int config_state;
+  static int sw_mode;
+  static bool bd_triggered;
   static float pos_zero_offset;
   static void init(uint8_t _sda, uint8_t _scl, uint16_t delay_s);
-  static void process();
-  static float read();
+  void process();
+  static float bd_read();
+  static void prepare_homing();
+  static void adjust_probe_up(float up_steps);
+  static float adjust_probe_down(float down_steps);
+  static void end_homing();
+  static bool read_endstop();
+  static void read_version(int len_s);
   static float interpret(const uint16_t data);
   static float good_data(const uint16_t data) { return (data & 0x3FF) < 1016; }
   static bool check(const uint16_t data, const bool raw_data=false, const bool hicheck=false);

Marlin/src/gcode/bedlevel/abl/G29.cpp

@@ -766,13 +766,13 @@ G29_TYPE GcodeSuite::G29() {
             float pos;
             for (;;) {
               pos = planner.get_axis_position_mm(axis);
-              if (inInc > 0 ? (pos >= cmp) : (pos <= cmp)) break;
+              if (inInc > 0 ? ((pos+1) >= cmp) : (pos <= (cmp+1))) break;
               idle_no_sleep();
             }
             //if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM_P(axis == Y_AXIS ? PSTR("Y=") : PSTR("X=", pos);
 
             safe_delay(4);
-            abl.measured_z = current_position.z - bdl.read();
+            abl.measured_z = current_position.z - bdl.bd_read();
             if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("x_cur ", planner.get_axis_position_mm(X_AXIS), " z ", abl.measured_z);
 
           #else // !BD_SENSOR_PROBE_NO_STOP

Marlin/src/gcode/probe/M102.cpp

@@ -52,7 +52,7 @@ void GcodeSuite::M102() {
   if (parser.seenval('S')) {
     const int8_t command = parser.value_int();
     if (command == BDS_READ_MM)
-      SERIAL_ECHOLNPGM("Bed Distance:", bdl.read(), "mm");
+      SERIAL_ECHOLNPGM("Bed Distance:", bdl.bd_read(), "mm");
     else if ((command < BDS_IDLE) && printingIsActive())
       return;
     else

Marlin/src/module/endstops.cpp

@@ -78,9 +78,9 @@ volatile Endstops::endstop_mask_t Endstops::hit_state;
 Endstops::endstop_mask_t Endstops::live_state = 0;
 
 #if ENABLED(BD_SENSOR)
-  bool Endstops::bdp_state; // = false
+  #include "../feature/bedlevel/bdl/bdl.h"
   #if HOMING_Z_WITH_PROBE
-    #define READ_ENDSTOP(P) ((P == TERN(Z_MIN_PROBE_USES_Z_MIN_ENDSTOP_PIN, Z_MIN_PIN, Z_MIN_PROBE_PIN)) ? bdp_state : READ(P))
+    #define READ_ENDSTOP(P) ((P == TERN(Z_MIN_PROBE_USES_Z_MIN_ENDSTOP_PIN, Z_MIN_PIN, Z_MIN_PROBE_PIN)) ? bdl.read_endstop() : READ(P))
   #else
     #define READ_ENDSTOP(P) READ(P)
   #endif

Marlin/src/module/endstops.h

@@ -193,11 +193,6 @@ class Endstops {
      */
     static void update();
 
-    #if ENABLED(BD_SENSOR)
-      static bool bdp_state;
-      static void bdp_state_update(const bool z_state) { bdp_state = z_state; }
-    #endif
-
     /**
      * Get Endstop hit state.
      */