#include "lopigaengsel.h"
lopigaengsel Servo1;
lopigaengsel Servo2;
lopigaengsel Servo3;
lopigaengsel Servo4;

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;
}
byte servoTangan(byte sudut, byte interval) {
  bool status = Servo4.Servo4(sudut, interval);
  return status;
}
char perintah;
char jejakPerintah;
bool a, b, c;
void setup() {
  Serial.begin(9600);
  pinMode(A0, INPUT_PULLUP);
  pinMode(A1, INPUT_PULLUP);
  Servo1.pin(A4);
  Servo2.pin(9);
  Servo3.pin(10);
  Servo4.pin(11);
  /*
  Servo1.pin(5);
  Servo2.pin(4);
  Servo3.pin(3);
  Servo4.pin(2);
  */
  pinMode(LED_BUILTIN, OUTPUT);
  /*
      EEPROM.write(0, 90); //posServo1
      EEPROM.write(1, 90); //posServo2
      EEPROM.write(2, 90); //posServo3
      EEPROM.write(3, 90); //posServo3
  */

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

}

void loop() {

  if (Serial.available())
  {
    perintah = Serial.read();
    //  delay (500);
    if (perintah == 'a') {
      a = 1;
      b = 0;
    }
    if (perintah == 'b') {
      b = 1;
      a = 0 ;
    }
  }
  if (a) {
    if ((servoAtas(40, 10) || servoTengah(130, 10)) && servoBawah(45, 10) && servoTangan(60, 10)) {
    }
  }
  if (b) {
    if ((servoAtas(90, 10) || servoTengah(30, 10)) && servoBawah(145, 10) && servoTangan(160, 10)) {
    }
  }

  // Serial.println(perintah);

  // Serial.println("Isi perintah :" + perintah);
  /*
    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) {
    //  //        sekalian, step 1              //==//   step 2     //==//    step 3      //
    if ((servoAtas(90, 10) || servoTengah(90, 10)) && servoBawah(90, 10) && servoTangan(10, 10)) {
    }
    }
    if (flag2 == 1) {
    //  //        sekalian, step 1              //==//   step 2     //==//    step 3      //
    if ((servoAtas(90, 10) || servoTengah(90, 10)) && servoBawah(90, 10) && servoTangan(90, 10)) {
    }
    }
    /////////// gerak salam //////////////
  */

}