AutoClaw 1.4

/*  ARDUINO AUTOCLAW V1.3.7
 *  APSC 101 T2
 *  LAST EDITED BY MANSUR HE, 2016-01-16
 */

// library
#include <Servo.h>

// ---- Pin constants
// Sonar
#define SONAR_ECHO 11
#define SONAR_TRIG 12
#define SONAR_POWER 10
// Servo
#define SERVO_PIN 9

// sonar constants
#define MIN_DISTANCE 5
#define SONAR_DELAY 1000

// servo constants
#define GRAB_TIME 5000
#define SERVO_MIN 40
#define SERVO_MAX 130

// shared variables and instances
bool isActivated;
Servo claw_servo;


void setup() {
  // Enables Serial Monitoring
  Serial.begin(9600);

  // Pin setup
  pinMode(SONAR_ECHO, INPUT);
  pinMode(SONAR_TRIG, OUTPUT);
  pinMode(SONAR_POWER, OUTPUT);

  digitalWrite(SONAR_POWER, HIGH);

  // Servo setup
  claw_servo.attach(SERVO_PIN);
  claw_servo.write(SERVO_MIN);
}


void loop() {

  // Get sonar distance
  int distance = getDistance();

  // Cereal output
  if (! isActivated) {

    // --- IN SONAR STATE
    if (distance >= 200 || distance <= 0) {
      Serial.println("Invalid/Range Error");
    } else {

      // Activate SERVO state when distance is correct
      if (distance < MIN_DISTANCE) {
        isActivated = true;
      }

      // Feedback to serial monitor
      Serial.println(distance);
    }
  } else {

    // --- IN SERVO STATE
    Serial.println("CLAW ACTIVATED...");

    // Servo control
    claw_servo.write(SERVO_MAX);

    // wait for 5 seconds
    delay(GRAB_TIME);
    isActivated = false;
    claw_servo.write(SERVO_MIN);
  }
}


float getDistance() {
  int duration;

  // activate sonar trigger
  digitalWrite(SONAR_TRIG, HIGH);

  // deactivates sonar trigger
  delay(SONAR_DELAY);
  digitalWrite(SONAR_TRIG, LOW);

  // reads the pulse duration
  duration = pulseIn(SONAR_ECHO, HIGH);

  // converts into distance
  return (duration / 2) /29.1;
}