#define HINGE_SERVO 2

persistent int HINGE_OPEN;
persistent int HINGE_CLOSED;

void calibrate_hinge() {
  printf("Set Servo:      Hinge Open\n");
  start_press();
  HINGE_CLOSED = servo_getvalue(HINGE_SERVO);
  printf("Set Servo:      Hinge Closed\n");
  start_press();
  HINGE_CLOSED = servo_getvalue(HINGE_SERVO);
}

void open_hinge() {
  steer_straight();
  drive_forward(20);
  servo(HINGE_SERVO, HINGE_OPEN);
  sleep(.6);
  drive_forward(0);
}

void close_hinge() {
  steer_straight();
  drive_forward(-20);
  servo(HINGE_SERVO, HINGE_CLOSED);
  sleep(.6);
  drive_forward(0);
}

void init_hinge() {
  HINGE_OPEN=600;
  HINGE_CLOSED=3390;
}