StallGuard homing implemented, working

Firmware/Configuration.h

@@ -246,6 +246,7 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 //#define DISABLE_MAX_ENDSTOPS
 //#define DISABLE_MIN_ENDSTOPS
 
+
 // Disable max endstops for compatibility with endstop checking routine
 #if defined(COREXY) && !defined(DISABLE_MAX_ENDSTOPS)
   #define DISABLE_MAX_ENDSTOPS

Firmware/Marlin_main.cpp

@@ -1762,40 +1762,90 @@ static float probe_pt(float x, float y, float z_before) {
 
 void homeaxis(int axis) {
 #define HOMEAXIS_DO(LETTER) \
-  ((LETTER##_MIN_PIN > -1 && LETTER##_HOME_DIR==-1) || (LETTER##_MAX_PIN > -1 && LETTER##_HOME_DIR==1))
+((LETTER##_MIN_PIN > -1 && LETTER##_HOME_DIR==-1) || (LETTER##_MAX_PIN > -1 && LETTER##_HOME_DIR==1))
-
+    
-  if (axis==X_AXIS ? HOMEAXIS_DO(X) :
+    if (axis==X_AXIS ? HOMEAXIS_DO(X) :
-      axis==Y_AXIS ? HOMEAXIS_DO(Y) :
+        axis==Y_AXIS ? HOMEAXIS_DO(Y) :
-      axis==Z_AXIS ? HOMEAXIS_DO(Z) :
+        0) {
-      0) {
+        int axis_home_dir = home_dir(axis);
-    int axis_home_dir = home_dir(axis);
+        
-
+        #ifdef HAVE_TMC2130_DRIVERS
-    current_position[axis] = 0;
+            st_setSGHoming(axis);
-    plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+            
-
+            // Configuration to spreadCycle
-    destination[axis] = 1.5 * max_length(axis) * axis_home_dir;
+            tmc2130_write((axis == X_AXIS)? X_TMC2130_CS : Y_TMC2130_CS,0x0,0,0,0,0x01);
-    feedrate = homing_feedrate[axis];
+            
-    plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+            tmc2130_write((axis == X_AXIS)? X_TMC2130_CS : Y_TMC2130_CS,0x6D,0,SG_THRESHOLD,0,0);
-    st_synchronize();
+            
-
+            tmc2130_write((axis == X_AXIS)? X_TMC2130_CS : Y_TMC2130_CS,0x14,0,0,0,TCOOLTHRS);
-    current_position[axis] = 0;
+        #endif
-    plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+        
-    destination[axis] = -home_retract_mm(axis) * axis_home_dir;
+        current_position[axis] = 0;
-    plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+        plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-    st_synchronize();
+        
-
+        destination[axis] = 1.5 * max_length(axis) * axis_home_dir;
-    destination[axis] = 2*home_retract_mm(axis) * axis_home_dir;
+        feedrate = homing_feedrate[axis];
-    feedrate = homing_feedrate[axis]/2 ;
+        plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
-    plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+        st_synchronize();
-    st_synchronize();
+        
-    axis_is_at_home(axis);
+        current_position[axis] = 0;
-    destination[axis] = current_position[axis];
+        plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-    feedrate = 0.0;
+        destination[axis] = -home_retract_mm(axis) * axis_home_dir;
-    endstops_hit_on_purpose();
+        plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
-    axis_known_position[axis] = true;
+        st_synchronize();
-  }
+        
+        destination[axis] = 2*home_retract_mm(axis) * axis_home_dir;
+        if(st_didLastHomingStall())
+            feedrate = homing_feedrate[axis];
+        else
+            feedrate = homing_feedrate[axis]/2 ;
+        plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+        st_synchronize();
+        axis_is_at_home(axis);
+        destination[axis] = current_position[axis];
+        feedrate = 0.0;
+        endstops_hit_on_purpose();
+        axis_known_position[axis] = true;
+        
+        #ifdef HAVE_TMC2130_DRIVERS
+            // Configuration back to stealthChop
+            tmc2130_write((axis == X_AXIS)? X_TMC2130_CS : Y_TMC2130_CS,0x0,0,0,0,0x05);
+            
+            st_setSGHoming(0xFF);
+            st_resetSGflags();
+        #endif
+    }
+    else if (axis==Z_AXIS ? HOMEAXIS_DO(Z) :
+             0) {
+        int axis_home_dir = home_dir(axis);
+        
+        current_position[axis] = 0;
+        plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+        
+        destination[axis] = 1.5 * max_length(axis) * axis_home_dir;
+        feedrate = homing_feedrate[axis];
+        plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+        st_synchronize();
+        
+        current_position[axis] = 0;
+        plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+        destination[axis] = -home_retract_mm(axis) * axis_home_dir;
+        plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+        st_synchronize();
+        
+        destination[axis] = 2*home_retract_mm(axis) * axis_home_dir;
+        feedrate = homing_feedrate[axis]/2 ;
+        plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+        st_synchronize();
+        axis_is_at_home(axis);
+        destination[axis] = current_position[axis];
+        feedrate = 0.0;
+        endstops_hit_on_purpose();
+        axis_known_position[axis] = true;
+    }
 }
 
+
 void home_xy()
 {
     set_destination_to_current();

Firmware/stepper.cpp

@@ -82,9 +82,18 @@ static bool old_y_max_endstop=false;
 static bool old_z_min_endstop=false;
 static bool old_z_max_endstop=false;
 
-static bool check_endstops = true;
+#ifdef SG_HOMING
+    static bool check_endstops = false;
+#else
+    static bool check_endstops = true;
+#endif
+
 static bool check_z_endstop = false;
 
+static uint8_t sg_homing_axis = 0xFF;
+static uint8_t sg_axis_stalled[2] = {0, 0};
+static uint8_t sg_lastHomingStalled = false;
+
 int8_t SilentMode;
 
 volatile long count_position[NUM_AXIS] = { 0, 0, 0, 0};
@@ -399,6 +408,11 @@ ISR(TIMER1_COMPA_vect)
         {
           #if defined(X_MIN_PIN) && X_MIN_PIN > -1
             bool x_min_endstop=(READ(X_MIN_PIN) != X_MIN_ENDSTOP_INVERTING);
+            #ifdef SG_HOMING
+                x_min_endstop=false;
+            #endif
+            if(sg_homing_axis == X_AXIS && !x_min_endstop)
+                x_min_endstop = sg_axis_stalled[X_AXIS];
             if(x_min_endstop && old_x_min_endstop && (current_block->steps_x > 0)) {
               endstops_trigsteps[X_AXIS] = count_position[X_AXIS];
               endstop_x_hit=true;
@@ -415,6 +429,8 @@ ISR(TIMER1_COMPA_vect)
         {
           #if defined(X_MAX_PIN) && X_MAX_PIN > -1
             bool x_max_endstop=(READ(X_MAX_PIN) != X_MAX_ENDSTOP_INVERTING);
+            if(sg_homing_axis == X_AXIS && !x_max_endstop)
+                x_max_endstop = sg_axis_stalled[X_AXIS];
             if(x_max_endstop && old_x_max_endstop && (current_block->steps_x > 0)){
               endstops_trigsteps[X_AXIS] = count_position[X_AXIS];
               endstop_x_hit=true;
@@ -435,6 +451,11 @@ ISR(TIMER1_COMPA_vect)
       {
         #if defined(Y_MIN_PIN) && Y_MIN_PIN > -1
           bool y_min_endstop=(READ(Y_MIN_PIN) != Y_MIN_ENDSTOP_INVERTING);
+            #ifdef SG_HOMING
+                      y_min_endstop=false;
+            #endif
+          if(sg_homing_axis == Y_AXIS && !y_min_endstop)
+              y_min_endstop = sg_axis_stalled[Y_AXIS];
           if(y_min_endstop && old_y_min_endstop && (current_block->steps_y > 0)) {
             endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS];
             endstop_y_hit=true;
@@ -449,6 +470,8 @@ ISR(TIMER1_COMPA_vect)
       {
         #if defined(Y_MAX_PIN) && Y_MAX_PIN > -1
           bool y_max_endstop=(READ(Y_MAX_PIN) != Y_MAX_ENDSTOP_INVERTING);
+          if(sg_homing_axis == Y_AXIS && !y_max_endstop)
+              y_max_endstop = sg_axis_stalled[Y_AXIS];
           if(y_max_endstop && old_y_max_endstop && (current_block->steps_y > 0)){
             endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS];
             endstop_y_hit=true;
@@ -765,6 +788,66 @@ ISR(TIMER1_COMPA_vect)
           
       }
       
+      uint32_t tmc2130_readRegister(uint8_t chipselect, uint8_t address){
+          
+          //datagram1 - write
+          SPI.beginTransaction(SPISettings(4000000, MSBFIRST, SPI_MODE3));
+          digitalWrite(chipselect,LOW);
+          SPI.transfer(address);
+          SPI.transfer(0x00);
+          SPI.transfer(0x00);
+          SPI.transfer(0x00);
+          SPI.transfer(0x00);
+          digitalWrite(chipselect, HIGH);
+          SPI.endTransaction();
+          
+          uint32_t val0;
+          
+          //datagram2 - response
+          SPI.beginTransaction(SPISettings(4000000, MSBFIRST, SPI_MODE3));
+          digitalWrite(chipselect,LOW);
+          
+          SPI.transfer(0); // ignore status bits
+          
+          val0 = SPI.transfer(0); // MSB
+          val0 = (val0 << 8) | SPI.transfer(0);
+          val0 = (val0 << 8) | SPI.transfer(0);
+          val0 = (val0 << 8) | SPI.transfer(0); //LSB
+          
+          digitalWrite(chipselect, HIGH);
+          SPI.endTransaction();
+          
+          return val0;
+      }
+      
+      uint16_t tmc2130_readSG(uint8_t chipselect){
+          
+          uint8_t address = 0x6F;
+          
+          uint32_t registerValue = tmc2130_readRegister(chipselect, address);
+          
+          uint16_t val0 = registerValue & 0x3ff;
+          
+          return val0;
+      }
+      
+      uint16_t tmc2130_readTStep(uint8_t chipselect){
+          
+          
+          uint8_t address = 0x12;
+          
+          uint32_t registerValue = tmc2130_readRegister(chipselect, address);
+          
+          uint16_t val0 = 0;
+          if(registerValue & 0x000f0000)
+              val0 = 0xffff;
+          else
+              val0 = registerValue & 0xffff;
+          
+          return val0;
+      }
+
+      
       void tmc2130_chopconf(uint8_t cs, bool extrapolate256 = 0, uint16_t microstep_resolution = 16)
       {
           uint8_t mres=0b0100;
@@ -793,6 +876,23 @@ ISR(TIMER1_COMPA_vect)
       {
           tmc2130_write(cs,0x13,0x00,0x00,0x00,0x00); // TMC LJ -> Adds possibility to swtich from stealthChop to spreadCycle automatically
       }
+      
+      
+      void st_setSGHoming(uint8_t axis){
+          sg_homing_axis = axis;
+      }
+      
+      void st_resetSGflags(){
+          sg_axis_stalled[X_AXIS] = false;
+          sg_axis_stalled[Y_AXIS] = false;
+      }
+      
+      uint8_t st_didLastHomingStall(){
+          uint8_t returnValue = sg_lastHomingStalled;
+          sg_lastHomingStalled = false;
+          return returnValue;
+      }
+
 
       void tmc2130_disable_motor(uint8_t driver)
       {
@@ -1063,11 +1163,53 @@ void st_init()
 // Block until all buffered steps are executed
 void st_synchronize()
 {
-    while( blocks_queued()) {
+  uint8_t delay = 0;
-    manage_heater();
+  while( blocks_queued()) {
-    // Vojtech: Don't disable motors inside the planner!
+      manage_heater();
-    manage_inactivity(true);
+      // Vojtech: Don't disable motors inside the planner!
-    lcd_update();
+      manage_inactivity(true);
+      lcd_update();
+      
+      if(sg_homing_axis == X_AXIS || sg_homing_axis == Y_AXIS)
+      {
+          uint8_t axis;
+          if(sg_homing_axis == X_AXIS)
+              axis = X_TMC2130_CS;
+          else
+              axis = Y_TMC2130_CS;
+          
+          uint16_t tstep = tmc2130_readTStep(axis);
+          //    	SERIAL_PROTOCOLLN(tstep);
+          
+          if(tstep < TCOOLTHRS)
+          {
+              if(delay < 255) // wait for a few tens microsteps until stallGuard is used //todo: read out microsteps directly, instead of delay counter
+                  delay++;
+              else
+              {
+                  uint16_t sg = tmc2130_readSG(axis);
+                  if(sg==0)
+                  {
+                      sg_axis_stalled[sg_homing_axis] = true;
+                      sg_lastHomingStalled = true;
+                  }
+                  else
+                      sg_axis_stalled[sg_homing_axis] = false;
+                  //            	SERIAL_PROTOCOLLN(sg);
+              }
+          }
+          else
+          {
+              sg_axis_stalled[sg_homing_axis] = false;
+              delay = 0;
+          }
+          
+      }
+      else
+      {
+          sg_axis_stalled[X_AXIS] = false;
+          sg_axis_stalled[Y_AXIS] = false;
+      }
   }
 }
 

Firmware/stepper.h

@@ -90,6 +90,17 @@ void microstep_readings();
 
 #ifdef HAVE_TMC2130_DRIVERS
 void tmc2130_check_overtemp();
+
+void tmc2130_write(uint8_t chipselect, uint8_t address,uint8_t wval1,uint8_t wval2,uint8_t wval3,uint8_t wval4);
+uint8_t tmc2130_read8(uint8_t chipselect, uint8_t address);
+uint16_t tmc2130_readSG(uint8_t chipselect);
+uint16_t tmc2130_readTStep(uint8_t chipselect);
+void tmc2130_PWMconf(uint8_t cs, uint8_t PWMgrad, uint8_t PWMampl);
+
+void st_setSGHoming(uint8_t axis);
+void st_resetSGflags();
+uint8_t st_didLastHomingStall();
+
 #endif
 
 #ifdef BABYSTEPPING