#include <Servo.h>
#include <NewPing.h>

// Define the servo objects for each joint
Servo joint1, joint2, joint3, joint4, joint5;

// Define joystick pins
#define JOYSTICK_X A0
#define JOYSTICK_Y A1

// Define ultrasonic sensor pins
#define TRIG_PIN 7
#define ECHO_PIN 6
#define MAX_DISTANCE 200

NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE);

void setup() {
  // Attach each servo to a pin
  joint1.attach(9);
  joint2.attach(10);
  joint3.attach(11);
  joint4.attach(3);
  joint5.attach(4);
}

void loop() {
  // Read joystick values
  int x = analogRead(JOYSTICK_X);
  int y = analogRead(JOYSTICK_Y);

  // Map joystick values to angles for joints 1 and 2
  int angle1 = map(x, 0, 1023, 0, 180);
  int angle2 = map(y, 0, 1023, 0, 180);

  // Read the distance from the ultrasonic sensor for joint 3
  int distance = sonar.ping_cm();
  if (distance == 0) {
    distance = MAX_DISTANCE;  // Handle out-of-range distances
  }
  int angle3 = map(distance, 0, MAX_DISTANCE, 0, 180);

  // Set fixed angles for joints 4 and 5 (if no dynamic input is available)
  int angle4 = 90; // Fixed angle for joint 4
  int angle5 = 90; // Fixed angle for joint 5

  // Move the servos to the calculated angles
  joint1.write(angle1);
  joint2.write(angle2);
  joint3.write(angle3);
  joint4.write(angle4);
  joint5.write(angle5);

  delay(50); // Small delay for smooth motion
}