// A sample of how to control 2 servos at the same time.
// Note –   Apparently, the Servo library can handle this. The library on Arduino's playground at
//          http://www.arduino.cc/playground/ComponentLib/Servotimer1 is really broken and generates
//          a bunch of errors straight from the import command. 
//
//  Khazar, 11/08

#include <Servo.h>
Servo myservo1;          // Create a servo object to control servo 1
Servo myservo2;          // Create a servo object to control servo 2

int pos1 = 0;            // Variable to hold the position of servo 1
int pos2 = 0;            // Variable to hold the position of servo 2
long timer1Val = 100;    // Variable to hold the timer for servo 1
long timer2Val = 100;    // Variable to hold the timer for servo 2
int updown1 = 1;         // Variable to hold the positive or negative change in angle 
int updown2 = 1;         // for servo 1, ditto for servo 2

void setup() {
  myservo1.attach(9);    // Attaches servo1 to pin 9;
  myservo2.attach(10);   // Attaches servo2 to pin 10;
  Serial.begin(9600);    // Turn on serial communication just in case...
  timer1Val = millis();  // init timer 1;
  timer2Val = millis();  // init timer 2;
}

void loop() {
  // Timer 1 loop
  if ((timer1Val + 100) <= millis()) {
    myservo1.write(pos1);    // do movement
    Serial.print("Servo 1 position: ");
    Serial.println(pos1);    // check value
    pos1 = pos1 + updown1;   // increment or decrement value depending on updown's value
    if (pos1 == 180) {       // If pos1 reaches 180, change directon by making updown negative
       updown1 = -1;
    }
    if (pos1 == 0) {         // If pos1 reaches 0, change direction by making updown positive
      updown1 = 1;
    }
    timer1Val = millis();    // Reset the timer
  }
  if ((timer2Val + 50) <= millis()) {
    myservo2.write(pos2);    // do movement
    Serial.print("Servo 2 position: ");
    Serial.println(pos2);    // check value
    pos2 = pos2 + updown2;   // increment or decrement value depending on updown's value
    if (pos2 == 180) {       // If pos1 reaches 180, change directon by making updown negative
       updown2 = -1;
    }
    if (pos2 == 0) {         // If pos1 reaches 0, change direction by making updown positive
       updown2 = 1;
    }
    timer2Val = millis();    // Reset the timer
  }
}