Main_working2.ino 3.26 KB
Newer Older
1
2
3
4
5
6
7
8
9
const int trig_pin_left = 9;
const int echo_pin_left = 8;

const int trig_pin_mid = 7;
const int echo_pin_mid= 6;

const int trig_pin_right = 5;
const int echo_pin_right = 4;

sonaraju's avatar
sonaraju committed
10
11
struct Ultrasoundreads {
  long data1, data2, data3;
12
13
14
} ultrasound;

Ultrasoundreads data[20];
sonaraju's avatar
sonaraju committed
15

16
17
int index = 0;

18
19
20
21
22
23
24
25
26
27
28
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); 
sonaraju's avatar
sonaraju committed
29
30
31
32

  for (int i = 0; i < 19; ++i) {
    data[i] = {0, 0, 0};
  }
33
34
35
36
37
38
39
40
  
  Serial.begin(9600); // Starts the serial communication
}

long calculate_distance(long duration) {
  return duration / 58.2; 
}

sonaraju's avatar
sonaraju committed
41
//read data from one US sensor
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
long read_pin(int trig_pin, int echo_pin, int start_microseconds, int end_microseconds) {
  digitalWrite(trig_pin, LOW);
  delayMicroseconds(start_microseconds);

  digitalWrite(trig_pin, HIGH);
  
  delayMicroseconds(end_microseconds);
  digitalWrite(trig_pin, LOW);

  return pulseIn(echo_pin, HIGH);
}

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);
sonaraju's avatar
sonaraju committed
63
  
64
65
66
  ultrasound.data1 = datapoints[0];
  ultrasound.data2 = datapoints[1];
  ultrasound.data3 = datapoints[2];
sonaraju's avatar
sonaraju committed
67
68
  
  //data[(index++)%20] = ultrasound;
69

sonaraju's avatar
sonaraju committed
70
71
72
73
  //shift each element in data back one spot in the array
  for (int i = 19; i > 0; --i) {
    data[i-1] = data[i];
  }
74

sonaraju's avatar
sonaraju committed
75
76
  //update the last element of data
  data[19] = ultrasound;
77
78
}

79
80
81
void print_sensors(long * reads) {
  Serial.print("Sensor data: ");
  Serial.print(reads[0]);
82
  Serial.print(" ");
83
  Serial.print(reads[1]);
84
  Serial.print(" ");
85
  Serial.print(reads[2]);
86
87
88
89
90
91
92
  Serial.println();
}

long average(long a, long b) {
  return (a + b) / 2;
}

sonaraju's avatar
sonaraju committed
93
94
//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, 
95
96
97
98
99
bool check_against(long a, long b, long comp) {
  long avg = average(a, b);
  return abs(avg - comp) > 30;
}

sonaraju's avatar
sonaraju committed
100
101
//calculating current speed of the user
double currspeed() {
102
103
104
105
106
  
  long dist1 = (data[19].data1-data[0].data1);
  long dist2 = (data[19].data2-data[0].data2);
  long dist3 = (data[19].data3-data[0].data3);

sonaraju's avatar
sonaraju committed
107
  double speed = ((double)(dist1 + dist2 + dist3) * 2) / ((double) 3);
108
109
110
  return speed;
}

111
112

void loop() {
sonaraju's avatar
sonaraju committed
113
  //array containg read ins from each US
114
  long reads[3];
sonaraju's avatar
sonaraju committed
115
116

  //read in from US sensors
117
  read_inputs(reads);
sonaraju's avatar
sonaraju committed
118
119

  
120
  print_sensors(reads);
sonaraju's avatar
sonaraju committed
121
122
  
  
123
124
125
126
  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]);

sonaraju's avatar
sonaraju committed
127
128
  double time = ((double)reads[1]) / currspeed();
  
129
  //time > 3 <- value can be set
sonaraju's avatar
sonaraju committed
130
  if ((check_right || check_left || check_mid) && time > 3) {
131

132
133
134
    Serial.println("There seems to be an object in the way...");
  }
}