#include <Wire.h>  // Library for I2C communication
#include <LiquidCrystal_I2C.h>  // Library for I2C LCD
#include <Servo.h> // Library for Servo Motors

// Initialize LCD
LiquidCrystal_I2C lcd(0x27, 16, 2); // Address 0x27, 16 columns, 2 rows

// Initialize Servos
Servo servo1, servo2, servo3, servo4, servo5, servo6, servo7;


// Joystick Pins
const int joy1XPin = A0; // Joystick 1 X-axis
const int joy1YPin = A1; // Joystick 1 Y-axis
const int joy2XPin = A2; // Joystick 2 X-axis
const int joy2YPin = A3; // Joystick 2 Y-axis

// Servo Angles
int angle1 = 90, angle2 = 90, angle3 = 90, angle4 = 90;

void setup() {
  // Initialize LCD
  lcd.init();
  lcd.backlight();

  // Attach Servos
  servo1.attach(3); // Pin 9 for servo 3
  servo2.attach(4); // Pin 10 for servo 4
  servo3.attach(5); // Pin 11 for servo 5
  servo4.attach(6);  // Pin 6 for servo 6
  servo5.attach(7);  // Pin 6 for servo 7
  servo6.attach(8);  // Pin 6 for servo 8
  servo7.attach(9);  // Pin 6 for servo 9
 
}

void loop() {
  // Read Joystick 1
  int joy1X = analogRead(joy1XPin);
  int joy1Y = analogRead(joy1YPin);

  // Read Joystick 2
  int joy2X = analogRead(joy2XPin);
  int joy2Y = analogRead(joy2YPin);

  // Map joystick values to servo angles (adjust mapping according to your joysticks)
  angle1 = map(joy1X, 0, 1023, 0, 180);
  angle2 = map(joy1Y, 0, 1023, 0, 180);
  angle3 = map(joy2X, 0, 1023, 0, 180);

  // Control Servos
  servo1.write(angle1);
  servo2.write(angle1);
  servo3.write(angle2);
  servo4.write(angle3);
  servo5.write(angle1);
  servo6.write(angle2);
  servo7.write(angle3);
  // Display messages on LCD based on servo positions
  lcd.clear(); // Clear previous content


         if (angle1 < 90) {
    lcd.clear();
    lcd.print("ROLL RIGHT");
  } else if (angle1 > 90) {
    lcd.clear();
    lcd.print("ROLL LEFT");
  } else if (angle2 < 90) {
    lcd.clear();
    lcd.print("PITCH UP");
  } else if (angle2 > 90) {
    lcd.clear();
    lcd.print("PITCH DOWN");
  } else if (angle3 < 90) {
    lcd.clear();
    lcd.print("YAW RIGHT");
  } else if (angle3 > 90) {
    lcd.clear();
    lcd.print("YAW LEFT");
  } else {
    lcd.clear();

  //delay(1000); // Adjust delay as needed
}
}