#include <ESP32Servo.h>
int buttonPin1 = 15; 
int buttonState1 = 0;
int buttonPin2 = 14; 
int buttonState2 = 0;
int buttonPin3 = 27; 
int buttonState3 = 0;

Servo leftLegHip;
Servo leftLegfoot;
Servo rightLegHip;
Servo rightLegfoot;
Servo body;


void setup() {
  
  pinMode(buttonPin1, INPUT); // for movefront
  pinMode(buttonPin2, INPUT); // For turnbackon
  pinMode(buttonPin3, INPUT); // for turnbackoff

  leftLegHip.setPeriodHertz(50);
  leftLegfoot.setPeriodHertz(50);
  leftLegHip.attach(12);  
  leftLegfoot.attach(13); 

 
  rightLegHip.setPeriodHertz(50);
  rightLegfoot.setPeriodHertz(50);
  rightLegHip.attach(4);  
  rightLegfoot.attach(2); 

  body.setPeriodHertz(50);
  body.attach(26); 
  stand_straight();
}

void stand_straight() {
  body.write(0); 
}

void loop() {

  buttonState1 = digitalRead(buttonPin1); 
  buttonState2 = digitalRead(buttonPin2); 
  buttonState3 = digitalRead(buttonPin3); 

  if (buttonState1 == HIGH) {   
   
    movefront();
  }
  
   else {
    
  }
  if (buttonState2 == HIGH) {   
   
    turnbackon();
  }
  
   else {
    
  }
  if (buttonState3 == HIGH) {   
   
   turnbackoff();
  }
  
   else {
    
  }
}

void movefront() {
  
   for (int pos = 0; pos <= 45; pos += 1) { 
   leftLegHip.write(pos);          
  }
   for (int pos = 0; pos <= 30; pos += 1) { 
   leftLegfoot.write(pos);      
  }

  delay(500);  
  for (int pos = 45; pos >= 0; pos -= 1) {  
    leftLegHip.write(pos);  
  }
for (int pos = 30; pos >= 0; pos -= 1) { 
    leftLegfoot.write(pos); 
} 
             
  for (int pos = 0; pos <= 45; pos += 1) { 
   rightLegHip.write(pos);  
             
  }
   for (int pos = 0; pos <= 30; pos += 1) { 
   rightLegfoot.write(pos);  
           
  }
  delay(500);
  for (int pos = 45; pos >= 0; pos -= 1) {  
    rightLegHip.write(pos);  
             
  }
for (int pos = 30; pos >= 0; pos -= 1) {  
    rightLegfoot.write(pos);  
           
  }
  delay(1000);
}

void turnbackon(){ 
  body.write(0);
  delay(15);
  }         
  
  

void turnbackoff(){ 
  body.write(180);
  delay(15);
  }