#include "lopigaengsel.h"
lopigaengsel putarKanan;
lopigaengsel putarKiri;
lopigaengsel bahuKanan;
lopigaengsel sikuKanan;
lopigaengsel bahuKiri;
lopigaengsel sikuKiri;


byte btn_memper, btn_sikapsiap;
bool status, statusSiap;

void setup() {
  Serial.begin(9600);
  pinMode(A0, INPUT_PULLUP);
  pinMode(A1, INPUT_PULLUP);
  putarKanan.pin(3);
  bahuKanan.pin(5);
  sikuKanan.pin(6);
  bahuKiri.pin(9);
  sikuKiri.pin(10);
  putarKiri.pin(11);

}
void loop() {

  btn_memper = digitalRead(A0);
  btn_sikapsiap = digitalRead(A1);

  if (btn_memper == LOW) {
    status = true;
    statusSiap = false;
  }
  if (btn_sikapsiap == LOW) {
    status = false;
    statusSiap = true;
  }
  if (status == 1) {
    bahuKiri.Servo1(100, 10);
    sikuKiri.Servo2(120, 10);
    putarKiri.Servo3(140, 10);

    bahuKanan.Servo4(100, 10);
    sikuKanan.Servo5(120, 10);
    putarKanan.Servo6(140, 10);
  }
  if (statusSiap == 1) {
    bahuKiri.Servo1(20, 10);
    sikuKiri.Servo2(30, 10);
    putarKiri.Servo3(40, 10);

    bahuKanan.Servo4(20, 10);
    sikuKanan.Servo5(30, 10);
    putarKanan.Servo6(40, 10);
  }
}