#include <Servo.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

const int servoPin1 = 9;  // Choose the appropriate PWM pin on the Arduino Mega
const int servoPin2 = 10; // Choose another PWM pin
const int servoPin3 = 11; // Choose another PWM pin

Servo servo1;
Servo servo2;
Servo servo3;

LiquidCrystal_I2C lcd(0x27, 16, 2);  // I2C address may vary (0x27 is common for many modules)

void setup() {
  servo1.attach(servoPin1);
  servo2.attach(servoPin2);
  servo3.attach(servoPin3);

  lcd.begin(16, 2);  // Initialize the LCD module with 16 columns and 2 rows
  lcd.clear();       // Clear the LCD screen
}

void loop() {
  // Loop to gradually increase the angle for all three servos
  for (int angle = 0; angle <= 180; angle++) {
    servo1.writeMicroseconds(map(angle, 0, 180, 1000, 2000));
    servo2.writeMicroseconds(map(angle, 0, 180, 1000, 2000));
    servo3.writeMicroseconds(map(angle, 0, 180, 1000, 2000));

    lcd.clear();  // Clear the LCD screen before displaying new data
    lcd.setCursor(0, 0);
    lcd.print("Servo Angles:");

    lcd.setCursor(0, 1);
    lcd.print("S1: ");
    lcd.print(angle);

    delay(20);  // Adjust the delay based on your servo's speed
  }

  // Delay before going to the next cycle
  delay(1000);
    // Loop to gradually increase the angle for all three servos
  for (int angle = 180; angle >= 0; angle) {
    servo1.writeMicroseconds(map(angle, 0, 180, 1000, 2000));
    servo2.writeMicroseconds(map(angle, 0, 180, 1000, 2000));
    servo3.writeMicroseconds(map(angle, 0, 180, 1000, 2000));

    lcd.clear();  // Clear the LCD screen before displaying new data
    lcd.setCursor(0, 0);
    lcd.print("Servo Angles:");

    lcd.setCursor(0, 1);
    lcd.print("S1: ");
    lcd.print(angle);

    delay(20);  // Adjust the delay based on your servo's speed
  }

  // Delay before going to the next cycle
  delay(1000);
}