arduino DS3115 and RDS3115 servo control

// Sketch for rapidly moving two servers to a point from -90 to 90 degrees on one motor while
// at the same time driving a second servo from -45 to 90 degrees
// Using a serial interface to allow me to type in test coordinates to direct the servos.
//
//

//#define DEBUG

#include <Servo.h>

Servo lowerservo;  // create servo object to control a servo  DS3115 MG (0-180 degrees) Digital Servo
Servo upperservo;  // create servo object to control a servo RDS3115 MG (0-135 degrees) Robot Digital Servo


void setup() {
  Serial.begin(9600);
  lowerservo.attach( 9,544,2400);  // attaches lower servo on pin  9 on servo object  DS3115 MG
  upperservo.attach(10,544,2400);  // attaches upper servo on pin 10 on servo object RDS3115 MG
//  544uS =   0 degrees
// 1500uS =  90 degrees
// 2400uS = 180 degrees
  servos_init();
}


void loop() {
 while (Serial.available() == 0);
 int lower = Serial.parseInt(); //read int from serial
 lower=constrain(lower, -90, 90); // limit the input value from -90 degrees to +90 degrees for the lower servo
 lower=map(lower,-90, 90, 0, 180); // here I am re-mapping -90<->+90 to 0<->180

 while (Serial.available() == 0);
 int upper = Serial.parseInt(); //read int from serial
 upper=constrain(upper, -45, 90); // limit the input value from -45 degrees to +90 degrees for the upper servo
 upper=map(upper,-45,90,135,0); // here I am re-mapping  -45<->90 to 135<->0
 servo_line(lower,upper);
 //delay(1000);
 //servo_line(random(0, 180),random(0, 135)); // point in random directions.
}


int servos_init() // wiggle the servo about to make sure that its internal tracking latches.
{                 // Only needed at power on when the internal state is unknown to the servo from powerdown.
  servo_line( 90, 135); //   0,-45
  servo_line( 90,   0); //   0, 90
  servo_line(  0,   0); // -90, 90
  servo_line(180,   0); //  90, 90
  servo_line( 90, 135); //   0,-45
  servo_line( 90,  90); //   0,  0
}


void servo_line(int new_lower_angle, int new_upper_angle)
{
 int new_lower_pulsewidth = map(new_lower_angle, 0, 180, 544,  2400); // convert angles to uS duration for PWM(high)
 int new_upper_pulsewidth = map(new_upper_angle, 0, 180, 544,  2400); // convert angles to uS duration for PWM(high)
#ifdef DEBUG
// print out the angles in degrees
 Serial.print(new_lower_angle); Serial.print(" "); Serial.println(new_upper_angle);
// print out the Microseconds that the PWM is high the rest of the 20 miliseconds it is low
 Serial.print(lowerservo.readMicroseconds()); Serial.print(" ");
 Serial.print(upperservo.readMicroseconds()); Serial.print(" ");
 Serial.print(new_lower_pulsewidth); Serial.print(" ");
 Serial.println(new_upper_pulsewidth);
#endif
 line(lowerservo.readMicroseconds(), upperservo.readMicroseconds(), new_lower_pulsewidth, new_upper_pulsewidth);
}


// line drawing code from http://rosettacode.org/wiki/Bitmap/Bresenham's_line_algorithm#C

void line(int x0, int y0, int x1, int y1) // current x_pwm, current y_pwm, destination x_pwm, destination y_pwm
{
  int dx = abs(x1-x0), sx = x0<x1 ? 1 : -1;
  int dy = abs(y1-y0), sy = y0<y1 ? 1 : -1;
  int err = (dx>dy ? dx : -dy)/2, e2;

  for(;;){
//    setPixel(x0,y0);
    lowerservo.writeMicroseconds(x0);
   delay(1);           //delay for settling and to not move both servos at once (less power required)
    upperservo.writeMicroseconds(y0);
    delay(1);           //delay for settling and to not move both servos at once (less power required)

    if (x0==x1 && y0==y1) break;
    e2 = err;
    if (e2 >-dx) { err -= dy; x0 += sx; }
    if (e2 < dy) { err += dx; y0 += sy; }
  }

}