 String serialString;
 
 // Servos
-Servo *servo1;
-Servo *servo2;
-int pos = 0;
+int active_servos = 1;
+int const NUM_SERVOS = 20;
+Servo* servo[NUM_SERVOS];
+int pattern = 1;
 
 void setup()
 {
   // Start listening on the serial port for commands
   Serial.begin(57600);
  
-      servo1 = new Servo();
-      servo2 = new Servo();
-
-      // Attach a servo controller to pin 4.
-      DDRD |= DDRD | 1<<7;
-      attach(servo1, &PORTD, 1<<7);
-      
-      // Attach a servo controller to pins 5 and 6, (will control 2 servos as mirrors)
-      DDRD |= DDRD | 1<<5 | 1<<6;
-      attach(servo2, &PORTD, 1<<5 | 1<<6);
-      
-      servo1->maxVelocity(425);
-      servo1->rotateTo(0, 20);
-      
-      servo2->rotateTo(180);
-      
-      Serial.println("Setup Complete."); 
-      Serial.println(DDRD); 
+  DDRD = B11111100;
+  DDRB = B00111111;
+  DDRC = B11111111;
+  
+ // Servo s = new Servo();
+  
+  for(int i = 0; i < NUM_SERVOS; i++)
+  {
+    servo[i] = new Servo();
+    
+    if(i < 6)
+    {
+      attach(servo[i], &PORTD, 1<<(i+2));
+    }
+    else if(i < 12)
+    {
+      attach(servo[i], &PORTB, 1<<(i-6));
+    }
+    else if(i < 20)
+    {
+      attach(servo[i], &PORTC, 1<<(i-12));
+    }
+    
+    servo[i]->rotateTo(0);
+  }
+  
+  Serial.println("Setup Complete."); 
 }
 
 void printHex(int num, int precision) {

        Serial.print("Received command:"); 
        Serial.print(serialString);
        
          Servo *selectedServo = NULL;
        //ex: "1:45" set servo1 to 45 degrees
          if(serialString.substring(0, 1) == "1")
            selectedServo = servo1;
          else if(serialString.substring(0, 1) == "2")
            selectedServo = servo2;
          
          if(selectedServo){
            char angleChars[] = {' ', ' ', ' ', '\0'};
            serialString.substring(2, 5).toCharArray(angleChars, 4);
            int angle = atoi(angleChars);
            selectedServo->rotateTo(angle);
            Serial.print("Moving to:");
            Serial.println(angle);
          }
        serialString = "";
      }
  } 

}

void loop(){
 
  /* DEBUG INFO */
/*   
  Serial.print("current angle: "); Serial.println(servo1->currentAngle(), DEC);
  Serial.print("current pulse: "); printHex(servo1->currentPulse(), 8);
  Serial.print("destination angle: "); Serial.println(servo1->destinationAngle());
  Serial.print("next angle: "); Serial.println(servo1->nextAngle());
  Serial.print("max velocity: ");  printHex(servo1->maxVelocity(), 8);
  Serial.print("destination angle: "); Serial.println(servo1->destinationAngle());
  Serial.print("is limited: "); Serial.println(servo1->isLimited());
*/

// Rotates all the servos in unison
if(pattern == 1)
{
  for(int i=0; i<active_servos; i++)
  {
    if(servo[i]->currentAngle() <= 0)
      servo[i]->rotateTo(180);
    else if(servo[i]->currentAngle() >= 180)
      servo[i]->rotateTo(0);
  }
}

// Rotates all the servos at varying speeds.
if(pattern == 2)
{
  for(int i=0; i<active_servos; i++)
  {
    if(servo[i]->currentAngle() <= 0)
      servo[i]->rotateTo(180, 100/(i+1));
    else if(servo[i]->currentAngle() >= 180)
      servo[i]->rotateTo(0,100/(i+1));
  }
}
  
  
  if(Serial.available() > 0){
