/*

  Servo Logic Code

  This script handles logic related to servo operations

*/

//Calibrated offset to add or reduce for this particilar machine
const int servoAlignmentOffset = 0;

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

//Push switch back with delay
void pushSwitchDelayed(int coverDelay, int pusherDelay) {
  servoCoverPusher.write(90);
  delay(coverDelay);
  servoSwitchPusher.write(130 + servoAlignmentOffset);
  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 + servoAlignmentOffset);
  delay(2000);
  servoSwitchPusher.write(130 + servoAlignmentOffset);
  delay(2500);
  servoCoverPusher.write(0);
  servoSwitchPusher.write(0);
}


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