arduino_simple_servo.cpp

// servoMove - can use this instead of servo.write
// function to easily move servo at any speed
// end_pos - position you would like to move the servo to
// ms_deg - how many milliseconds per degree (inverse speed)
//   *  can't move faster than 2 ms per degree
//   *  typically range from 2 (fast) to 50 (slow)
// Exp:  servoMove(0, 2);
void servoMove(Servo _servo, int end_pos, int ms_deg) {
  
  // get current position
  int start_pos = _servo.read();
  // wrap position between 0 and 180
  end_pos = abs(end_pos) % 181;
  // can't be faster than 2 ms per degree
  if (ms_deg < 2) ms_deg = 2;

  // if moving to a higher degree angle
  if (start_pos < end_pos) {
    for (int i=start_pos; i<=end_pos; i++) {
      // tell servo to go to position in variable 'pos'
      _servo.write(i);
      // wait for the servo to reach the position
      delay(ms_deg);
    }
  }

  // if moving to a lower degree angle
  else if (start_pos > end_pos) {
    for (int i=start_pos; i>=end_pos; i--) {
      // tell servo to go to position in variable 'pos'
      _servo.write(i);
      // wait for the servo to reach the position
      delay(ms_deg);
    }
  }
}