Commit e6880be2 authored by sonaraju's avatar sonaraju
Browse files

Changes to read_inputs

parent fce6b04d
...@@ -7,11 +7,12 @@ const int echo_pin_mid= 6; ...@@ -7,11 +7,12 @@ const int echo_pin_mid= 6;
const int trig_pin_right = 5; const int trig_pin_right = 5;
const int echo_pin_right = 4; const int echo_pin_right = 4;
struct Ultrasoundreads{ struct Ultrasoundreads {
long data1, data2, data3 long data1, data2, data3;
} ultrasound; } ultrasound;
Ultrasoundreads data[20]; Ultrasoundreads data[20];
int index = 0; int index = 0;
void setup() { void setup() {
...@@ -25,6 +26,10 @@ void setup() { ...@@ -25,6 +26,10 @@ void setup() {
pinMode(trig_pin_right, OUTPUT); pinMode(trig_pin_right, OUTPUT);
pinMode(echo_pin_right, INPUT); pinMode(echo_pin_right, INPUT);
for (int i = 0; i < 19; ++i) {
data[i] = {0, 0, 0};
}
Serial.begin(9600); // Starts the serial communication Serial.begin(9600); // Starts the serial communication
} }
...@@ -33,6 +38,7 @@ long calculate_distance(long duration) { ...@@ -33,6 +38,7 @@ long calculate_distance(long duration) {
return duration / 58.2; 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) { long read_pin(int trig_pin, int echo_pin, int start_microseconds, int end_microseconds) {
digitalWrite(trig_pin, LOW); digitalWrite(trig_pin, LOW);
delayMicroseconds(start_microseconds); delayMicroseconds(start_microseconds);
...@@ -54,12 +60,20 @@ void read_inputs(long * datapoints) { ...@@ -54,12 +60,20 @@ void read_inputs(long * datapoints) {
long duration_right = read_pin(trig_pin_right, echo_pin_right, 2, 10); long duration_right = read_pin(trig_pin_right, echo_pin_right, 2, 10);
datapoints[2] = calculate_distance(duration_right); datapoints[2] = calculate_distance(duration_right);
ultrasound.data1 = datapoints[0]; ultrasound.data1 = datapoints[0];
ultrasound.data2 = datapoints[1]; ultrasound.data2 = datapoints[1];
ultrasound.data3 = datapoints[2]; 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) { void print_sensors(long * reads) {
...@@ -76,34 +90,44 @@ long average(long a, long b) { ...@@ -76,34 +90,44 @@ long average(long a, long b) {
return (a + b) / 2; 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) { bool check_against(long a, long b, long comp) {
long avg = average(a, b); long avg = average(a, b);
return abs(avg - comp) > 30; return abs(avg - comp) > 30;
} }
double currspeed(){ //calculating current speed of the user
double currspeed() {
long dist1 = (data[19].data1-data[0].data1); long dist1 = (data[19].data1-data[0].data1);
long dist2 = (data[19].data2-data[0].data2); long dist2 = (data[19].data2-data[0].data2);
long dist3 = (data[19].data3-data[0].data3); 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; return speed;
} }
void loop() { void loop() {
//array containg read ins from each US
long reads[3]; long reads[3];
//read in from US sensors
read_inputs(reads); read_inputs(reads);
print_sensors(reads); print_sensors(reads);
bool check_right = check_against(reads[0], reads[1], reads[2]); bool check_right = check_against(reads[0], reads[1], reads[2]);
bool check_left = check_against(reads[1], reads[2], reads[0]); bool check_left = check_against(reads[1], reads[2], reads[0]);
bool check_mid = check_against(reads[0], reads[2], reads[1]); bool check_mid = check_against(reads[0], reads[2], reads[1]);
double time = ((double)reads[1])/currspeed(); double time = ((double)reads[1]) / currspeed();
//time > 3 <- value can be set //time > 3 <- value can be set
if ((check_right || check_left || check_mid) && time>3) { if ((check_right || check_left || check_mid) && time > 3) {
Serial.println("There seems to be an object in the way..."); Serial.println("There seems to be an object in the way...");
} }
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment