#include <ESP32Servo.h>

byte cs1 = 90;
byte cs3 = 90;
byte cs5 = 90;
byte cs7 = 90;

byte cs2 = 110;
byte cs4 = 70;
byte cs6 = 50;
byte cs8 = 130;

byte speed = 5;

Servo servo1, servo2, servo3, servo4, servo5, servo6, servo7, servo8;

void setup(){
  Serial.begin(9600);

  servo1.attach(32); // telapak depan kiri
  servo2.attach(33); // kaki depan kiri
  servo3.attach(23); // telapak depan kanan
  servo4.attach(22); // kaki depan kanan
  servo5.attach(13); // telapak belakang kiri
  servo6.attach(12); // kaki belakang kiri
  servo7.attach(15); // telapak belakang kanan
  servo8.attach(2); // kaki belakang kanan

  servoReset();

}

void loop() {
  servoReset();
  forward();
  forward();
  forward();
  forward();
  forward();
  delay(500);
  turnLeft(20);
  turnLeft(20);
  turnLeft(20);
  turnLeft(20);
  servoReset();
  delay(500);
  backward();
  backward();
  backward();
  backward();
  delay(500);
  turnRight(20);
  turnRight(20);
  turnRight(20);
  turnRight(20);
     
  delay(500);
}

void forward(){    
  leftHand(true);
  delay(30);
  rightHand(false);
  for(int i = 0; i < 40; i++){
    servo(2, 110 + i);
    servo(8, 130 + (i * (-1)));
    servo(4, 30 + i);
    servo(6, 90 + (i * (-1)));
    delay(speed);
  }  
  
  rightHand(true);
  delay(30);
  leftHand(false);  
  for(int i = 40; i > 0; i--){
    servo(2, 110 + i);
    servo(8, 130 + (i * (-1)));
    servo(4, 30 + i);
    servo(6, 90 + (i * (-1)));
    delay(speed);
  }
}

void backward(){    
  rightHand(true);
  delay(30);
  leftHand(false);
  for(int i = 0; i < 40; i++){
    servo(2, 110 + i);
    servo(8, 130 + (i * (-1)));
    servo(4, 30 + i);
    servo(6, 90 + (i * (-1)));
    delay(speed);
  }  
  
  leftHand(true);
  delay(30);
  rightHand(false);  
  for(int i = 40; i > 0; i--){
    servo(2, 110 + i);
    servo(8, 130 + (i * (-1)));
    servo(4, 30 + i);
    servo(6, 90 + (i * (-1)));
    delay(speed);
  }
}

void turnLeft(int s){    
  leftHand(true);
  delay(30);
  rightHand(false);
  for(int i = 0; i < 20; i++){
    servo(2, 100 + (i * (-1)));
    servo(8, 100 + (i * (-1)));
    servo(4, 80 + i);
    servo(6, 80 + i);
    delay(s);
  }  
  
  rightHand(true);
  delay(30);
  leftHand(false);  
  for(int i = 20; i > 0; i--){
    servo(2, 100 + (i * (-1)));
    servo(8, 100 + (i * (-1)));
    servo(4, 80 + i);
    servo(6, 80 + i);
    delay(s);
  }
}

void turnRight(int s){    
  rightHand(true);
  delay(30);
  leftHand(false);
  for(int i = 0; i < 20; i++){
    servo(2, 100 + (i * (-1)));
    servo(8, 100 + (i * (-1)));
    servo(4, 80 + i);
    servo(6, 80 + i);
    delay(s);
  }  
  
  leftHand(true);
  delay(30);
  rightHand(false);  
  for(int i = 20; i > 0; i--){
    servo(2, 100 + (i * (-1)));
    servo(8, 100 + (i * (-1)));
    servo(4, 80 + i);
    servo(6, 80 + i);
    delay(s);
  }
}

void legSet(){
  servo(1, cs1);
  servo(3, cs3);
  servo(5, cs5);
  servo(7, cs7);
}

void rightHand(boolean x){
  if(x){
    cs3 = 90;
    cs5 = 90;
  }else{
    cs3 = 110;
    cs5 = 70;
  }
  servo(3, cs3);
  servo(5, cs5);  
}

void leftHand(boolean x){
  if(x){
    cs1 = 90;
    cs7 = 90;
  }else{
    cs1 = 70;
    cs7 = 110;
  }
  servo(1, cs1);
  servo(7, cs7);  
}

void servoReset(){
  cs2 = 90;
  cs4 = 90;
  cs6 = 90;
  cs8 = 90; 

  cs1 = 90;
  cs3 = 90;
  cs5 = 90;
  cs7 = 90;
  
  servo(1, cs1);
  servo(2, cs2);
  servo(3, cs3);
  servo(4, cs4);
  servo(5, cs5);
  servo(6, cs6);
  servo(7, cs7);
  servo(8, cs8);
}

void servo(byte n, byte c){
  switch(n){
    case 1:
      servo1.write(c);
    break;
    case 2:
      servo2.write(c);
    break;
    case 3:
      servo3.write(c);
    break;
    case 4:
      servo4.write(c);
    break;
    case 5:
      servo5.write(c);
    break;
    case 6:
      servo6.write(c);
    break;
    case 7:
      servo7.write(c);
    break;
    case 8:
      servo8.write(c);
    break;
  }
}