 theta3 => 3
 theta4 => 5
 */
-int thetaPin[] = {9,11,3,5};
-const int numberOfMotors = 4;
+int thetaPin[] = {3,5,6,9,10,11};
+const int numberOfMotors = 6;
 //We define a servo instance for each motor
 Servo thetaServo[numberOfMotors];
 

  //Initialize Serial Com
  Serial.begin(9600);
  //Attach each instance with the corresponding pin
  for (int i; i < numberOfMotors; i++) {
    thetaServo[i].attach(i);
  }
}
int Angle;

   if (servoNo < numberOfMotors) {
     Angle = Serial.readStringUntil(':').toInt();
     thetaServo[servoNo].write(Angle);
     Serial.print("Theta ");
   Serial.print(servoNo);
   Serial.print(" Was sent an angle of ");
   Serial.println(Angle);
   } else {
    Serial.println("HERE");
    for (int i = 0; i < numberOfMotors; i++) {
      int val = thetaServo[i].read();
      Serial.println(val);
    }
   }
  }
}
