void moveServo(tMotor servoName, int  endPos, int timeToTake){
  int currPos = motor[servoName];
  int period = timeToTake / abs(currPos - endPos);//time to wait between each movement
  if(endPos > currPos){
    for(int i = currPos; i < endPos; i++){
      motor[servoName] = i;
      wait1Msec(period);
    }
  }
  else if(currPos > endPos){
    for(int i = currPos; i > endPos; i--){
      motor[servoName] = i;
      wait1Msec(period);
    }
  }
}