// Arduino Door Catcher
// vgmlr
#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();
}