From e24b7ce06ac1427b6e69d7d0fc9e84278ed61adc Mon Sep 17 00:00:00 2001
From: Petr Krejci <petr.krejci@it-g.cz>
Date: Wed, 2 Nov 2022 22:08:47 +0100
Subject: [PATCH] Auto sleep stepper

---
 grbl/defaults.h | 13 +++++++++++++
 grbl/protocol.c | 30 ++++++++++++++++++++++++++++++
 grbl/report.c   |  1 +
 grbl/settings.c |  4 +++-
 grbl/settings.h |  3 ++-
 grbl/stepper.c  | 15 ++++++++++++---
 grbl/stepper.h  |  3 +++
 7 files changed, 64 insertions(+), 5 deletions(-)

diff --git a/grbl/defaults.h b/grbl/defaults.h
index 2e461300..a57b4b77 100644
--- a/grbl/defaults.h
+++ b/grbl/defaults.h
@@ -47,6 +47,7 @@
   #define DEFAULT_STEPPING_INVERT_MASK 0
   #define DEFAULT_DIRECTION_INVERT_MASK 0
   #define DEFAULT_STEPPER_IDLE_LOCK_TIME 25 // msec (0-254, 255 keeps steppers enabled)
+  #define DEFAULT_AUTOSLEEP 10 // minutes
   #define DEFAULT_STATUS_REPORT_MASK 1 // MPos enabled
   #define DEFAULT_JUNCTION_DEVIATION 0.01 // mm
   #define DEFAULT_ARC_TOLERANCE 0.002 // mm
@@ -89,6 +90,7 @@
   #define DEFAULT_STEPPING_INVERT_MASK 0
   #define DEFAULT_DIRECTION_INVERT_MASK ((1<<Y_AXIS)|(1<<Z_AXIS))
   #define DEFAULT_STEPPER_IDLE_LOCK_TIME 25 // msec (0-254, 255 keeps steppers enabled)
+  #define DEFAULT_AUTOSLEEP 10 // minutes
   #define DEFAULT_STATUS_REPORT_MASK 1 // MPos enabled
   #define DEFAULT_JUNCTION_DEVIATION 0.01 // mm
   #define DEFAULT_ARC_TOLERANCE 0.002 // mm
@@ -127,6 +129,7 @@
   #define DEFAULT_STEPPING_INVERT_MASK 0
   #define DEFAULT_DIRECTION_INVERT_MASK ((1<<Y_AXIS)|(1<<Z_AXIS))
   #define DEFAULT_STEPPER_IDLE_LOCK_TIME 250 // msec (0-254, 255 keeps steppers enabled)
+  #define DEFAULT_AUTOSLEEP 10 // minutes
   #define DEFAULT_STATUS_REPORT_MASK 3 // WPos enabled
   #define DEFAULT_JUNCTION_DEVIATION 0.01 // mm
   #define DEFAULT_ARC_TOLERANCE 0.002 // mm
@@ -172,6 +175,7 @@
   #define DEFAULT_STEPPING_INVERT_MASK 0
   #define DEFAULT_DIRECTION_INVERT_MASK ((1<<Y_AXIS)|(1<<Z_AXIS))
   #define DEFAULT_STEPPER_IDLE_LOCK_TIME 255 // msec (0-254, 255 keeps steppers enabled)
+  #define DEFAULT_AUTOSLEEP 10 // minutes
   #define DEFAULT_STATUS_REPORT_MASK 1 // MPos enabled
   #define DEFAULT_JUNCTION_DEVIATION 0.02 // mm
   #define DEFAULT_ARC_TOLERANCE 0.002 // mm
@@ -217,6 +221,7 @@
   #define DEFAULT_STEPPING_INVERT_MASK 0
   #define DEFAULT_DIRECTION_INVERT_MASK ((1<<X_AXIS)|(1<<Z_AXIS))
   #define DEFAULT_STEPPER_IDLE_LOCK_TIME 255 // msec (0-254, 255 keeps steppers enabled)
+  #define DEFAULT_AUTOSLEEP 10 // minutes
   #define DEFAULT_STATUS_REPORT_MASK 1 // MPos enabled
   #define DEFAULT_JUNCTION_DEVIATION 0.02 // mm
   #define DEFAULT_ARC_TOLERANCE 0.002 // mm
@@ -261,6 +266,7 @@
   #define DEFAULT_STEPPING_INVERT_MASK 0
   #define DEFAULT_DIRECTION_INVERT_MASK ((1<<X_AXIS)|(1<<Z_AXIS))
   #define DEFAULT_STEPPER_IDLE_LOCK_TIME 255 // msec (0-254, 255 keeps steppers enabled)
+  #define DEFAULT_AUTOSLEEP 10 // minutes
   #define DEFAULT_STATUS_REPORT_MASK 1 // MPos enabled
   #define DEFAULT_JUNCTION_DEVIATION 0.02 // mm
   #define DEFAULT_ARC_TOLERANCE 0.01 // mm
@@ -306,6 +312,7 @@
   #define DEFAULT_STEPPING_INVERT_MASK 0
   #define DEFAULT_DIRECTION_INVERT_MASK ((1<<X_AXIS)|(1<<Y_AXIS))
   #define DEFAULT_STEPPER_IDLE_LOCK_TIME 255 // msec (0-254, 255 keeps steppers enabled)
+  #define DEFAULT_AUTOSLEEP 10 // minutes
   #define DEFAULT_STATUS_REPORT_MASK 1 // MPos enabled
   #define DEFAULT_JUNCTION_DEVIATION 0.02 // mm
   #define DEFAULT_ARC_TOLERANCE 0.002 // mm
@@ -351,6 +358,7 @@
   #define DEFAULT_STEPPING_INVERT_MASK 0
   #define DEFAULT_DIRECTION_INVERT_MASK ((1<<X_AXIS)|(1<<Y_AXIS))
   #define DEFAULT_STEPPER_IDLE_LOCK_TIME 255 // msec (0-254, 255 keeps steppers enabled)
+  #define DEFAULT_AUTOSLEEP 10 // minutes
   #define DEFAULT_STATUS_REPORT_MASK 1 // MPos enabled
   #define DEFAULT_JUNCTION_DEVIATION 0.02 // mm
   #define DEFAULT_ARC_TOLERANCE 0.002 // mm
@@ -390,6 +398,7 @@
   #define DEFAULT_STEPPING_INVERT_MASK 0
   #define DEFAULT_DIRECTION_INVERT_MASK 0
   #define DEFAULT_STEPPER_IDLE_LOCK_TIME 25 // msec (0-254, 255 keeps steppers enabled)
+  #define DEFAULT_AUTOSLEEP 10 // minutes
   #define DEFAULT_STATUS_REPORT_MASK 1 // MPos enabled
   #define DEFAULT_JUNCTION_DEVIATION 0.01 // mm
   #define DEFAULT_ARC_TOLERANCE 0.002 // mm
@@ -429,6 +438,7 @@
   #define DEFAULT_STEPPING_INVERT_MASK 0
   #define DEFAULT_DIRECTION_INVERT_MASK 0
   #define DEFAULT_STEPPER_IDLE_LOCK_TIME 25 // msec (0-254, 255 keeps steppers enabled)
+  #define DEFAULT_AUTOSLEEP 10 // minutes
   #define DEFAULT_STATUS_REPORT_MASK 1 // MPos enabled
   #define DEFAULT_JUNCTION_DEVIATION 0.01 // mm
   #define DEFAULT_ARC_TOLERANCE 0.002 // mm
@@ -472,6 +482,7 @@
   #define DEFAULT_STEPPING_INVERT_MASK 0
   #define DEFAULT_DIRECTION_INVERT_MASK ((1<<Y_AXIS))
   #define DEFAULT_STEPPER_IDLE_LOCK_TIME 25 // msec (0-254, 255 keeps steppers enabled)
+  #define DEFAULT_AUTOSLEEP 10 // minutes
   #define DEFAULT_STATUS_REPORT_MASK 1 // MPos enabled
   #define DEFAULT_JUNCTION_DEVIATION 0.02 // mm
   #define DEFAULT_ARC_TOLERANCE 0.002 // mm
@@ -511,6 +522,7 @@
   #define DEFAULT_STEPPING_INVERT_MASK 0
   #define DEFAULT_DIRECTION_INVERT_MASK 0
   #define DEFAULT_STEPPER_IDLE_LOCK_TIME 25 // msec (0-254, 255 keeps steppers enabled)
+  #define DEFAULT_AUTOSLEEP 10 // minutes
   #define DEFAULT_STATUS_REPORT_MASK 1 // MPos enabled
   #define DEFAULT_JUNCTION_DEVIATION 0.02 // mm
   #define DEFAULT_ARC_TOLERANCE 0.002 // mm
@@ -550,6 +562,7 @@
   #define DEFAULT_STEPPING_INVERT_MASK 0
   #define DEFAULT_DIRECTION_INVERT_MASK 0
   #define DEFAULT_STEPPER_IDLE_LOCK_TIME 25 // msec (0-254, 255 keeps steppers enabled)
+  #define DEFAULT_AUTOSLEEP 10 // minutes
   #define DEFAULT_STATUS_REPORT_MASK 1 // MPos enabled
   #define DEFAULT_JUNCTION_DEVIATION 0.01 // mm
   #define DEFAULT_ARC_TOLERANCE 0.002 // mm
diff --git a/grbl/protocol.c b/grbl/protocol.c
index 08ea48bf..278e4da9 100644
--- a/grbl/protocol.c
+++ b/grbl/protocol.c
@@ -71,6 +71,10 @@ void protocol_main_loop()
   uint8_t line_flags = 0;
   uint8_t char_counter = 0;
   uint8_t c;
+
+  const uint_fast32_t stepper_sleep_counts = 60000 * settings.auto_sleep;
+  uint_fast32_t stepper_sleep = stepper_sleep_counts;
+
   for (;;) {
 
     // Process one line of incoming serial data, as the data becomes available. Performs an
@@ -100,6 +104,13 @@ void protocol_main_loop()
           // Everything else is gcode. Block if in alarm or jog mode.
           report_status_message(STATUS_SYSTEM_GC_LOCK);
         } else {
+          // Wakeup stepper if IU have disabled before
+		  if (stepper_sleep == 0) {
+			 st_wake_up();
+		  }
+          // Reset loop sleep
+          stepper_sleep = stepper_sleep_counts;
+
           // Parse and execute g-code block.
           report_status_message(gc_execute_line(line));
         }
@@ -158,6 +169,25 @@ void protocol_main_loop()
 
     protocol_execute_realtime();  // Runtime command check point.
     if (sys.abort) { return; } // Bail to main() program loop to reset system.
+
+    // idle lock is off?
+    if (settings.stepper_idle_lock_time == 255) {
+    	// Will I do a block in next loop?
+    	if (sys.state == STATE_IDLE) {
+    		// Last waiting loop. so disable stepper
+    		if (stepper_sleep == 1) {
+    			st_disable_set(true);
+    		}
+    		// I will sleep ...
+    		if (stepper_sleep > 0) {
+    			stepper_sleep--;
+    			_delay_ms(1);
+    		}
+    	} else {
+    		// I will do something, so don't sleep
+    		stepper_sleep = stepper_sleep_counts;
+    	}
+    }
   }
 
   return; /* Never reached */
diff --git a/grbl/report.c b/grbl/report.c
index 000836fa..f7b11630 100644
--- a/grbl/report.c
+++ b/grbl/report.c
@@ -208,6 +208,7 @@ void report_grbl_settings() {
   #else
     report_util_uint8_setting(32,0);
   #endif
+  report_util_uint8_setting(33,settings.auto_sleep);
   // Print axis settings
   uint8_t idx, set_idx;
   uint8_t val = AXIS_SETTINGS_START_VAL;
diff --git a/grbl/settings.c b/grbl/settings.c
index a9c830e7..1f3dfe60 100644
--- a/grbl/settings.c
+++ b/grbl/settings.c
@@ -57,7 +57,8 @@ const __flash settings_t defaults = {\
     .acceleration[Z_AXIS] = DEFAULT_Z_ACCELERATION,
     .max_travel[X_AXIS] = (-DEFAULT_X_MAX_TRAVEL),
     .max_travel[Y_AXIS] = (-DEFAULT_Y_MAX_TRAVEL),
-    .max_travel[Z_AXIS] = (-DEFAULT_Z_MAX_TRAVEL)};
+    .max_travel[Z_AXIS] = (-DEFAULT_Z_MAX_TRAVEL),
+    .auto_sleep = DEFAULT_AUTOSLEEP };
 
 
 // Method to store startup lines into EEPROM
@@ -286,6 +287,7 @@ uint8_t settings_store_global_setting(uint8_t parameter, float value) {
       case 27: settings.homing_pulloff = value; break;
       case 30: settings.rpm_max = value; spindle_init(); break; // Re-initialize spindle rpm calibration
       case 31: settings.rpm_min = value; spindle_init(); break; // Re-initialize spindle rpm calibration
+      case 33: settings.auto_sleep = int_value; break; // autosleep sec
       case 32:
         #ifdef VARIABLE_SPINDLE
           if (int_value) { settings.flags |= BITFLAG_LASER_MODE; }
diff --git a/grbl/settings.h b/grbl/settings.h
index 2d83ba9f..601f25a6 100644
--- a/grbl/settings.h
+++ b/grbl/settings.h
@@ -27,7 +27,7 @@
 
 // Version of the EEPROM data. Will be used to migrate existing data from older versions of Grbl
 // when firmware is upgraded. Always stored in byte 0 of eeprom
-#define SETTINGS_VERSION 10  // NOTE: Check settings_reset() when moving to next version.
+#define SETTINGS_VERSION 11  // NOTE: Check settings_reset() when moving to next version.
 
 // Define bit flag masks for the boolean settings in settings.flag.
 #define BIT_REPORT_INCHES      0
@@ -110,6 +110,7 @@ typedef struct {
   float homing_seek_rate;
   uint16_t homing_debounce_delay;
   float homing_pulloff;
+  uint8_t auto_sleep;
 } settings_t;
 extern settings_t settings;
 
diff --git a/grbl/stepper.c b/grbl/stepper.c
index 8e0b151d..328217a8 100644
--- a/grbl/stepper.c
+++ b/grbl/stepper.c
@@ -262,11 +262,20 @@ void st_go_idle()
     delay_ms(settings.stepper_idle_lock_time);
     pin_state = true; // Override. Disable steppers.
   }
-  if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { pin_state = !pin_state; } // Apply pin invert.
-  if (pin_state) { STEPPERS_DISABLE_PORT |= (1<<STEPPERS_DISABLE_BIT); }
-  else { STEPPERS_DISABLE_PORT &= ~(1<<STEPPERS_DISABLE_BIT); }
+	st_disable_set(pin_state);
 }
 
+void st_disable_set(bool a_state) {
+	bool pin_state = a_state; // Keep enabled.
+	if (bit_istrue(settings.flags, BITFLAG_INVERT_ST_ENABLE)) {
+		pin_state = !pin_state;
+	} // Apply pin invert.
+	if (pin_state) {
+		STEPPERS_DISABLE_PORT |= (1 << STEPPERS_DISABLE_BIT);
+	} else {
+		STEPPERS_DISABLE_PORT &= ~(1 << STEPPERS_DISABLE_BIT);
+	}
+}
 
 /* "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Grbl. Grbl employs
    the venerable Bresenham line algorithm to manage and exactly synchronize multi-axis moves.
diff --git a/grbl/stepper.h b/grbl/stepper.h
index 41871a66..5e1a78a6 100644
--- a/grbl/stepper.h
+++ b/grbl/stepper.h
@@ -56,4 +56,7 @@ void st_update_plan_block_parameters();
 // Called by realtime status reporting if realtime rate reporting is enabled in config.h.
 float st_get_realtime_rate();
 
+// Disable stepper
+void st_disable_set(bool a_state) ;
+
 #endif