From 2ddb2a2be9a2aeaac0e2f811d384710c3a969a44 Mon Sep 17 00:00:00 2001
From: AnHardt <github@kitelab.de>
Date: Sun, 12 Jul 2015 23:02:47 +0200
Subject: [PATCH] Add Servo::move() to servo.cpp

move(pin, angel) - Sequence of attach(pin), write(angel),
                   if DEACTIVATE_SERVOS_AFTER_MOVE is defined waits SERVO_DEACTIVATION_DELAY, than detaches.

As we have jitter on the servos during moves of the steppers, and detaching them improves this behaviour,
the usual sequence to handle a servo movement is:
attach(pin)
write(angel)
delay(until the servo finished the move)
detache()

Here a function to handle the complete sequence.
---
 Marlin/servo.cpp | 17 ++++++++++++++++-
 Marlin/servo.h   |  5 +++++
 2 files changed, 21 insertions(+), 1 deletion(-)

diff --git a/Marlin/servo.cpp b/Marlin/servo.cpp
index 32e5d8493..6e3f398ce 100644
--- a/Marlin/servo.cpp
+++ b/Marlin/servo.cpp
@@ -35,12 +35,14 @@
 
  write()     - Sets the servo angle in degrees.  (invalid angle that is valid as pulse in microseconds is treated as microseconds)
  writeMicroseconds() - Sets the servo pulse width in microseconds
+ move(pin, angel) - Sequence of attach(pin), write(angel),
+                    if DEACTIVATE_SERVOS_AFTER_MOVE is defined waits SERVO_DEACTIVATION_DELAY, than detaches.
  read()      - Gets the last written servo pulse width as an angle between 0 and 180.
  readMicroseconds()   - Gets the last written servo pulse width in microseconds. (was read_us() in first release)
  attached()  - Returns true if there is a servo attached.
  detach()    - Stops an attached servos from pulsing its i/o pin.
 
-*/
+ */
 #include "Configuration.h" 
 
 #ifdef NUM_SERVOS
@@ -301,4 +303,17 @@ int Servo::readMicroseconds() {
 
 bool Servo::attached() { return servos[this->servoIndex].Pin.isActive; }
 
+uint8_t Servo::move(int pin, int value) {
+  uint8_t ret;
+  ret = this->attach(pin);
+  if (ret) {
+    this->write(value);
+    #ifdef DEACTIVATE_SERVOS_AFTER_MOVE && (SERVO_DEACTIVATION_DELAY > 0)
+      delay(SERVO_DEACTIVATION_DELAY);
+      this->detach();
+    #endif
+  }
+  return ret;
+}
+
 #endif
diff --git a/Marlin/servo.h b/Marlin/servo.h
index 464756175..4c58991c7 100644
--- a/Marlin/servo.h
+++ b/Marlin/servo.h
@@ -40,6 +40,8 @@
    readMicroseconds()   - Gets the last written servo pulse width in microseconds. (was read_us() in first release)
    attached()  - Returns true if there is a servo attached.
    detach()    - Stops an attached servos from pulsing its i/o pin.
+   move(pin, angel) - Sequence of attach(pin), write(angel),
+                      if DEACTIVATE_SERVOS_AFTER_MOVE is defined waits SERVO_DEACTIVATION_DELAY, than detaches.
  */
 
 #ifndef servo_h
@@ -120,6 +122,9 @@ class Servo {
     void detach();
     void write(int value);             // if value is < 200 it is treated as an angle, otherwise as pulse width in microseconds
     void writeMicroseconds(int value); // Write pulse width in microseconds
+    uint8_t move(int pin, int value);  // attach the given pin to the next free channel, sets pinMode, returns channel number or 0 if failure.
+                                       // if value is < 200 it is treated as an angle, otherwise as pulse width in microseconds.
+                                       // if DEACTIVATE_SERVOS_AFTER_MOVE is defined waits SERVO_DEACTIVATION_DELAY, than detaches.
     int read();                        // returns current pulse width as an angle between 0 and 180 degrees
     int readMicroseconds();            // returns current pulse width in microseconds for this servo (was read_us() in first release)
     bool attached();                   // return true if this servo is attached, otherwise false