Here is some code I use to test the MegaServo library that should give you an idea of how to do what you want:
#include <MegaServo.h>
#define NBR_SERVOS 12 // the number of servos, up to 48 for Mega, 12 for other boards
#define FIRST_SERVO_PIN 2
MegaServo myServos[NBR_SERVOS] ; // max servos is 48 for mega, 12 for other boards
void setup()
{
Serial.begin(9600);
for(int i=0; i < NBR_SERVOS; i++)
myServos[i].attach(FIRST_SERVO_PIN +i);
}
void loop()
{
static int pos = 0;
if ( Serial.available())
{
char ch = Serial.read();
if(ch >= '0' && ch <= '9') // is ch a number?
pos = pos * 10 + ch - '0'; // yes, accumulate the value
else if(ch >= 'a' && ch <= 'a'+ NBR_SERVOS) // is ch a letter for one of our servos?
{
myServos[ch - 'a'].write(pos); // yes, save the position in the position array
pos = 0;
int channel = ch - 'a';
int angle = myServos[channel].read();
int pulseWidth = myServos[channel].readMicroseconds();
Serial.print("Servo on pin ");
Serial.print(FIRST_SERVO_PIN + channel, DEC);
Serial.print(": angle = ");
Serial.print(angle,DEC), Serial.print(", pulse = ");
Serial.println(pulseWidth,DEC);
}
else if (ch == '*')
{
// position all the servos
for(int i=0; i < NBR_SERVOS; i++)
myServos[i].write(pos);
pos = 0;
}
else if (ch == '+')
{
// sweep the servos from 0 to 180
for(int angle = 0;angle < 180; )
{
for(int i=0; i < NBR_SERVOS; i++)
{
myServos[i].write(angle);
}
angle = myServos[0].read();
Serial.print("Angle = "); Serial.print(angle,DEC), Serial.print(", pulse = "); Serial.println(myServos[0].readMicroseconds(),DEC);
++angle;
delay(20);
}
pos = 0;
}
else if (ch == '-')
{
// sweep the servos from 180 to 0
for(int angle = 180;angle >= 0; )
{
for(int i=0; i < NBR_SERVOS; i++)
{
myServos[i].write(angle);
}
angle = myServos[0].read();
Serial.print("Angle = "); Serial.print(angle,DEC), Serial.print(", pulse = "); Serial.println(myServos[0].readMicroseconds(),DEC);
--angle;
delay(20);
}
pos = 0;
}
}
}
the code expects a serial command consisting of a valid value (0 through 180) followed by a letter indicating the servo to write to. The letter 'a' is the first servo, 'b' the second and so on.
For example:
90a will write a value of 90 to the first servo
180b will write 180 to the second servo
A value followed by a * instead of a letter will be written to all servos
The single character + will sweep all the servos from 0 to 180
The single character – will sweep all the servos from 180 to 0