fix again

10102021_main.ino

+const int trig_pin_left = 9;
+const int echo_pin_left = 8;
 
-const int trigPin = 12;
-const int echoPin = 11;
+const int trig_pin_mid = 7;
+const int echo_pin_mid= 6;
 
-const int trigPin2 = 8;
-const int echoPin2 = 9;
+const int trig_pin_right = 5;
+const int echo_pin_right = 4;
 
-const int motor = 3;
+struct Ultrasoundreads {
+  long data1, data2, data3;
+} ultrasound;
 
-const int button = 13;
+Ultrasoundreads data[20];
 
-const int potentiometer = A0;
-
-long duration2;
-long duration;
-
-int distance;
-int distance2;
-
-int volume;
-Int volumeScaled;
+int index = 0;
 
 void setup() {
+  // put your setup code here, to run once:
+  // add reading of specific pins
+  pinMode(trig_pin_left, OUTPUT); 
+  pinMode(echo_pin_left, INPUT); 
+  
+  pinMode(trig_pin_mid, OUTPUT); 
+  pinMode(echo_pin_mid, INPUT); 
+  
+  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
+}
 
-    //US1
-    pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
-    pinMode(echoPin, INPUT); // Sets the echoPin as an Input
+long calculate_distance(long duration) {
+  return duration / 58.2; 
+}
 
-    //US2
-    pinMode(trigPin2, OUTPUT); // Sets the trigPin as an Output
-    pinMode(echoPin2, INPUT); // Sets the echoPin as an Input
+//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);
 
-    pinMode(potentiometer, INPUT);
-    pinMode(button, INPUT_PULLUP);
-    pinMode(motor, OUTPUT);
+  digitalWrite(trig_pin, HIGH);
+  
+  delayMicroseconds(end_microseconds);
+  digitalWrite(trig_pin, LOW);
 
-    Serial.begin(9600); // Starts the serial communication
+  return pulseIn(echo_pin, HIGH);
 }
 
-void loop() {
+void read_inputs(long * datapoints) {
+  long duration_left = read_pin(trig_pin_left, echo_pin_left, 2, 10);
+  datapoints[0] = calculate_distance(duration_left);
+  
+  long duration_mid = read_pin(trig_pin_mid, echo_pin_mid, 2, 10);
+  datapoints[1] = calculate_distance(duration_mid);
+
+  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;
+
+  //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) {
+  Serial.print("Sensor data: ");
+  Serial.print(reads[0]);
+  Serial.print(" ");
+  Serial.print(reads[1]);
+  Serial.print(" ");
+  Serial.print(reads[2]);
+  Serial.println();
+}
 
-    if(digitalRead(button)== HIGH) {
-        // Clears the trigPin
-        digitalWrite(trigPin2, LOW);
-        digitalWrite(trigPin, LOW);
-        delayMicroseconds(2);
-        
-        // Sets the trigPin on HIGH state for 10 micro seconds
-        digitalWrite(trigPin2, HIGH);
-        digitalWrite(trigPin, HIGH);
-        
-        delayMicroseconds(10);
-        digitalWrite(trigPin2, LOW);
-        digitalWrite(trigPin, LOW);
-        
-        // Reads the echoPin, returns the sound wave travel time in microseconds
-        duration2 = pulseIn(echoPin2, HIGH);
-        duration = pulseIn(echoPin, HIGH);
-
-        //Potentiometer
-        volume = analogRead(potentiometer);
-        volumeScaled = volume* 0.25;
-        
-        // Calculating the distance
-        distance = duration * 0.034/2;
-        distance2 = duration2 * 0.034/2;
-
-        if (distance2 > 30) {
-        Serial.println("No output");
-        }111    
-
-        if (distance2 < 30)   {
-                if (distance < 75) {
-                        Serial.println(distance);
-                        analogWrite(motor, volumeScaled);  
-                        delay(distance * 15);
-            }
-
-        else{
-            
-        }
-    }
-
-    else{
-        digitalWrite(motor,LOW);
-    }
+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;
 }
 
-    if(digitalRead(button)== LOW) {
-        digitalWrite(motor, LOW);
-    }
+//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);
+  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) {
+
+    Serial.println("There seems to be an object in the way...");
+  }
+}