Commit fce6b04d authored by gujames's avatar gujames
Browse files

Update Main_working2/Main_working2.ino

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