#include <WiFi.h>
#include <ESP32Servo.h>

// Define servo objects and their corresponding GPIO pins
Servo th5_b1;  // GPIO 14
Servo th4_b1;  // GPIO 27
Servo th3_b1;  // GPIO 26
Servo th2_b1;  // GPIO 25
Servo th1_b1;  // GPIO 33

void pos_in_b1() {
  // Set initial angles for the servos
  th1_b1.write(90+90);  // Initial angle for th5_b1
  th2_b1.write(34+90);  // Initial angle for th4_b1
  th3_b1.write(43+90);  // Initial angle for th3_b1
  th4_b1.write(-77+90); // Initial angle for th2_b1
  th5_b1.write(0+90);   // Initial angle for th1_b1
  delay(1000);
}
void pos_fin_b1(){
  th1_b1.write(-90+90);  // Initial angle for th5_b1
  th2_b1.write(43+90);  // Initial angle for th4_b1
  th3_b1.write(12+90);  // Initial angle for th3_b1
  th4_b1.write(-55+90); // Initial angle for th2_b1
  th5_b1.write(0+90);   // Initial angle for th1_b1
  delay(1000);
}

void setup() {
  // Attach servos to their respective pins
  th5_b1.attach(14);
  th4_b1.attach(27);
  th3_b1.attach(26);
  th2_b1.attach(25);
  th1_b1.attach(33);
  
  // Call the function to set initial angles
}

void loop() {
  pos_in_b1();
  pos_fin_b1();
  
}