Fix endstop software and more

MK/Configuration_Store.cpp

@@ -45,7 +45,7 @@
  *  M666  XYZ             endstop_adj (x3)
  *  M666  R               delta_radius
  *  M666  D               delta_diagonal_rod
- *  M666  H               Z max_pos
+ *  M666  H               Z sw_endstop_max
  *  M666  ABCIJK          tower_adj (x6)
  *  M666  UVW             diagrod_adj (x3)
  *  M666  P XYZ           XYZ probe_offset (x3)
@@ -166,7 +166,7 @@ void Config_StoreSettings() {
     EEPROM_WRITE_VAR(i, endstop_adj);
     EEPROM_WRITE_VAR(i, delta_radius);
     EEPROM_WRITE_VAR(i, delta_diagonal_rod);
-    EEPROM_WRITE_VAR(i, max_pos);
+    EEPROM_WRITE_VAR(i, sw_endstop_max);
     EEPROM_WRITE_VAR(i, tower_adj);
     EEPROM_WRITE_VAR(i, diagrod_adj);
     EEPROM_WRITE_VAR(i, z_probe_offset);
@@ -315,7 +315,7 @@ void Config_RetrieveSettings() {
       EEPROM_READ_VAR(i, endstop_adj);
       EEPROM_READ_VAR(i, delta_radius);
       EEPROM_READ_VAR(i, delta_diagonal_rod);
-      EEPROM_READ_VAR(i, max_pos);
+      EEPROM_READ_VAR(i, sw_endstop_max);
       EEPROM_READ_VAR(i, tower_adj);
       EEPROM_READ_VAR(i, diagrod_adj);
       EEPROM_READ_VAR(i, z_probe_offset);
@@ -542,7 +542,6 @@ void Config_ResetDefault() {
     diagrod_adj[0] = TOWER_A_DIAGROD_ADJ;
     diagrod_adj[1] = TOWER_B_DIAGROD_ADJ;
     diagrod_adj[2] = TOWER_C_DIAGROD_ADJ;
-    max_pos[2] = MANUAL_Z_HOME_POS;
     z_probe_offset[0] = X_PROBE_OFFSET_FROM_EXTRUDER;
     z_probe_offset[1] = Y_PROBE_OFFSET_FROM_EXTRUDER;
     z_probe_offset[2] = Z_PROBE_OFFSET_FROM_EXTRUDER;
@@ -741,7 +740,7 @@ void Config_ResetDefault() {
       ECHO_MV(" W", diagrod_adj[2], 3);
       ECHO_MV(" R", delta_radius);
       ECHO_MV(" D", delta_diagonal_rod);
-      ECHO_EMV(" H", max_pos[2]);
+      ECHO_EMV(" H", sw_endstop_max[2]);
 
       if (!forReplay) {
         ECHO_LM(CFG, "Endstop Offsets:");

MK/module/MK_Main.h

@@ -128,8 +128,8 @@ extern float current_position[NUM_AXIS];
 extern float destination[NUM_AXIS];
 extern float home_offset[3];
 extern float hotend_offset[3][HOTENDS];
-extern float min_pos[3];
-extern float max_pos[3];
+extern float sw_endstop_min[3];
+extern float sw_endstop_max[3];
 extern float zprobe_zoffset;
 extern uint8_t axis_known_position;
 extern uint8_t axis_was_homed;

MK/module/macros.h

@@ -69,4 +69,8 @@
 // Macro for debugging
 #define DEBUGGING(F) (mk_debug_flags & (DEBUG_## F))
 
+// Macro for String
+#define STRINGIFY_(n) #n
+#define STRINGIFY(n) STRINGIFY_(n)
+
 #endif //__MACROS_H

MK/module/motion/planner.h

@@ -50,7 +50,7 @@
 // the source g-code and may never actually be reached if acceleration management is active.
 typedef struct {
   // Fields used by the bresenham algorithm for tracing the line
-  long steps[NUM_AXIS];                     // Step count along each axis
+  unsigned long steps[NUM_AXIS];                      // Step count along each axis
 
   #if ENABLED(COLOR_MIXING_EXTRUDER)
     unsigned long mix_event_count[DRIVER_EXTRUDERS];  // Step count for each stepper in a mixing extruder

MK/module/motion/stepper.cpp

@@ -56,7 +56,7 @@ static unsigned int cleaning_buffer_counter;
 #endif
 
 // Counter variables for the Bresenham line tracer
-static long counter_x, counter_y, counter_z, counter_e;
+static long counter_X, counter_Y, counter_Z, counter_E;
 volatile static unsigned long step_events_completed; // The number of step events executed in the current block
 
 #if ENABLED(ADVANCE)
@@ -657,7 +657,7 @@ ISR(TIMER1_COMPA_vect) {
 
       // Initialize Bresenham counters to 1/2 the ceiling
       long new_count = -(current_block->step_event_count >> 1);
-      counter_x = counter_y = counter_z = counter_e = new_count;
+      counter_X = counter_Y = counter_Z = counter_E = new_count;
 
       #if ENABLED(COLOR_MIXING_EXTRUDER)
         for (uint8_t i = 0; i < DRIVER_EXTRUDERS; i++)
@@ -694,9 +694,9 @@ ISR(TIMER1_COMPA_vect) {
         MKSERIAL.checkRx(); // Check for serial chars.
 
       #if ENABLED(ADVANCE)
-        counter_e += current_block->steps[E_AXIS];
-        if (counter_e > 0) {
-          counter_e -= current_block->step_event_count;
+        counter_E += current_block->steps[E_AXIS];
+        if (counter_E > 0) {
+          counter_E -= current_block->step_event_count;
           #if DISABLED(COLOR_MIXING_EXTRUDER)
             // Don't step E for mixing extruder
             e_steps[current_block->active_driver] += TEST(out_bits, E_AXIS) ? -1 : 1;
@@ -715,13 +715,13 @@ ISR(TIMER1_COMPA_vect) {
         #endif // !COLOR_MIXING_EXTRUDER
       #endif // ADVANCE
 
-      #define _COUNTER(axis) counter_## axis
+      #define _COUNTER(AXIS) counter_## AXIS
       #define _APPLY_STEP(AXIS) AXIS ##_APPLY_STEP
       #define _INVERT_STEP_PIN(AXIS) INVERT_## AXIS ##_STEP_PIN
 
-      #define STEP_START(axis, AXIS) \
-        _COUNTER(axis) += current_block->steps[_AXIS(AXIS)]; \
-        if (_COUNTER(axis) > 0) _APPLY_STEP(AXIS)(!_INVERT_STEP_PIN(AXIS),0);
+      #define STEP_START(AXIS) \
+        _COUNTER(AXIS) += current_block->steps[_AXIS(AXIS)]; \
+        if (_COUNTER(AXIS) > 0) _APPLY_STEP(AXIS)(!_INVERT_STEP_PIN(AXIS),0);
 
       #define STEP_START_MIXING \
         for (uint8_t j = 0; j < DRIVER_EXTRUDERS; j++) {  \
@@ -729,9 +729,9 @@ ISR(TIMER1_COMPA_vect) {
           if (counter_m[j] > 0) En_STEP_WRITE(j, !INVERT_E_STEP_PIN); \
         }
 
-      #define STEP_END(axis, AXIS) \
-        if (_COUNTER(axis) > 0) { \
-          _COUNTER(axis) -= current_block->step_event_count; \
+      #define STEP_END(AXIS) \
+        if (_COUNTER(AXIS) > 0) { \
+          _COUNTER(AXIS) -= current_block->step_event_count; \
           count_position[_AXIS(AXIS)] += count_direction[_AXIS(AXIS)]; \
           _APPLY_STEP(AXIS)(_INVERT_STEP_PIN(AXIS),0); \
         }
@@ -744,11 +744,11 @@ ISR(TIMER1_COMPA_vect) {
           } \
         }
 
-      STEP_START(x, X);
-      STEP_START(y, Y);
-      STEP_START(z, Z);
+      STEP_START(X);
+      STEP_START(Y);
+      STEP_START(Z);
       #if DISABLED(ADVANCE)
-        STEP_START(e, E);
+        STEP_START(E);
         #if ENABLED(COLOR_MIXING_EXTRUDER)
           STEP_START_MIXING;
         #endif
@@ -758,11 +758,11 @@ ISR(TIMER1_COMPA_vect) {
         HAL::delayMicroseconds(STEPPER_HIGH_LOW_DELAY);
       #endif
 
-      STEP_END(x, X);
-      STEP_END(y, Y);
-      STEP_END(z, Z);
+      STEP_END(X);
+      STEP_END(Y);
+      STEP_END(Z);
       #if DISABLED(ADVANCE)
-        STEP_END(e, E);
+        STEP_END(E);
         #if ENABLED(COLOR_MIXING_EXTRUDER)
           STEP_END_MIXING;
         #endif
@@ -1187,11 +1187,45 @@ void st_init() {
  */
 void st_synchronize() { while (blocks_queued()) idle(); }
 
+/**
+ * Set the stepper positions directly in steps
+ *
+ * The input is based on the typical per-axis XYZ steps.
+ * For CORE machines XYZ needs to be translated to ABC.
+ *
+ * This allows get_axis_position_mm to correctly
+ * derive the current XYZ position later on.
+ */
 void st_set_position(const long& x, const long& y, const long& z, const long& e) {
   CRITICAL_SECTION_START;
-  count_position[X_AXIS] = x;
-  count_position[Y_AXIS] = y;
-  count_position[Z_AXIS] = z;
+
+  #if MECH(COREXY)
+    // corexy positioning
+    count_position[A_AXIS] = x + COREX_YZ_FACTOR * y;
+    count_position[B_AXIS] = x - COREX_YZ_FACTOR * y;
+    count_position[Z_AXIS] = z;
+  #elif MECH(COREYX)
+    // coreyx positioning
+    count_position[A_AXIS] = y + COREX_YZ_FACTOR * x;
+    count_position[B_AXIS] = y - COREX_YZ_FACTOR * x;
+    count_position[Z_AXIS] = z;
+  #elif MECH(COREXZ)
+    // corexz planning
+    count_position[A_AXIS] = x + COREX_YZ_FACTOR * z;
+    count_position[Y_AXIS] = y;
+    count_position[C_AXIS] = x - COREX_YZ_FACTOR * z;
+  #elif MECH(COREZX)
+    // corezx planning
+    count_position[A_AXIS] = z + COREX_YZ_FACTOR * x;
+    count_position[Y_AXIS] = y;
+    count_position[C_AXIS] = z - COREX_YZ_FACTOR * x;
+  #else
+    // default non-h-bot planning
+    count_position[X_AXIS] = x;
+    count_position[Y_AXIS] = y;
+    count_position[Z_AXIS] = z;
+  #endif
+
   count_position[E_AXIS] = e;
   CRITICAL_SECTION_END;
 }
@@ -1263,6 +1297,43 @@ void quickStop() {
   ENABLE_STEPPER_DRIVER_INTERRUPT();
 }
 
+void report_positions() {
+  CRITICAL_SECTION_START;
+  long xpos = count_position[X_AXIS],
+       ypos = count_position[Y_AXIS],
+       zpos = count_position[Z_AXIS];
+  CRITICAL_SECTION_END;
+
+  #if MECH(COREXY) || MECH(COREYX) || MECH(COREXZ) || MECH(COREZX)
+    ECHO_M(SERIAL_COUNT_A);
+  #elif MECH(DELTA)
+    ECHO_M(SERIAL_COUNT_ALPHA);
+  #else
+    ECHO_M(SERIAL_COUNT_X);
+  #endif
+  ECHO_V(xpos);
+
+  #if MECH(COREXY) || MECH(COREYX)
+    ECHO_M(" B:");
+  #elif MECH(DELTA)
+    ECHO_M(" Beta:");
+  #else
+    ECHO_M(" Y:");
+  #endif
+  ECHO_V(ypos);
+
+  #if MECH(COREXZ) || MECH(COREZX)
+    ECHO_M(" C:");
+  #elif MECH(DELTA)
+    ECHO_M(" Teta:");
+  #else
+    ECHO_M(" Z:");
+  #endif
+  ECHO_V(zpos);
+
+  ECHO_E;
+}
+
 #if ENABLED(NPR2)
   void colorstep(long csteps,const bool direction) {
     enable_e1();

MK/module/motion/stepper.h

@@ -64,6 +64,11 @@
   // to notify the subsystem that it is time to go to work.
   void st_wake_up();
 
+  //
+  // Report the positions of the steppers, in steps
+  //
+  void report_positions();
+
   void checkHitEndstops(); //call from somewhere to create an serial error message with the locations the endstops where hit, in case they were triggered
   void endstops_hit_on_purpose(); //avoid creation of the message, i.e. after homing and before a routine call of checkHitEndstops();