Merge pull request #506 from codexmas/ServoEndstops

Servo Enstops working!

Marlin/Configuration.h

@@ -470,7 +470,15 @@ const bool Z_ENDSTOPS_INVERTING = true; // set to true to invert the logic of th
 // leaving it undefined or defining as 0 will disable the servo subsystem
 // If unsure, leave commented / disabled
 //
-// #define NUM_SERVOS 3
+//#define NUM_SERVOS 3 // Servo index starts with 0
+
+// Servo Endstops
+// 
+// This allows for servo actuated endstops, primary usage is for the Z Axis to eliminate calibration or bed height changes.
+// Use M206 command to correct for switch height offset to actual nozzle height. Store that setting with M500.
+// 
+//#define SERVO_ENDSTOPS {-1, -1, 0} // Servo index for X, Y, Z. Disable with -1
+//#define SERVO_ENDSTOP_ANGLES {0,0, 0,0, 70,0} // X,Y,Z Axis Extend and Retract angles
 
 #include "Configuration_adv.h"
 #include "thermistortables.h"

Marlin/Marlin_main.cpp

@@ -177,6 +177,10 @@ float extruder_offset[2][EXTRUDERS] = {
 #endif
 uint8_t active_extruder = 0;
 int fanSpeed=0;
+#ifdef SERVO_ENDSTOPS
+  int servo_endstops[] = SERVO_ENDSTOPS;
+  int servo_endstop_angles[] = SERVO_ENDSTOP_ANGLES;
+#endif
 #ifdef BARICUDA
 int ValvePressure=0;
 int EtoPPressure=0;
@@ -351,6 +355,16 @@ void servo_init()
   #if (NUM_SERVOS >= 5)
     #error "TODO: enter initalisation code for more servos"
   #endif
+
+  // Set position of Servo Endstops that are defined
+  #ifdef SERVO_ENDSTOPS
+  for(int8_t i = 0; i < 3; i++)
+  {
+    if(servo_endstops[i] > -1) {
+      servos[servo_endstops[i]].write(servo_endstop_angles[i * 2 + 1]);
+    }
+  }
+  #endif
 }
 
 void setup()
@@ -664,11 +678,16 @@ static void axis_is_at_home(int axis) {
 static void homeaxis(int axis) {
 #define HOMEAXIS_DO(LETTER) \
   ((LETTER##_MIN_PIN > -1 && LETTER##_HOME_DIR==-1) || (LETTER##_MAX_PIN > -1 && LETTER##_HOME_DIR==1))
-
   if (axis==X_AXIS ? HOMEAXIS_DO(X) :
       axis==Y_AXIS ? HOMEAXIS_DO(Y) :
       axis==Z_AXIS ? HOMEAXIS_DO(Z) :
       0) {
+
+    // Engage Servo endstop if enabled
+    #ifdef SERVO_ENDSTOPS[axis] > -1
+      servos[servo_endstops[axis]].write(servo_endstop_angles[axis * 2]);
+    #endif
+
     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) * home_dir(axis);
@@ -691,6 +710,11 @@ static void homeaxis(int axis) {
     destination[axis] = current_position[axis];
     feedrate = 0.0;
     endstops_hit_on_purpose();
+
+    // Retract Servo endstop if enabled
+    #ifdef SERVO_ENDSTOPS[axis] > -1
+      servos[servo_endstops[axis]].write(servo_endstop_angles[axis * 2 + 1]);
+    #endif
   }
 }
 #define HOMEAXIS(LETTER) homeaxis(LETTER##_AXIS)
@@ -2281,4 +2305,4 @@ bool setTargetedHotend(int code){
     }
   }
   return false;
-}
+}

Marlin/Servo.cpp

@@ -41,6 +41,8 @@
  detach()    - Stops an attached servos from pulsing its i/o pin.
 
 */
+#include "Configuration.h" 
+
 #ifdef NUM_SERVOS
 #include <avr/interrupt.h>
 #include <Arduino.h>

Marlin/pins.h

@@ -391,7 +391,7 @@
       #define SERVO2_PIN         5
     #endif
 
-    #if NUM_SERVOS > 2
+    #if NUM_SERVOS > 3
       #define SERVO3_PIN         4
     #endif
   #endif
@@ -1846,4 +1846,4 @@
                         HEATER_BED_PIN, FAN_PIN,                  \
                         _E0_PINS _E1_PINS _E2_PINS             \
                         analogInputToDigitalPin(TEMP_0_PIN), analogInputToDigitalPin(TEMP_1_PIN), analogInputToDigitalPin(TEMP_2_PIN), analogInputToDigitalPin(TEMP_BED_PIN) }
-#endif
+#endif