#include <Servo.h>
#include <AFMotor.h>       // For DC motors
#include <NewPing.h>       // For ultrasonic sensor
#include <ESP32Servo.h>     // For servos
#include <Ultrasonic.h>     // For HC-SR04 ultrasonic sensor


// Ultrasonic setup
#define TRIG_PIN 7
#define ECHO_PIN 8
#define MAX_DISTANCE 200
NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE);

// DC motors (using Adafruit Motor Shield)
AF_DCMotor leftMotor(1);
AF_DCMotor rightMotor(2);

// Servos (arm)
Servo clawMotor;
Servo shoulderMotor;
Servo elbowMotor;
Servo baseMotor;

void setup() {
  Serial.begin(9600);

  // Attach servos
  clawMotor.attach(3);
  shoulderMotor.attach(9);
  elbowMotor.attach(5);
  baseMotor.attach(6);

  // Initialize DC motors
  leftMotor.setSpeed(150);
  rightMotor.setSpeed(150);
}

void loop() {
  // Example: obstacle detection
  int distance = sonar.ping_cm();
  if (distance > 0 && distance < 20) {
    // Defensive move: stop and raise arm
    leftMotor.run(RELEASE);
    rightMotor.run(RELEASE);
    shoulderMotor.write(90);
    clawMotor.write(45);
  } else {
    // Move forward
    leftMotor.run(FORWARD);
    rightMotor.run(FORWARD);
  }
}