Fix Servo

MK/module/servo/servo.cpp

@@ -299,13 +299,10 @@ void Servo::writeMicroseconds(int value)
   }
 }
 
-int Servo::read() // return the value as degrees
-{
-  return map(readMicroseconds()+1, SERVO_MIN(), SERVO_MAX(), 0, 180);
-}
+// return the value as degrees
+int Servo::read() { return map(this->readMicroseconds() + 1, SERVO_MIN(), SERVO_MAX(), 0, 180); }
 
-int Servo::readMicroseconds()
-{
+int Servo::readMicroseconds() {
   unsigned int pulsewidth;
   if (this->servoIndex != INVALID_SERVO)
     pulsewidth = ticksToUs(servos[this->servoIndex].ticks)  + TRIM_DURATION;
@@ -315,9 +312,16 @@ int Servo::readMicroseconds()
   return pulsewidth;
 }
 
-bool Servo::attached()
-{
-  return servos[this->servoIndex].Pin.isActive;
+bool Servo::attached() { return servos[this->servoIndex].Pin.isActive; }
+
+void Servo::move(int value) {
+  if (this->attach(0) >= 0) {
+    this->write(value);
+    #if ENABLED(DEACTIVATE_SERVOS_AFTER_MOVE)
+      delay(SERVO_DEACTIVATION_DELAY);
+      this->detach();
+    #endif
+  }
 }
 
 #endif

MK/module/servo/servo.h

@@ -83,22 +83,24 @@ typedef struct {
   volatile unsigned int ticks;
 } servo_t;
 
-class Servo
-{
-public:
-  Servo();
-  uint8_t attach(int pin);           // attach the given pin to the next free channel, sets pinMode, returns channel number or 0 if failure
-  uint8_t attach(int pin, int min, int max); // as above but also sets min and max values for writes. 
-  void detach();
-  void write(int value);             // if value is < 200 its treated as an angle, otherwise as pulse width in microseconds 
-  void writeMicroseconds(int value); // Write pulse width in microseconds 
-  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 
-private:
-   uint8_t servoIndex;               // index into the channel data for this servo
-   int8_t min;                       // minimum is this value times 4 added to MIN_PULSE_WIDTH    
-   int8_t max;                       // maximum is this value times 4 added to MAX_PULSE_WIDTH   
+class Servo {
+  public:
+    Servo();
+    uint8_t attach(int pin);           // attach the given pin to the next free channel, sets pinMode, returns channel number or 0 if failure
+    uint8_t attach(int pin, int min, int max); // as above but also sets min and max values for writes.
+    void detach();
+    void write(int value);             // if value is < 200 its treated as an angle, otherwise as pulse width in microseconds
+    void writeMicroseconds(int value); // Write pulse width in microseconds
+    void move(int value);              // attach the servo, then move to value
+                                       // if value is < 200 it is treated as an angle, otherwise as pulse width in microseconds
+                                       // if DEACTIVATE_SERVOS_AFTER_MOVE wait SERVO_DEACTIVATION_DELAY, then detach
+    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
+  private:
+     uint8_t servoIndex;               // index into the channel data for this servo
+     int8_t min;                       // minimum is this value times 4 added to MIN_PULSE_WIDTH    
+     int8_t max;                       // maximum is this value times 4 added to MAX_PULSE_WIDTH   
 };
 
 #endif