#include <Servo.h>
Servo servo1, servo2, servo3, servo4, servo5, servo6;  // create servo object to control a servo

int SERVOSTATE_OPEN = 1; //indicator for communication via serial
int SERVOSTATE_CLOSED = 0; //indicator for communication via serial

//default positions (not normally used)
int SERVOSTATE_OPEN_POSITION = 2;//degrees position for open
int SERVOSTATE_CLOSED_POSITION = 95;// degrees position for closed


int ARDUINO_NUMBER = 1; //range 1,2,3 or 4 (Arduino position in servo)
int NUMBER_OF_SERVOS = 5; //number of servo's per Arduino;

//The servo structure
struct ServoStruct {
  unsigned int  servoNumber: 5; // the number we addres from our serial communication, the :5 is the number of bits used
  unsigned int  pinNumber: 4; // the pin it is connected to, the :4 is the number of bits
  int   currentPosition; // current position in degrees
  Servo  servoObject; // the actual object
  int OPEN_POSITION; // open position (anything between 0 and 180)
  int CLOSED_POSITION;// closed position (anything between 0 and 180)
};

struct ServoStruct *ss0, ss1, ss2, ss3, ss4, ss5, ss6;

void setup() {
  // initialize serial:
  Serial.begin(9600);

  ss1.servoNumber = (ARDUINO_NUMBER - 1) * NUMBER_OF_SERVOS + 1;
  ss1.pinNumber = 3;
  ss1.currentPosition = 0;
  ss1.OPEN_POSITION = SERVOSTATE_OPEN_POSITION;
  ss1.CLOSED_POSITION = SERVOSTATE_CLOSED_POSITION;
  ss1.servoObject = servo1;
  ss1.servoObject.attach(ss1.pinNumber);
  ss1.servoObject.write(ss1.OPEN_POSITION); // move the servo to its start position

  ss2.servoNumber = (ARDUINO_NUMBER - 1) * NUMBER_OF_SERVOS + 2;
  ss2.pinNumber = 5;
  ss2.currentPosition = 0;
  ss2.OPEN_POSITION = SERVOSTATE_OPEN_POSITION;
  ss2.CLOSED_POSITION = SERVOSTATE_CLOSED_POSITION;
  ss2.servoObject = servo2;
  ss2.servoObject.attach(ss2.pinNumber);
  ss2.servoObject.write(ss2.OPEN_POSITION);

  ss3.servoNumber = (ARDUINO_NUMBER - 1) * NUMBER_OF_SERVOS + 3;
  ss3.pinNumber = 6;
  ss3.currentPosition = 0;
  ss3.OPEN_POSITION = SERVOSTATE_OPEN_POSITION;
  ss3.CLOSED_POSITION = SERVOSTATE_CLOSED_POSITION;
  ss3.servoObject = servo3;
  ss3.servoObject.attach(ss3.pinNumber);
  ss3.servoObject.write(ss3.OPEN_POSITION);

  ss4.servoNumber = (ARDUINO_NUMBER - 1) * NUMBER_OF_SERVOS + 4;
  ss4.pinNumber = 10;
  ss4.currentPosition = 0;
  ss4.OPEN_POSITION = SERVOSTATE_OPEN_POSITION;
  ss4.CLOSED_POSITION = SERVOSTATE_CLOSED_POSITION;
  ss4.servoObject = servo4;
  ss4.servoObject.attach(ss4.pinNumber);
  ss4.servoObject.write(ss4.OPEN_POSITION);

  ss5.servoNumber = (ARDUINO_NUMBER - 1) * NUMBER_OF_SERVOS + 5;
  ss5.pinNumber = 9;
  ss5.currentPosition = 0;
  ss6.OPEN_POSITION = SERVOSTATE_OPEN_POSITION;
  ss6.CLOSED_POSITION = SERVOSTATE_CLOSED_POSITION;
  ss5.servoObject = servo5;
  ss5.servoObject.attach(ss5.pinNumber);
  ss5.servoObject.write(ss5.OPEN_POSITION);

  ss6.servoNumber = (ARDUINO_NUMBER - 1) * NUMBER_OF_SERVOS + 6;
  ss6.pinNumber = 11;
  ss6.currentPosition = 0;
  ss6.OPEN_POSITION = SERVOSTATE_OPEN_POSITION;
  ss6.CLOSED_POSITION = SERVOSTATE_CLOSED_POSITION;
  ss6.servoObject = servo6;
  ss6.servoObject.attach(ss6.pinNumber);
  ss6.servoObject.write(ss6.OPEN_POSITION);
}

void loop() {
  if ( Serial.available() >= 2 ) {
    if ((int)Serial.peek() <=  NUMBER_OF_SERVOS) {
      int servoNr = Serial.read();
      if (ss1.servoNumber == servoNr) {
        ss0 = &ss1;
      } else   if (ss2.servoNumber == servoNr) {
        ss0 = &ss2;
      } else   if (ss3.servoNumber == servoNr) {
        ss0 = &ss3;
      } else   if (ss4.servoNumber == servoNr) {
        ss0 = &ss4;
      } else   if (ss5.servoNumber == servoNr) {
        ss0 = &ss5;
      } else   if (ss6.servoNumber == servoNr) {
        ss0 = &ss6;
      } else {
        //Serial.print("No servos found for number:");
        //Serial.println(servoNr, BIN);
        //Serial.println(ss1.servoNumber, BIN);
        //Serial.println(ss2.servoNumber, BIN);
        //Serial.println(ss3.servoNumber, BIN);
        //Serial.println(ss4.servoNumber, BIN);
        //Serial.println(ss5.servoNumber, BIN);
        //Serial.println(ss6.servoNumber, BIN);
      }
    }
    if ((int)Serial.peek() <= 180 ) {
      moveServo(ss0, Serial.read());
    }
  }
}

void moveServo(struct ServoStruct *servo, int newPosition){
 servo->servoObject.write(newPosition); 
  servo->currentPosition = newPosition;
  Serial.println(servo->servoNumber);
  Serial.println(servo->currentPosition);
}