ghost_trap_v3

#include <Servo.h>

// --door variables: servo1--  SERVO OPEN AND CLOSED VALUES NEED TO BE ADJUSTED FOR YOUR SERVOS!
Servo servo1;
const int servo1Pin = 9;
const int servo1OpenPosition = 68;
const int servo1ClosedPosition = 32;

// --door variables: servo_2--
Servo servo2;
const int servo2Pin = 10;
const int servo2OpenPosition = 15;
const int servo2ClosedPosition = 63;

// --door variables: state--
bool doorOpen = false;
const int doorWaitTime = 300; // If your doors don't have enough time to close, make this longer

// --input variables--
const int postInputDelay = 100;

// --Pedal variables--
const int pedalPin = 2;
// pedalState tracks whether or not the pedal is on or off
int pedalState = 0;
int pedalPress = false;

void setup() {
  Serial.begin(9600);
  // Set up the pin that watches for the pedal button
  pinMode(pedalPin, INPUT);
  CloseDoors();
}

void loop() {
  // First, read the state of the pedal
  pedalState = digitalRead(pedalPin);

  // This block detects if a button has been newly pressed (button down)
  if(pedalState == HIGH && pedalPress == false) {
    Serial.print("Pedal Press");
    Serial.print('\n');
    pedalPress = true;

    if(pedalPress == true && doorOpen == false) {
      OpenDoors();
      doorOpen = true;
    }

    else if(pedalPress == true && doorOpen == true) {
      CloseDoors();
      doorOpen = false;
    }
    delay(postInputDelay);
  }

  // This detects if the button is let go (button up)
  else if(pedalState == LOW && pedalPress == true) {
    pedalPress = false;
    Serial.print("Pedal Release");
    Serial.print('\n');
    delay(postInputDelay);
  }
}

void OpenDoors() {
  AttachServos();
  MoveServos(servo1OpenPosition, servo2OpenPosition);
  delay(doorWaitTime);
  DetachServos();
}

void CloseDoors() {
  AttachServos();
  MoveServos(servo1ClosedPosition, servo2ClosedPosition);
  delay(doorWaitTime);
  DetachServos();
}

void AttachServos() {
  servo1.attach(servo1Pin);
  servo2.attach(servo2Pin);
}

void MoveServos(int servo1Position, int servo2Position) {
  servo1.write(servo1Position);
  servo2.write(servo2Position);
}

void DetachServos() {
  servo1.detach();
  servo2.detach();
}