#include "Adafruit_LEDBackpack.h"
#include "Adafruit_GFX.h"
#include <Wire.h>

const short num_samples = 20;
int vcc = 2; //attach pin 2 to vcc
int trig = 3; // attach pin 3 to Trig
int echo = 4; //attach pin 4 to Echo
int gnd = 5; //attach pin 5 to GND
Adafruit_7segment matrix = Adafruit_7segment();
long duration, cm;

void setup() {

pinMode (vcc,OUTPUT);
pinMode (gnd,OUTPUT);
// initialize serial communication:
Serial.begin(9600);
matrix.begin(0x70);
}

void loop()
{
  duration = 0;
  for (int i = 0; i < num_samples; i++) {
    digitalWrite(vcc, HIGH);
    // establish variables for duration of the ping,
    // and the distance result in inches and centimeters:
    
    // The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
    // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
    pinMode(trig, OUTPUT);
    digitalWrite(trig, LOW);
    delayMicroseconds(2);
    digitalWrite(trig, HIGH);
    delayMicroseconds(5);
    digitalWrite(trig, LOW);
    
    // The same pin is used to read the signal from the PING))): a HIGH
    // pulse whose duration is the time (in microseconds) from the sending
    // of the ping to the reception of its echo off of an object.
    pinMode(echo,INPUT);
    duration += pulseIn(echo, HIGH);
    delay(2);
  }
  cm = microsecondsToCentimeters(duration/num_samples);
  
  matrix.print(cm);
  matrix.writeDisplay();
  
  Serial.print(cm);
  Serial.print("cm");
  Serial.println();
  
  delay(100);
}

long microsecondsToInches(long microseconds)
{
// According to Parallax's datasheet for the PING))), there are
// 73.746 microseconds per inch (i.e. sound travels at 1130 feet per
// second). This gives the distance travelled by the ping, outbound
// and return, so we divide by 2 to get the distance of the obstacle.
// See: http://www.parallax.com/dl/docs/prod/acc/28015-PI...
return microseconds / 74 / 2;
}

long microsecondsToCentimeters(long microseconds)
{
// The speed of sound is 340 m/s or 29 microseconds per centimeter.
// The ping travels out and back, so to find the distance of the
// object we take half of the distance travelled.
return microseconds / 29 / 2;
}