#include <Servo.h>

// Define pins for servos
const int servoZPin = 9;       // Pin for Z-axis servo
const int servoYPin = 10;      // Pin for Y-axis servo
const int servoExtensionPin = 11; // Pin for extension servo

// Define pins for joysticks
const int joystick1XPin = A0;  // Joystick 1 X-axis (for Z-axis control)
const int joystick1YPin = A1;  // Joystick 1 Y-axis (for Y-axis control)
const int joystick2XPin = A2;  // Joystick 2 X-axis (for extension control)

// Create Servo objects
Servo servoZ;
Servo servoY;
Servo servoExtension;

void setup() {
  Serial.begin(9600);
  
  // Attach servos to their respective pins
  servoZ.attach(servoZPin);
  servoY.attach(servoYPin);
  servoExtension.attach(servoExtensionPin);
}
int ay;
void loop() {
  // Read joystick values from the first joystick
  int joystick1X = analogRead(joystick1XPin); // Read X-axis for Z-axis control
  int joystick1Y = analogRead(joystick1YPin); // Read Y-axis for Y-axis control

  // Read joystick values from the second joystick
  int joystick2X = analogRead(joystick2XPin);

  // Map joystick 1 X-axis to servo Z positions (0 to 180 degrees)
  int servoZPos = map(joystick1X, 0, 1023, 0, 180);
  servoZ.write(servoZPos);

  // Map joystick 1 Y-axis to servo Y positions (0 to 180 degrees)
  int servoYPos = map(joystick1Y, 0, 1023, 0, 180);
  servoY.write(servoYPos);

  // Map joystick 2 X-axis to extension servo positions (0 to 180 degrees)
  int servoExtensionPos = map(joystick2X, 0, 1023, 0, 180);
  servoExtension.write(servoExtensionPos);

  // Print values for debugging


  delay(100); // Small delay for stability
}