Commit f18236cc authored by gujames's avatar gujames
Browse files

fix again

parent 36930b29
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...");
}
}
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