// Arduino Door Catcher
// Vije Miller

#include <VarSpeedServo.h>

VarSpeedServo stopper;
VarSpeedServo base;
VarSpeedServo mid;
VarSpeedServo handle;

int pos;

// Magnetic Sensor
int hall = 2;
int magnet;
int state = 0;

void setup() {

  Serial.begin(9600);

  pinMode(hall, INPUT);

  stopper.attach(3);
  stopper.write(140, 60, true);

  handle.attach(6);
  handle.write(95, 60, true);

  base.attach(4);
  mid.attach(5);
  base.write(180, 70);
  mid.write(25, 50);
  base.wait();
  mid.wait();

}

void loop() {

  // Did Door Open?
  magnet = digitalRead(hall);

  // Door Opened
  if (magnet == HIGH) {
    // Door Was Closed
    if (state == 0) {
      Serial.println("Active");
      delay(1000);
      engage();
      state = 1;
      // Door Was Open
    } else if (state == 1) {
      do_it();
      Serial.println("Deactived");
      delay(1000);
      state = 0;
    }
  }
  delay(47);
}

// Release Door Slide
void engage() {
  stopper.write(40, 80, true);
}

void do_it() {
  // Retract
  stopper.write(140, 80, true);
  // Set
  mid.write(90, 50);
  base.write(130, 30);
  mid.wait();
  base.wait();
  handle.write(0, 50, true);
  // Push
  mid.write(70, 40);
  base.write(90, 40);
  mid.wait();
  base.wait();
  // Retract
  mid.write(90, 50, false);
  base.write(130, 50, true);
  // Set to Grab
  handle.write(95, 50, true);
  mid.write(85, 50, true);
  // Grab Door Handle
  base.write(95, 100, true);
  handle.write(0, 50, true);
  // Close Door
  mid.write(77, 50, false);
  base.write(77, 50, true);
  handle.write(95, 50, true);
  // Retract
  mid.write(90, 50);
  base.write(120, 30);
  mid.wait();
  base.wait();
  // Home
  mid.write(25, 50);
  base.write(180, 70);
  mid.wait();
  base.wait();
}