/*
 * ESP32S3 XIAO
 * make sure that PSRAM is enabled
 */

// servos * * * * * * * * * * * * * * * * * * * *
#include <s3servo.h>
s3servo servo1;
s3servo servo2;
s3servo servo3;
s3servo servo4;
s3servo servo5;
s3servo servo6;

int center1 = 75;     //best center position of each leg (individual)
//int center2 = 105;
int center2 = 125;
int center3 = 90;
int center4 = 90;
int center5 = 115;
int center6 = 75;

int pos = 90;
int pos2;
int Min = 50;         //move betwenn this two values
int Max = 100;
int speed1 = 200;       //pause between forward and backward movement
int steplength = 30;    //big step
int smallstep = 5;      //small step
int flag = 0;

// button * * * * * * * * * * * * * * * * * * * *
const int buttonLeft = 3;
const int buttonRight = 4;
int leftState = 1;
int rightState = 1;



void setup() {
  WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0); //disable brownout detector
  
  Serial.begin(115200);
  Serial.setDebugOutput(false);
  
// button * * * * * * * * * * * * * * * * * * 
  pinMode(buttonLeft, INPUT_PULLUP);
  pinMode(buttonRight, INPUT_PULLUP);

 // servos * * * * * * * * * * * * * * * * * * 
  servo1.attach(6);   //please check the correct wiring
  servo2.attach(5);
  servo3.attach(44);
  servo4.attach(7);
  servo5.attach(8);
  servo6.attach(9);
  delay(20);

  servo1.write(center1);  //all servos in start position
  servo2.write(center2);
  servo3.write(center3);
  servo4.write(center4);
  servo5.write(center5);
  servo6.write(center6);
  delay(2000);              //wait



}

void loop() {
  
forward();
forward();
forward();
forward();
left();
left();
forward();
forward();
forward();
stop();
rise();

}


// walking modes * * * * * * * * * * * * * * * * * * 

void forward() {
  servo3.write(center3 - 10);  //left mid down
  servo4.write(center4 - 10);  //right mid up
  delay(20);
  servo1.write(center1 - steplength);  //left side forward
  delay(30);
  servo5.write(center5 - steplength);
  servo2.write(center2 - steplength);  //right side backwards/give push
  delay(30);
  servo6.write(center6 - steplength);
  delay(speed1);

  servo3.write(center3 + 10);  //left mid up
  servo4.write(center4 + 10);  //right mid down
  delay(20);
  servo1.write(center1 + steplength);  //left side backwards/give push
  delay(30);
  servo5.write(center5 + steplength);
  servo2.write(center2 + steplength);  //right side forward
  delay(30);
  servo6.write(center6 + steplength);
  delay(speed1);
}


void left() {
  servo3.write(center3 - 10);  //left mid down
  servo4.write(center4 - 10);  //right mid up
  delay(20);
  servo1.write(center1 - smallstep);  //left side forward
  servo5.write(center5 - smallstep);
  servo2.write(center2 - steplength);  //right side backwards/give push
  servo6.write(center6 - steplength);
  delay(speed1);

  servo3.write(center3 + 10);  //left mid up
  servo4.write(center4 + 10);  //right mid down
  delay(20);
  servo1.write(center1 + smallstep);  //left side backwards/give push
  servo5.write(center5 + smallstep);
  servo2.write(center2 + steplength);  //right side forward
  servo6.write(center6 + steplength);
  delay(speed1);
}


void right() {
  servo3.write(center3 + 10);  //left mid up
  servo4.write(center4 + 10);  //right mid down
  delay(20);
  servo1.write(center1 + steplength);  //left side backward/give push
  servo5.write(center5 + steplength);
  servo2.write(center2 + smallstep);  //right side forward
  servo6.write(center6 + smallstep);
  delay(speed1);

  servo3.write(center3 - 10);  //left mid down
  servo4.write(center4 - 10);  //right mid up
  delay(20);
  servo1.write(center1 - steplength);  //left side forward
  servo5.write(center5 - steplength);
  servo2.write(center2 - smallstep);  //right side backwards/give push
  servo6.write(center6 - smallstep);
  delay(speed1);
}

void backward() {
  servo3.write(center3 + 10);  //left mid up
  servo4.write(center4 + 10);  //right mid down
  delay(20);
  servo1.write(center1 - steplength);  //left side forward/give push
  delay(30);
  servo5.write(center5 - steplength);
  servo2.write(center2 - steplength);  //right side backwards
  delay(30);
  servo6.write(center6 - steplength);
  delay(speed1 + 100);

  servo3.write(center3 - 10);  //left mid down
  servo4.write(center4 - 10);  //right mid up
  delay(20);
  servo1.write(center1 + steplength);  //left side backwards
  delay(30);
  servo5.write(center5 + steplength);
  servo2.write(center2 + steplength);  //right side forward/give push
  delay(30);
  servo6.write(center6 + steplength);
  delay(speed1 + 100);
}

void rise() {
  servo1.write(center1 - 30);  //front legs up
  servo2.write(center2 + 30);
  servo3.write(center3 - 20);  //left mid down
  servo4.write(center4 + 20);  //right mid down
  servo5.write(center5 - 25);  //rear legs centered
  servo6.write(center6 + 25);
  delay(100);
}
void stop() {
  servo1.write(center1);
  servo2.write(center2);
  servo3.write(center3);
  servo4.write(center4);
  servo5.write(center5);
  servo6.write(center6);
  delay(50);
}

void automode(){
   leftState = digitalRead(buttonLeft);
   rightState = digitalRead(buttonRight);
   delay(50);
  
   if  ((leftState == LOW) && (leftState == HIGH)) {
     Serial.println("stop and go right");
     for (int i = 0; i<=3; i++){
      backward();
     }
     for (int i = 0; i<=3; i++){
      right();
     }
   }
   if  ((leftState == HIGH) && (leftState == LOW)) {
     Serial.println("stop and go left");
     for (int i = 0; i<=3; i++){
      backward();
     }
     for (int i = 0; i<=3; i++){
      left();
     }     
   }
   if  ((leftState == LOW) && (leftState == LOW)) {
     Serial.println("stop, go back and try again");
     for (int i = 0; i<=6; i++){
        backward();
     }
     for (int i = 0; i<=5; i++){
      left();
     }  
   }  
   else {
    forward();
   }
  if (flag = 0) {
    return;
  }
   delay(200);
}