claw text

#include <Servo.h>
#define GroundPin 10
#define echoPin 11
#define trigPin 12
#define VccPin 13
#define lowerThreshold 7
#define upperThreshold 10
#define lowDegree 12
#define highDegree 100

/* Servo ports:
 * Orange: 9
 * Red: 5V
 * Brown: GND
 *
 * Sonar ports:
 * Vcc: 13
 * Trig: 12
 * Echo: 11
 * GND: 10
 */

Servo myservo;
int distance, duration;
int pos = 0;
int toggle = 0;
int spd = 5; //sets the amount of time that it takes for the servo to turn 180 degrees

void setup() {
  myservo.attach(9);
  Serial.begin(9600);
  pinMode(VccPin, OUTPUT) ;    //tell pin 13 it is going to be an output
  digitalWrite(VccPin, HIGH) ; //tell pin 13 to output HIGH (+5V)
  pinMode(echoPin, INPUT);     //tell pin 11 it is going to be an input
  pinMode(trigPin, OUTPUT);    //tell pin 12 it is going to be an output
  pinMode(GroundPin, OUTPUT) ; //tell pin 10 it is going to be an output
  digitalWrite(GroundPin,LOW) ;//tell pin 10 to output LOW (0V, or ground)
}

void loop() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(1000);
  digitalWrite(trigPin, HIGH);           //set trigger pin to HIGH
  delayMicroseconds(1000);               //wait 1000 microseconds
  digitalWrite(trigPin, LOW);            //set trigger pin to LOW
  duration = pulseIn(echoPin, HIGH);     //read echo pin
  distance = (duration/2) / 29.1;        //compute distance from duration of echo Pin

  if (distance <= lowerThreshold && distance >=0 && toggle == 0) {
    //picks up object
    delay(1000);
    for (pos = lowDegree; pos <= highDegree; pos++) {
      myservo.write(pos);
      delay(spd);
    }
    toggle = 1;
  }
  else if (toggle == 1 && distance >= upperThreshold && distance <= 200){
    //holding object above 20cm
    toggle = 2;
  }
  else if (toggle == 2 && distance <= lowerThreshold && distance >= 0) {
    //once claw is lowered below 10cm, drops object
    delay(1000);
    for (pos = highDegree; pos >= lowDegree; pos--) {
      myservo.write(pos);
      delay(spd);
    }
    toggle = 3;
  }
  else if (toggle == 3 && distance >= upperThreshold && distance <= 200){
    //once claw is back above 20cm, restarts sequence
    toggle = 0;
  }
  delay(500);
}