Changes to read_inputs

Main_working2/Main_working2.ino

@@ -7,11 +7,12 @@ const int echo_pin_mid= 6;
 const int trig_pin_right = 5;
 const int echo_pin_right = 4;
 
-struct Ultrasoundreads{
-  long data1, data2, data3
+struct Ultrasoundreads {
+  long data1, data2, data3;
 } ultrasound;
 
 Ultrasoundreads data[20];
+
 int index = 0;
 
 void setup() {
@@ -26,6 +27,10 @@ void setup() {
   pinMode(trig_pin_right, OUTPUT); 
   pinMode(echo_pin_right, INPUT); 
 
+  for (int i = 0; i < 19; ++i) {
+    data[i] = {0, 0, 0};
+  }
+  
   Serial.begin(9600); // Starts the serial communication
 }
 
@@ -33,6 +38,7 @@ long calculate_distance(long duration) {
   return duration / 58.2; 
 }
 
+//read data from one US sensor
 long read_pin(int trig_pin, int echo_pin, int start_microseconds, int end_microseconds) {
   digitalWrite(trig_pin, LOW);
   delayMicroseconds(start_microseconds);
@@ -54,12 +60,20 @@ void read_inputs(long * datapoints) {
 
   long duration_right = read_pin(trig_pin_right, echo_pin_right, 2, 10);
   datapoints[2] = calculate_distance(duration_right);
+  
   ultrasound.data1 = datapoints[0];
   ultrasound.data2 = datapoints[1];
   ultrasound.data3 = datapoints[2];
-  data[(index++)%20] = ultrasound;
   
+  //data[(index++)%20] = ultrasound;
 
+  //shift each element in data back one spot in the array
+  for (int i = 19; i > 0; --i) {
+    data[i-1] = data[i];
+  }
+
+  //update the last element of data
+  data[19] = ultrasound;
 }
 
 void print_sensors(long * reads) {
@@ -76,34 +90,44 @@ long average(long a, long b) {
   return (a + b) / 2;
 }
 
+//compare the avg distance read by 2 sensors and compare to the third, if large distance between them, then true
+//would not detect: large objects spanning all 3 sensors, multiple objects blocking mult sensors, 
 bool check_against(long a, long b, long comp) {
   long avg = average(a, b);
   return abs(avg - comp) > 30;
 }
 
-double currspeed(){
+//calculating current speed of the user
+double currspeed() {
   
   long dist1 = (data[19].data1-data[0].data1);
   long dist2 = (data[19].data2-data[0].data2);
   long dist3 = (data[19].data3-data[0].data3);
 
-  double speed = ((double) (dist1+dist2+dist3)*2)/((double) 3);
+  double speed = ((double)(dist1 + dist2 + dist3) * 2) / ((double) 3);
   return speed;
 }
 
 
 void loop() {
+  //array containg read ins from each US
   long reads[3];
+
+  //read in from US sensors
   read_inputs(reads);
+
+  
   print_sensors(reads);
+  
+  
   bool check_right = check_against(reads[0], reads[1], reads[2]);
   bool check_left = check_against(reads[1], reads[2], reads[0]);
   bool check_mid = check_against(reads[0], reads[2], reads[1]);
 
-  double time = ((double)reads[1])/currspeed();
-  //time > 3 <- value can be set
-  if ((check_right || check_left || check_mid) && time>3) {
+  double time = ((double)reads[1]) / currspeed();
   
+  //time > 3 <- value can be set
+  if ((check_right || check_left || check_mid) && time > 3) {
 
     Serial.println("There seems to be an object in the way...");
   }