/*

  Servo Logic Code

  This script handles logic related to servo operations

*/

//Quickily push the switch back
void pushSwitchNow() {
  pushSwitchDelayed(270, 270);
}

//Push switch back with delay
void pushSwitchDelayed(int coverDelay, int pusherDelay) {
  servoCoverPusher.write(90);
  delay(coverDelay);
  servoSwitchPusher.write(130 + SERVO_ALIGNMENT_OFFSET);
  delay(pusherDelay);
  servoCoverPusher.write(0);
  servoSwitchPusher.write(0);
}

//Push with a pause before the switch is pushed
void pushWithHesitation(){
  servoCoverPusher.write(90);
  delay(1000);
  servoSwitchPusher.write(90 + SERVO_ALIGNMENT_OFFSET);
  delay(2000);
  servoSwitchPusher.write(130 + SERVO_ALIGNMENT_OFFSET);
  delay(1000);
  servoCoverPusher.write(0);
  servoSwitchPusher.write(0);
}


//Set the pusher servo to arm installation position
void setSwitchToInstallPosition(){
  servoSwitchPusher.write(125 + SERVO_ALIGNMENT_OFFSET);
}