#include "NewPing.h"  // Include NewPing Library
#include <Servo.h>  // Include Servo Library

// Hook up HC-SR04 to Arduino
#define ECHO_PIN 9
#define TRIGGER_PIN 10

// Hook up Servo
#define LEFT_SERVO 8
#define RIGHT_SERVO 7

#define LEFT_MOTOR 4
#define RIGHT_MOTOR 3

Servo leftServo;
Servo rightServo;

Servo leftMotor;
Servo rightMotor;

void setup() {
  // put your setup code here, to run once:
  // setup digital pins (input/output)
  // initialize hardware component
  Serial.begin(9600);
  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(TRIGGER_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  leftServo.attach(8,0,120); // (pin, min, max)
  rightServo.attach(7,0,120);
  leftMotor.attach(4,0,180);
  rightMotor.attach(3,0,180);
;}

float readDistanceCM() {
  digitalWrite(TRIGGER_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIGGER_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIGGER_PIN, LOW);
  int duration = pulseIn(ECHO_PIN, HIGH);
  return duration * 0.034 / 2;
}

int x = 0;
int y = 180;

void loop() {
  // put your main code here, to run repeatedly:
  // READ DISTANCE
  float distance = readDistanceCM();
  bool isNearby = distance < 100;
  digitalWrite(LED_BUILTIN, isNearby);

  // Output Distance (cm)
  Serial.print("Measured distance: ");
  Serial.println(readDistanceCM());
  delay(100);
 
  // Simulate Motors 
  leftMotor.write(x);
  delay(50);
  rightMotor.write(y);
  delay(50);
  x=x+10; // Bottom (Left) Motor
  y=y-10;
  if (x >= 170) {
    x = 0;
  }
  if (y <= 0) {
    y = 180;
  }

  // ACTIVATE EXAPANSION OF WHEEL WHEN CLOSE TO OBSTACLE
  if (isNearby == true ) {
    //Open Configuration
    leftServo.write(0); //set servo to a particular angle
    rightServo.write(120);
    delay(500);
  } 
  else {
    //Closed Configuration
    leftServo.write(120);
    rightServo.write(0);
    delay(500);               
  }
}