#include <ESP32Servo.h>

Servo Lpinggul, Lpaha, Llutut, Lkaki;
Servo Rpinggul, Rpaha, Rlutut, Rkaki;
Servo RBahu, RLenganAtas, RLenganBawah, RTangan;
Servo LBahu, LLenganAtas, LLenganBawah, LTangan;

const int buttonPin = 19; 
int buttonState = 0; 

void setup() {

  pinMode(buttonPin, INPUT);
  
  // Kaki Kiri
  Lpinggul.attach(27);
  Lpaha.attach(14);
  Llutut.attach(12);
  Lkaki.attach(13);

  // Kaki Kanan
  Rpinggul.attach(0);
  Rpaha.attach(4);
  Rlutut.attach(2);
  Rkaki.attach(15);

  // Tangan Kanan
  RBahu.attach(18);
  RLenganAtas.attach(5);
  RLenganBawah.attach(17);
  RTangan.attach(16);

  // Tangan Kiri
  LBahu.attach(32);
  LLenganAtas.attach(33);
  LLenganBawah.attach(25);
  LTangan.attach(26);

  // Inisialisasi semua servo ke posisi awal (opsional)
  Lpinggul.write(0);
  Lpaha.write(0);
  Llutut.write(0);
  Lkaki.write(0);

  Rpinggul.write(0);
  Rpaha.write(0);
  Rlutut.write(0);
  Rkaki.write(0);

  RBahu.write(0);
  RLenganAtas.write(0);
  RLenganBawah.write(0);
  RTangan.write(0);

  LBahu.write(0);
  LLenganAtas.write(0);
  LLenganBawah.write(0);
  LTangan.write(0);

  delay(1000);
}

void loop() {
  buttonState = digitalRead(buttonPin);
  if (buttonState == HIGH) {
    gerakMaju();
  }
}
void gerakMaju(){
  Rkaki.write(90 - 40 - 10);         // right foot
  Rlutut.write(90 - 40 + 20);        // right lower leg
  Rpaha.write(90 + 40 - 20);         // right upper leg
  Rpinggul.write(90 - 40 - 10);      // right hip

  Lpinggul.write(90 + 40 - 10);      // left hip
  Lpaha.write(90 - 40 + 70);         // left upper leg
  Llutut.write(90 + 40 - 70);        // left lower leg
  Lkaki.write(90 + 40 - 10);         // left foot

  RBahu.write(90 + 40 - 20);         // right shoulder
  RLenganAtas.write(90 + 40 + 20);   // right upper arm
  RLenganBawah.write(90 - 40 + 40);  // right lower arm
  RTangan.write(90 + 40);            // right hand

  LBahu.write(90 - 40 + 20);         // left shoulder
  LLenganAtas.write(90 - 40 - 20);   // left upper arm
  LLenganBawah.write(90 + 40 - 40);  // left lower arm
  LTangan.write(90 - 40);            // left hand
  delay(100);


  Rkaki.write(90 - 40 - 10);          // right foot
  Rlutut.write(90 - 40 + 20);         // right lower leg
  Rpaha.write(90 + 40 - 20);          // right upper leg
  Rpinggul.write(90 - 40 - 10);       // right hip

  Lpinggul.write(90 + 40 - 10);       // left hip
  Lpaha.write(90 - 40 + 70);          // left upper leg
  Llutut.write(90 + 40 - 0);          // left lower leg
  Lkaki.write(90 + 40 - 10);          // left foot

  RBahu.write(90 + 40 - 20);          // right shoulder
  RLenganAtas.write(90 + 40 - 20);    // right upper arm
  RLenganBawah.write(90 - 40 + 40);   // right lower arm
  RTangan.write(90 + 40);             // right hand

  LBahu.write(90 - 40 + 20);          // left shoulder
  LLenganAtas.write(90 - 40 - 20);    // left upper arm
  LLenganBawah.write(90 + 40 - 40);   // left lower arm
  LTangan.write(90 - 40);             // left hand
  delay(100);


  Rkaki.write(90 - 40 - 0);           // right foot
  Rlutut.write(90 - 40 + 30);         // right lower leg
  Rpaha.write(90 + 40 - 0);           // right upper leg
  Rpinggul.write(90 - 40 - 0);        // right hip

  Lpinggul.write(90 + 40 - 0);        // left hip
  Lpaha.write(90 - 40 + 30);          // left upper leg
  Llutut.write(90 + 40 - 0);          // left lower leg
  Lkaki.write(90 + 40 - 0);           // left foot

  RBahu.write(90 + 40 - 20);          // right shoulder
  RLenganAtas.write(90 + 40 - 10);    // right upper arm
  RLenganBawah.write(90 - 40 + 40);   // right lower arm
  RTangan.write(90 + 40);             // right hand

  LBahu.write(90 - 40 + 20);          // left shoulder
  LLenganAtas.write(90 - 40 - 20);    // left upper arm
  LLenganBawah.write(90 + 40 - 40);   // left lower arm
  LTangan.write(90 - 40);             // left hand
  delay(100);


  Rkaki.write(90 - 40 + 10);          // right foot
  Rlutut.write(90 - 40 + 30);         // right lower leg
  Rpaha.write(90 + 40 - 0);           // right upper leg
  Rpinggul.write(90 - 40 + 10);       // right hip

  Lpinggul.write(90 + 40 + 10);       // left hip
  Lpaha.write(90 - 40 + 30);          // left upper leg
  Llutut.write(90 + 40 - 0);          // left lower leg
  Lkaki.write(90 + 40 + 10);          // left foot

  RBahu.write(90 + 40 - 20);          // right shoulder
  RLenganAtas.write(90 + 40 + 20);    // right upper arm
  RLenganBawah.write(90 - 40 + 40);   // right lower arm
  RTangan.write(90 + 40);             // right hand

  LBahu.write(90 - 40 + 20);          // left shoulder
  LLenganAtas.write(90 - 40 - 20);    // left upper arm
  LLenganBawah.write(90 + 40 - 40);   // left lower arm
  LTangan.write(90 - 40);             // left hand
  delay(100);


  Rkaki.write(90 - 40 + 10);          // right foot
  Rlutut.write(90 - 40 + 70);         // right lower leg
  Rpaha.write(90 + 40 - 70);          // right upper leg
  Rpinggul.write(90 - 40 + 10);       // right hip

  Lpinggul.write(90 + 40 + 10);       // left hip
  Lpaha.write(90 - 40 + 20);          // left upper leg
  Llutut.write(90 + 40 - 20);         // left lower leg
  Lkaki.write(90 + 40 + 10);          // left foot

  RBahu.write(90 + 40 - 20);          // right shoulder
  RLenganAtas.write(90 + 40 + 20);    // right upper arm
  RLenganBawah.write(90 - 40 + 40);   // right lower arm
  RTangan.write(90 + 40);             // right hand

  LBahu.write(90 - 40 + 20);          // left shoulder
  LLenganAtas.write(90 - 40 - 20);    // left upper arm
  LLenganBawah.write(90 + 40 - 40);   // left lower arm
  LTangan.write(90 - 40);             // left hand
  delay(100);


  Rkaki.write(90 - 40 + 10);          // right foot
  Rlutut.write(90 - 40 + 0);          // right lower leg
  Rpaha.write(90 + 40 - 70);          // right upper leg
  Rpinggul.write(90 - 40 + 10);       // right hip

  Lpinggul.write(90 + 40 + 10);       // left hip
  Lpaha.write(90 - 40 + 20);          // left upper leg
  Llutut.write(90 + 40 - 20);         // left lower leg
  Lkaki.write(90 + 40 + 10);          // left foot

  RBahu.write(90 + 40 - 20);          // right shoulder
  RLenganAtas.write(90 + 40 + 20);    // right upper arm
  RLenganBawah.write(90 - 40 + 40);   // right lower arm
  RTangan.write(90 + 40);             // right hand

  LBahu.write(90 - 40 + 20);          // left shoulder
  LLenganAtas.write(90 - 40 + 20);    // left upper arm
  LLenganBawah.write(90 + 40 - 40);   // left lower arm
  LTangan.write(90 - 40);             // left hand
  delay(100);


  Rkaki.write(90 - 40 + 0);          // right foot
  Rlutut.write(90 - 40 + 0);         // right lower leg
  Rpaha.write(90 + 40 - 30);         // right upper leg
  Rpinggul.write(90 - 40 + 0);       // right hip

  Lpinggul.write(90 + 40 + 0);       // left hip
  Lpaha.write(90 - 40 + 0);          // left upper leg
  Llutut.write(90 + 40 - 30);        // left lower leg
  Lkaki.write(90 + 40 + 0);          // left foot

  RBahu.write(90 + 40 - 20);         // right shoulder
  RLenganAtas.write(90 + 40 + 20);   // right upper arm
  RLenganBawah.write(90 - 40 + 40);  // right lower arm
  RTangan.write(90 + 40);            // right hand

  LBahu.write(90 - 40 + 20);         // left shoulder
  LLenganAtas.write(90 - 40 + 10);   // left upper arm
  LLenganBawah.write(90 + 40 - 40);  // left lower arm
  LTangan.write(90 - 40);            // left hand
  delay(100);


  Rkaki.write(90 - 40 - 10);         // right foot
  Rlutut.write(90 - 40 + 0);         // right lower leg
  Rpaha.write(90 + 40 - 30);         // right upper leg
  Rpinggul.write(90 - 40 - 10);      // right hip

  Lpinggul.write(90 + 40 - 10);      // left hip
  Lpaha.write(90 - 40 + 0);          // left upper leg
  Llutut.write(90 + 40 - 30);        // left lower leg
  Lkaki.write(90 + 40 - 10);         // left foot

  RBahu.write(90 + 40 - 20);         // right shoulder
  RLenganAtas.write(90 + 40 + 20);   // right upper arm
  RLenganBawah.write(90 - 40 + 40);  // right lower arm
  RTangan.write(90 + 40);            // right hand

  LBahu.write(90 - 40 + 20);         // left shoulder
  LLenganAtas.write(90 - 40 - 20);   // left upper arm
  LLenganBawah.write(90 + 40 - 40);  // left lower arm
  LTangan.write(90 - 40);            // left hand
  delay(100);


  Rkaki.write(90 - 40 - 10);         // right foot
  Rlutut.write(90 - 40 + 20);        // right lower leg
  Rpaha.write(90 + 40 - 20);         // right upper leg
  Rpinggul.write(90 - 40 - 10);      // right hip

  Lpinggul.write(90 + 40 - 10);      // left hip
  Lpaha.write(90 - 40 + 70);         // left upper leg
  Llutut.write(90 + 40 - 70);        // left lower leg
  Lkaki.write(90 + 40 - 10);         // left foot

  RBahu.write(90 + 40 - 20);         // right shoulder
  RLenganAtas.write(90 + 40 + 20);   // right upper arm
  RLenganBawah.write(90 - 40 + 40);  // right lower arm
  RTangan.write(90 + 40);            // right hand

  LBahu.write(90 - 40 + 20);         // left shoulder
  LLenganAtas.write(90 - 40 - 20);   // left upper arm
  LLenganBawah.write(90 + 40 - 40);  // left lower arm
  LTangan.write(90 - 40);            // left hand
  delay(50);

}