Servo smoothmove

#include <Servo.h>

class myServo : public Servo {
  public:
    uint16_t MINPULSE = 980;      // durata impulso minima
    uint16_t MAXPULSE = 2020;     // durata impulso massima
    float speed_constant = 0.03F; // servo factor, the higher the faster
    void smoothmove(int setPosition) ;
};

// servo_smoothmove by witpim. 2007
void myServo::smoothmove(int setPosition) {
 static int oldPosition;
 int anglediff = abs(setPosition - oldPosition);
 float speed_factor = speed_constant/anglediff;
 static int oldposition;
 for (float timeAngle=0.000; timeAngle<=1;) {
   timeAngle = timeAngle + speed_factor;
   float timeAngleadjust = (1.0/2.0) - (1.0/2.0*cos(timeAngle*PI));
   float moveAngle = (anglediff*timeAngleadjust);
   int calc_angle;
   if (oldPosition > setPosition)
     calc_angle =  oldPosition - (int)moveAngle;
   else
     calc_angle =  oldposition + (int)moveAngle ;
   // Uso writeMicroseconds perché cosi posso adeguare il software al reale comportamento del servo
   this->writeMicroseconds(map(calc_angle, 0, 180, MINPULSE, MAXPULSE));
 }
 oldPosition = setPosition;
}

myServo Servo1, Servo2;

void setup() {
   Serial.begin(9600);
   Servo1.attach(9);
   Servo2.attach(10);
   Servo2.speed_constant = 0.05F;
}

void loop() {
 Servo1.smoothmove(0);
 Servo1.smoothmove(180);
 Servo2.smoothmove(0);
 Servo2.smoothmove(180);
}