From 0af2a80e068008b772f545456bc5774ef127c9f2 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Tue, 28 Jul 2015 19:22:44 -0700
Subject: [PATCH 1/2] Make SERVO_ENDSTOP_ANGLES 2-dimensional

---
 Marlin/Configuration.h                                 |  2 +-
 Marlin/Marlin_main.cpp                                 | 10 +++++-----
 Marlin/configurator/config/Configuration.h             |  2 +-
 Marlin/example_configurations/Felix/Configuration.h    |  2 +-
 .../example_configurations/Felix/Configuration_DUAL.h  |  2 +-
 .../example_configurations/Hephestos/Configuration.h   |  2 +-
 Marlin/example_configurations/K8200/Configuration.h    |  2 +-
 .../RepRapWorld/Megatronics/Configuration.h            |  2 +-
 Marlin/example_configurations/RigidBot/Configuration.h |  2 +-
 Marlin/example_configurations/SCARA/Configuration.h    |  2 +-
 Marlin/example_configurations/WITBOX/Configuration.h   |  2 +-
 .../adafruit/ST7565/Configuration.h                    |  2 +-
 .../delta/biv2.5/Configuration.h                       |  2 +-
 .../delta/generic/Configuration.h                      |  2 +-
 .../delta/kossel_mini/Configuration.h                  |  2 +-
 .../delta/kossel_pro/Configuration.h                   |  2 +-
 Marlin/example_configurations/makibox/Configuration.h  |  2 +-
 .../tvrrug/Round2/Configuration.h                      |  2 +-
 18 files changed, 22 insertions(+), 22 deletions(-)

diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h
index 312ffd4ae8..87578f8055 100644
--- a/Marlin/Configuration.h
+++ b/Marlin/Configuration.h
@@ -789,7 +789,7 @@ const bool Z_PROBE_ENDSTOP_INVERTING = false; // set to true to invert the logic
 //#define X_ENDSTOP_SERVO_NR 1
 //#define Y_ENDSTOP_SERVO_NR 2
 //#define Z_ENDSTOP_SERVO_NR 0
-//#define SERVO_ENDSTOP_ANGLES {0,0, 0,0, 70,0} // X,Y,Z Axis Extend and Retract angles
+//#define SERVO_ENDSTOP_ANGLES {{0,0}, {0,0}, {70,0}} // X,Y,Z Axis Extend and Retract angles
 
 // Servo deactivation
 //
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 5cb9aa01cc..3bcb7655f8 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -316,7 +316,7 @@ bool target_direction;
 
 #ifdef SERVO_ENDSTOPS
   const int servo_endstops[] = SERVO_ENDSTOPS;
-  const int servo_endstop_angles[] = SERVO_ENDSTOP_ANGLES;
+  const int servo_endstop_angles[][] = SERVO_ENDSTOP_ANGLES;
 #endif
 
 #ifdef BARICUDA
@@ -581,7 +581,7 @@ void servo_init() {
   #ifdef SERVO_ENDSTOPS
     for (int i = 0; i < 3; i++)
       if (servo_endstops[i] >= 0)
-        servo[servo_endstops[i]].move(servo_endstop_angles[i * 2 + 1]);
+        servo[servo_endstops[i]].move(servo_endstop_angles[i][1]);
   #endif
 
 }
@@ -1325,7 +1325,7 @@ static void setup_for_endstop_move() {
     #ifdef SERVO_ENDSTOPS
 
       // Engage Z Servo endstop if enabled
-      if (servo_endstops[Z_AXIS] >= 0) servo[servo_endstops[Z_AXIS]].move(servo_endstop_angles[Z_AXIS * 2]);
+      if (servo_endstops[Z_AXIS] >= 0) servo[servo_endstops[Z_AXIS]].move(servo_endstop_angles[Z_AXIS][0]);
 
     #elif defined(Z_PROBE_ALLEN_KEY)
       feedrate = Z_PROBE_ALLEN_KEY_DEPLOY_1_FEEDRATE;
@@ -1425,7 +1425,7 @@ static void setup_for_endstop_move() {
         #endif
 
         // Change the Z servo angle
-        servo[servo_endstops[Z_AXIS]].move(servo_endstop_angles[Z_AXIS * 2 + 1]);
+        servo[servo_endstops[Z_AXIS]].move(servo_endstop_angles[Z_AXIS][1]);
       }
 
     #elif defined(Z_PROBE_ALLEN_KEY)
@@ -1783,7 +1783,7 @@ static void homeaxis(AxisEnum axis) {
       #ifdef SERVO_ENDSTOPS
         // Retract Servo endstop if enabled
         if (servo_endstops[axis] >= 0)
-          servo[servo_endstops[axis]].move(servo_endstop_angles[axis * 2 + 1]);
+          servo[servo_endstops[axis]].move(servo_endstop_angles[axis][1]);
       #endif
     }
 
diff --git a/Marlin/configurator/config/Configuration.h b/Marlin/configurator/config/Configuration.h
index 312ffd4ae8..87578f8055 100644
--- a/Marlin/configurator/config/Configuration.h
+++ b/Marlin/configurator/config/Configuration.h
@@ -789,7 +789,7 @@ const bool Z_PROBE_ENDSTOP_INVERTING = false; // set to true to invert the logic
 //#define X_ENDSTOP_SERVO_NR 1
 //#define Y_ENDSTOP_SERVO_NR 2
 //#define Z_ENDSTOP_SERVO_NR 0
-//#define SERVO_ENDSTOP_ANGLES {0,0, 0,0, 70,0} // X,Y,Z Axis Extend and Retract angles
+//#define SERVO_ENDSTOP_ANGLES {{0,0}, {0,0}, {70,0}} // X,Y,Z Axis Extend and Retract angles
 
 // Servo deactivation
 //
diff --git a/Marlin/example_configurations/Felix/Configuration.h b/Marlin/example_configurations/Felix/Configuration.h
index 6fe4c11059..0c85394f17 100644
--- a/Marlin/example_configurations/Felix/Configuration.h
+++ b/Marlin/example_configurations/Felix/Configuration.h
@@ -767,7 +767,7 @@ const bool Z_PROBE_ENDSTOP_INVERTING = false; // set to true to invert the logic
 //#define X_ENDSTOP_SERVO_NR 1
 //#define Y_ENDSTOP_SERVO_NR 2
 //#define Z_ENDSTOP_SERVO_NR 0
-//#define SERVO_ENDSTOP_ANGLES {0,0, 0,0, 70,0} // X,Y,Z Axis Extend and Retract angles
+//#define SERVO_ENDSTOP_ANGLES {{0,0}, {0,0}, {70,0}} // X,Y,Z Axis Extend and Retract angles
 
 // Servo deactivation
 //
diff --git a/Marlin/example_configurations/Felix/Configuration_DUAL.h b/Marlin/example_configurations/Felix/Configuration_DUAL.h
index ae0fecb59f..93f5ca27bc 100644
--- a/Marlin/example_configurations/Felix/Configuration_DUAL.h
+++ b/Marlin/example_configurations/Felix/Configuration_DUAL.h
@@ -719,7 +719,7 @@ const bool Z_PROBE_ENDSTOP_INVERTING = false; // set to true to invert the logic
 //#define X_ENDSTOP_SERVO_NR 1
 //#define Y_ENDSTOP_SERVO_NR 2
 //#define Z_ENDSTOP_SERVO_NR 0
-//#define SERVO_ENDSTOP_ANGLES {0,0, 0,0, 70,0} // X,Y,Z Axis Extend and Retract angles
+//#define SERVO_ENDSTOP_ANGLES {{0,0}, {0,0}, {70,0}} // X,Y,Z Axis Extend and Retract angles
 
 // Servo deactivation
 //
diff --git a/Marlin/example_configurations/Hephestos/Configuration.h b/Marlin/example_configurations/Hephestos/Configuration.h
index 79ba580b21..349a1edf3e 100644
--- a/Marlin/example_configurations/Hephestos/Configuration.h
+++ b/Marlin/example_configurations/Hephestos/Configuration.h
@@ -776,7 +776,7 @@ const bool Z_PROBE_ENDSTOP_INVERTING = true; // set to true to invert the logic
 //#define X_ENDSTOP_SERVO_NR 1
 //#define Y_ENDSTOP_SERVO_NR 2
 //#define Z_ENDSTOP_SERVO_NR 0
-//#define SERVO_ENDSTOP_ANGLES {0,0, 0,0, 70,0} // X,Y,Z Axis Extend and Retract angles
+//#define SERVO_ENDSTOP_ANGLES {{0,0}, {0,0}, {70,0}} // X,Y,Z Axis Extend and Retract angles
 
 // Servo deactivation
 //
diff --git a/Marlin/example_configurations/K8200/Configuration.h b/Marlin/example_configurations/K8200/Configuration.h
index 65ea729aae..b41470df35 100644
--- a/Marlin/example_configurations/K8200/Configuration.h
+++ b/Marlin/example_configurations/K8200/Configuration.h
@@ -772,7 +772,7 @@ const bool Z_PROBE_ENDSTOP_INVERTING = false; // set to true to invert the logic
 //#define X_ENDSTOP_SERVO_NR 1
 //#define Y_ENDSTOP_SERVO_NR 2
 //#define Z_ENDSTOP_SERVO_NR 0
-//#define SERVO_ENDSTOP_ANGLES {0,0, 0,0, 70,0} // X,Y,Z Axis Extend and Retract angles
+//#define SERVO_ENDSTOP_ANGLES {{0,0}, {0,0}, {70,0}} // X,Y,Z Axis Extend and Retract angles
 
 // Servo deactivation
 //
diff --git a/Marlin/example_configurations/RepRapWorld/Megatronics/Configuration.h b/Marlin/example_configurations/RepRapWorld/Megatronics/Configuration.h
index 0e06425669..24c2108caf 100644
--- a/Marlin/example_configurations/RepRapWorld/Megatronics/Configuration.h
+++ b/Marlin/example_configurations/RepRapWorld/Megatronics/Configuration.h
@@ -784,7 +784,7 @@ const bool Z_PROBE_ENDSTOP_INVERTING = false; // set to true to invert the logic
 //#define X_ENDSTOP_SERVO_NR 1
 //#define Y_ENDSTOP_SERVO_NR 2
 //#define Z_ENDSTOP_SERVO_NR 0
-//#define SERVO_ENDSTOP_ANGLES {0,0, 0,0, 70,0} // X,Y,Z Axis Extend and Retract angles
+//#define SERVO_ENDSTOP_ANGLES {{0,0}, {0,0}, {70,0}} // X,Y,Z Axis Extend and Retract angles
 
 // Servo deactivation
 //
diff --git a/Marlin/example_configurations/RigidBot/Configuration.h b/Marlin/example_configurations/RigidBot/Configuration.h
index 79db5dc172..6ec2b15d0e 100644
--- a/Marlin/example_configurations/RigidBot/Configuration.h
+++ b/Marlin/example_configurations/RigidBot/Configuration.h
@@ -767,7 +767,7 @@ const bool Z_PROBE_ENDSTOP_INVERTING = false; // set to true to invert the logic
 //#define X_ENDSTOP_SERVO_NR 1
 //#define Y_ENDSTOP_SERVO_NR 2
 //#define Z_ENDSTOP_SERVO_NR 0
-//#define SERVO_ENDSTOP_ANGLES {0,0, 0,0, 70,0} // X,Y,Z Axis Extend and Retract angles
+//#define SERVO_ENDSTOP_ANGLES {{0,0}, {0,0}, {70,0}} // X,Y,Z Axis Extend and Retract angles
 
 // Servo deactivation
 //
diff --git a/Marlin/example_configurations/SCARA/Configuration.h b/Marlin/example_configurations/SCARA/Configuration.h
index 87784198de..5ee968a188 100644
--- a/Marlin/example_configurations/SCARA/Configuration.h
+++ b/Marlin/example_configurations/SCARA/Configuration.h
@@ -792,7 +792,7 @@ const bool Z_PROBE_ENDSTOP_INVERTING = false; // set to true to invert the logic
 //#define X_ENDSTOP_SERVO_NR 1
 //#define Y_ENDSTOP_SERVO_NR 2
 //#define Z_ENDSTOP_SERVO_NR 0
-//#define SERVO_ENDSTOP_ANGLES {0,0, 0,0, 70,0} // X,Y,Z Axis Extend and Retract angles
+//#define SERVO_ENDSTOP_ANGLES {{0,0}, {0,0}, {70,0}} // X,Y,Z Axis Extend and Retract angles
 
 // Servo deactivation
 //
diff --git a/Marlin/example_configurations/WITBOX/Configuration.h b/Marlin/example_configurations/WITBOX/Configuration.h
index 6d8a332e95..ec36541dbb 100644
--- a/Marlin/example_configurations/WITBOX/Configuration.h
+++ b/Marlin/example_configurations/WITBOX/Configuration.h
@@ -776,7 +776,7 @@ const bool Z_PROBE_ENDSTOP_INVERTING = true; // set to true to invert the logic
 //#define X_ENDSTOP_SERVO_NR 1
 //#define Y_ENDSTOP_SERVO_NR 2
 //#define Z_ENDSTOP_SERVO_NR 0
-//#define SERVO_ENDSTOP_ANGLES {0,0, 0,0, 70,0} // X,Y,Z Axis Extend and Retract angles
+//#define SERVO_ENDSTOP_ANGLES {{0,0}, {0,0}, {70,0}} // X,Y,Z Axis Extend and Retract angles
 
 // Servo deactivation
 //
diff --git a/Marlin/example_configurations/adafruit/ST7565/Configuration.h b/Marlin/example_configurations/adafruit/ST7565/Configuration.h
index b4dc32ddb5..b2d56fea88 100644
--- a/Marlin/example_configurations/adafruit/ST7565/Configuration.h
+++ b/Marlin/example_configurations/adafruit/ST7565/Configuration.h
@@ -784,7 +784,7 @@ const bool Z_PROBE_ENDSTOP_INVERTING = false; // set to true to invert the logic
 //#define X_ENDSTOP_SERVO_NR 1
 //#define Y_ENDSTOP_SERVO_NR 2
 //#define Z_ENDSTOP_SERVO_NR 0
-//#define SERVO_ENDSTOP_ANGLES {0,0, 0,0, 70,0} // X,Y,Z Axis Extend and Retract angles
+//#define SERVO_ENDSTOP_ANGLES {{0,0}, {0,0}, {70,0}} // X,Y,Z Axis Extend and Retract angles
 
 // Servo deactivation
 //
diff --git a/Marlin/example_configurations/delta/biv2.5/Configuration.h b/Marlin/example_configurations/delta/biv2.5/Configuration.h
index cdf403873d..a9a7d8ba0c 100644
--- a/Marlin/example_configurations/delta/biv2.5/Configuration.h
+++ b/Marlin/example_configurations/delta/biv2.5/Configuration.h
@@ -907,7 +907,7 @@ const bool Z_PROBE_ENDSTOP_INVERTING = true; // set to true to invert the logic
 //#define X_ENDSTOP_SERVO_NR 1
 //#define Y_ENDSTOP_SERVO_NR 2
 //#define Z_ENDSTOP_SERVO_NR 0
-//#define SERVO_ENDSTOP_ANGLES {0,0, 0,0, 70,0} // X,Y,Z Axis Extend and Retract angles
+//#define SERVO_ENDSTOP_ANGLES {{0,0}, {0,0}, {70,0}} // X,Y,Z Axis Extend and Retract angles
 
 // Servo deactivation
 //
diff --git a/Marlin/example_configurations/delta/generic/Configuration.h b/Marlin/example_configurations/delta/generic/Configuration.h
index 2f5a177a8f..6908ea9e47 100644
--- a/Marlin/example_configurations/delta/generic/Configuration.h
+++ b/Marlin/example_configurations/delta/generic/Configuration.h
@@ -911,7 +911,7 @@ const bool Z_PROBE_ENDSTOP_INVERTING = true; // set to true to invert the logic
 //#define X_ENDSTOP_SERVO_NR 1
 //#define Y_ENDSTOP_SERVO_NR 2
 //#define Z_ENDSTOP_SERVO_NR 0
-//#define SERVO_ENDSTOP_ANGLES {0,0, 0,0, 70,0} // X,Y,Z Axis Extend and Retract angles
+//#define SERVO_ENDSTOP_ANGLES {{0,0}, {0,0}, {70,0}} // X,Y,Z Axis Extend and Retract angles
 
 // Servo deactivation
 //
diff --git a/Marlin/example_configurations/delta/kossel_mini/Configuration.h b/Marlin/example_configurations/delta/kossel_mini/Configuration.h
index 68373974e1..fec90f295d 100644
--- a/Marlin/example_configurations/delta/kossel_mini/Configuration.h
+++ b/Marlin/example_configurations/delta/kossel_mini/Configuration.h
@@ -911,7 +911,7 @@ const bool Z_PROBE_ENDSTOP_INVERTING = false; // set to true to invert the logic
 //#define X_ENDSTOP_SERVO_NR 1
 //#define Y_ENDSTOP_SERVO_NR 2
 //#define Z_ENDSTOP_SERVO_NR 0
-//#define SERVO_ENDSTOP_ANGLES {0,0, 0,0, 70,0} // X,Y,Z Axis Extend and Retract angles
+//#define SERVO_ENDSTOP_ANGLES {{0,0}, {0,0}, {70,0}} // X,Y,Z Axis Extend and Retract angles
 
 // Servo deactivation
 //
diff --git a/Marlin/example_configurations/delta/kossel_pro/Configuration.h b/Marlin/example_configurations/delta/kossel_pro/Configuration.h
index 5a88687056..6ba3f9af4d 100644
--- a/Marlin/example_configurations/delta/kossel_pro/Configuration.h
+++ b/Marlin/example_configurations/delta/kossel_pro/Configuration.h
@@ -906,7 +906,7 @@ const bool Z_PROBE_ENDSTOP_INVERTING = false; // set to true to invert the logic
 //#define X_ENDSTOP_SERVO_NR 1
 //#define Y_ENDSTOP_SERVO_NR 2
 //#define Z_ENDSTOP_SERVO_NR 0
-//#define SERVO_ENDSTOP_ANGLES {0,0, 0,0, 70,0} // X,Y,Z Axis Extend and Retract angles
+//#define SERVO_ENDSTOP_ANGLES {{0,0}, {0,0}, {70,0}} // X,Y,Z Axis Extend and Retract angles
 
 // Servo deactivation
 //
diff --git a/Marlin/example_configurations/makibox/Configuration.h b/Marlin/example_configurations/makibox/Configuration.h
index 8041ad226f..eb425026ae 100644
--- a/Marlin/example_configurations/makibox/Configuration.h
+++ b/Marlin/example_configurations/makibox/Configuration.h
@@ -787,7 +787,7 @@ const bool Z_PROBE_ENDSTOP_INVERTING = false; // set to true to invert the logic
 //#define X_ENDSTOP_SERVO_NR 1
 //#define Y_ENDSTOP_SERVO_NR 2
 //#define Z_ENDSTOP_SERVO_NR 0
-//#define SERVO_ENDSTOP_ANGLES {0,0, 0,0, 70,0} // X,Y,Z Axis Extend and Retract angles
+//#define SERVO_ENDSTOP_ANGLES {{0,0}, {0,0}, {70,0}} // X,Y,Z Axis Extend and Retract angles
 
 // Servo deactivation
 //
diff --git a/Marlin/example_configurations/tvrrug/Round2/Configuration.h b/Marlin/example_configurations/tvrrug/Round2/Configuration.h
index f61b2bf528..cd60724141 100644
--- a/Marlin/example_configurations/tvrrug/Round2/Configuration.h
+++ b/Marlin/example_configurations/tvrrug/Round2/Configuration.h
@@ -778,7 +778,7 @@ const bool Z_PROBE_ENDSTOP_INVERTING = true; // set to true to invert the logic
 //#define X_ENDSTOP_SERVO_NR 1
 //#define Y_ENDSTOP_SERVO_NR 2
 //#define Z_ENDSTOP_SERVO_NR 0
-//#define SERVO_ENDSTOP_ANGLES {0,0, 0,0, 70,0} // X,Y,Z Axis Extend and Retract angles
+//#define SERVO_ENDSTOP_ANGLES {{0,0}, {0,0}, {70,0}} // X,Y,Z Axis Extend and Retract angles
 
 // Servo deactivation
 //

From 3b2b2313ff63ca29c958b19c184ef307d6eeeabc Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Tue, 28 Jul 2015 19:23:16 -0700
Subject: [PATCH 2/2] Combine if block related to SERVO_ENDSTOPS

---
 Marlin/Marlin_main.cpp | 8 +++-----
 1 file changed, 3 insertions(+), 5 deletions(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 3bcb7655f8..7f7e623d18 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -1677,11 +1677,9 @@ static void homeaxis(AxisEnum axis) {
     #endif
 
     #ifdef SERVO_ENDSTOPS
-      if (axis != Z_AXIS) {
-        // Engage Servo endstop if enabled
-        if (servo_endstops[axis] >= 0) 
-          servo[servo_endstops[axis]].move(servo_endstop_angles[axis * 2]);
-      }
+      // Engage Servo endstop if enabled
+      if (axis != Z_AXIS && servo_endstops[axis] >= 0)
+        servo[servo_endstops[axis]].move(servo_endstop_angles[axis][0]);
     #endif
 
     // Set a flag for Z motor locking