#include "lopigaengsel.h"
lopigaengsel Servo1;
lopigaengsel Servo2;
lopigaengsel Servo3;
bool flag1;
bool flag2;
////////////////////perintah gerak///////////////////
byte servoAtas(byte sudut, byte interval) {
  bool status = Servo1.Servo1(sudut, interval);
  return status;
}
byte servoTengah(byte sudut, byte interval) {
  bool status = Servo2.Servo2(sudut, interval);
  return status;
}
byte servoBawah(byte sudut, byte interval) {
  bool status = Servo3.Servo3(sudut, interval);
  return status;
}

void setup() {
  Serial.begin(9600);
  pinMode(A0, INPUT_PULLUP);
  pinMode(A1, INPUT_PULLUP);
  Servo1.pin(9);
  Servo2.pin(10);
  Servo3.pin(11);

  /*
    EEPROM.write(0, 45); //posServo1
    EEPROM.write(1, 120); //posServo2
    EEPROM.write(2, 120); //posServo3
  */

  ///////////Posisi awal
  while (millis() < 4000) {
    servoAtas(90, 10);
    servoTengah(90, 10);
    servoBawah(90, 10);
  }
  ///////////Posisi awal
}

void loop() {
  if (digitalRead(A0) == 0) {
    flag1 = 1;
    flag2 = 0;
  }
  if (digitalRead(A1) == 0) {
    flag2 = 1;
    flag1 = 0;
  }
  // gerak 2 , gerak 1
  /////////// gerak salam //////////////
  if (flag1 == 1) {
    if ((servoAtas(0, 10) || servoTengah(45, 10)) && servoBawah(0, 10)) {
    }
  }
  if (flag2 == 1) {
    if ((servoAtas(140, 10) || servoTengah(145, 10)) && servoBawah(150, 10)) {
    }
  }
  /////////// gerak salam //////////////
}