---------------------------------------------------------------------------

//APSC 101-Mariya Fedoroff

// Arduino-  Servo, Sonor, and Serial Monitor

//

//This program reads values gathered by the sonar and shifts

//the servo motor accordingly

// ---------------------------------------------------------------------------


#include <Servo.h>

#define SERVO_PIN 9
#define groundPin 10
#define echoPin 11
#define trigPin 12
#define VccPin 13
#define CLOSE 60
#define OPEN 150

Servo myservo ;


void setup()

{

  Serial.begin(9600);

  pinMode(VccPin, OUTPUT) ;    //pin 13 shall be used as output
  digitalWrite(VccPin, HIGH); //Tell pin 13 to output HIGH (+5V)
  pinMode(echoPin, INPUT);
  pinMode(trigPin, OUTPUT);
  pinMode(groundPin, OUTPUT);
  digitalWrite(groundPin, LOW);


  myservo.attach(9);
  myservo.write(CLOSE);

}


void loop()

{

  delay(200);

  int duration, distance;
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(1000);
  digitalWrite(trigPin, LOW);

  duration = pulseIn(echoPin, HIGH);
  distance = (duration/2)/29.1;

  if(distance>=200 || distance<= 0){
    Serial.println("Out of Range");
  }
  while(distance >= 10){
      Serial.println("Claw is closed: it will remain closed above 10cm or 5 seconds below 10cm");

      myservo.write(CLOSE);
      Serial.print(distance);
      Serial.println(" cm");
      Serial.println("");
      delay(5000);

     // duration = pulseIn(echoPin, HIGH);   I think theres a problem here
     // distance = (duration/2)/29.1;        the distance is always read to be 0 here
     //                                      for some reason
    }

    if(distance < 10){
      myservo.write(OPEN);
      Serial.println("");
      Serial.print(distance);
      Serial.println(" cm");
      Serial.println("");
      Serial.println("Claw is open");
      Serial.println("You have 5 seconds before claw closes!");
      delay(5000);
      myservo.write(CLOSE);

      //duration = pulseIn(echoPin, HIGH);
      //distance = (duration/2)/29.1;

      if(distance<10){
        delay(5000);

    }
  }

}