From 632781c4a2a5b8f33eb652324259dccaed73452a Mon Sep 17 00:00:00 2001
From: MRprusa3d <martin.remis@prusa3d.cz>
Date: Thu, 21 Mar 2019 00:45:15 +0100
Subject: [PATCH] "disable_z()" for Delta PSU

---
 Firmware/Marlin.h                             |  6 +++
 Firmware/Marlin_main.cpp                      | 43 +++++++++++++++++++
 .../variants/1_75mm_MK3S-EINSy10a-E3Dv6full.h |  3 ++
 3 files changed, 52 insertions(+)

diff --git a/Firmware/Marlin.h b/Firmware/Marlin.h
index 2477d1c21..9ccb81c6f 100644
--- a/Firmware/Marlin.h
+++ b/Firmware/Marlin.h
@@ -172,6 +172,12 @@ void manage_inactivity(bool ignore_stepper_queue=false);
   #define disable_z() {}
 #endif
 
+#ifdef PSU_Delta
+#undef disable_z()
+#define disable_z() disable_force_z()
+void disable_force_z();
+#endif // PSU_Delta
+
 
 
 
diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index 7be151f9f..303d46934 100644
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -9602,3 +9602,46 @@ if((eSoundMode==e_SOUND_MODE_LOUD)||(eSoundMode==e_SOUND_MODE_ONCE))
 }
 
 #define FIL_LOAD_LENGTH 60
+
+#ifdef PSU_Delta
+void disable_force_z()
+{
+uint16_t z_microsteps=0;
+
+#ifdef TMC2130
+z_microsteps=tmc2130_rd_MSCNT(Z_TMC2130_CS);
+#endif //TMC2130
+/*
+//planner_abort_hard();
+//sei();
+plan_buffer_line(
+     current_position[X_AXIS]+10, 
+     current_position[Y_AXIS]+20, 
+//     current_position[Z_AXIS]+float((1024-z_microsteps+7)>>4)/axis_steps_per_unit[Z_AXIS], 
+current_position[Z_AXIS]+0.026, //0.025, 
+     current_position[E_AXIS],
+     40, active_extruder);
+st_synchronize();
+ z_microsteps=tmc2130_rd_MSCNT(Z_TMC2130_CS);
+ MYSERIAL.println(z_microsteps,DEC);
+
+  MYSERIAL.println(current_position[Z_AXIS]+float((1024-z_microsteps+7)>>4)/cs.axis_steps_per_unit[Z_AXIS],DEC);
+  MYSERIAL.println(current_position[Z_AXIS],DEC);
+  MYSERIAL.println(z_microsteps,DEC);
+  MYSERIAL.println(cs.axis_steps_per_unit[Z_AXIS],DEC);
+  MYSERIAL.println(float((1024-z_microsteps+7)>>4)/cs.axis_steps_per_unit[Z_AXIS],DEC);
+*/
+planner_abort_hard();
+sei();
+plan_buffer_line(
+     current_position[X_AXIS], 
+     current_position[Y_AXIS], 
+     current_position[Z_AXIS]+float((1024-z_microsteps+7)>>4)/cs.axis_steps_per_unit[Z_AXIS], 
+     current_position[E_AXIS],
+     40, active_extruder);
+st_synchronize();
+
+WRITE(Z_ENABLE_PIN,!Z_ENABLE_ON);
+axis_known_position[Z_AXIS]=false; 
+}
+#endif // PSU_Delta
diff --git a/Firmware/variants/1_75mm_MK3S-EINSy10a-E3Dv6full.h b/Firmware/variants/1_75mm_MK3S-EINSy10a-E3Dv6full.h
index 1c0c6d28e..9f82e79d8 100644
--- a/Firmware/variants/1_75mm_MK3S-EINSy10a-E3Dv6full.h
+++ b/Firmware/variants/1_75mm_MK3S-EINSy10a-E3Dv6full.h
@@ -22,6 +22,9 @@
 #define STEEL_SHEET
 #define HAS_SECOND_SERIAL_PORT
 
+// PSU
+#define PSU_Delta                                 // uncomment if DeltaElectronics PSU installed
+
 
 // Uncomment the below for the E3D PT100 temperature sensor (with or without PT100 Amplifier)
 //#define E3D_PT100_EXTRUDER_WITH_AMP