Update Main_working2/Main_working2.ino

Main_working2/Main_working2.ino

@@ -7,6 +7,13 @@ const int echo_pin_mid= 6;
 const int trig_pin_right = 5;
 const int echo_pin_right = 4;
 
+struct Ultrasoundreads{
+  long data1, data2, data3
+} ultrasound;
+
+Ultrasoundreads data[20];
+int index = 0;
+
 void setup() {
   // put your setup code here, to run once:
   // add reading of specific pins
@@ -47,15 +54,21 @@ 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;
+
+
 }
 
-void print_sensors(long * data) {
-  Serial.print("Sensor Data: ");
-  Serial.print(data[0]);
+void print_sensors(long * reads) {
+  Serial.print("Sensor data: ");
+  Serial.print(reads[0]);
   Serial.print(" ");
-  Serial.print(data[1]);
+  Serial.print(reads[1]);
   Serial.print(" ");
-  Serial.print(data[2]);
+  Serial.print(reads[2]);
   Serial.println();
 }
 
@@ -68,16 +81,30 @@ bool check_against(long a, long b, long comp) {
   return abs(avg - comp) > 30;
 }
 
+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() {
-  long data[3];
-  read_inputs(data);
-  print_sensors(data);
-  bool check_right = check_against(data[0], data[1], data[2]);
-  bool check_left = check_against(data[1], data[2], data[0]);
-  bool check_mid = check_against(data[0], data[2], data[1]);
-  
-  if (check_right || check_left || check_mid) {
+  long reads[3];
+  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...");
   }
 }