73 lines
2.8 KiB
C++
73 lines
2.8 KiB
C++
#define leftSensorPin A1 // Sharp 2Y0A21 sensor (10 to 80 cm) - Left
|
|
#define rightSensorPin A2 // Sharp 2Y0A21 sensor (10 to 80 cm) - Right
|
|
#define relayPin 2 // Digital pin for relay
|
|
#define confirmationPin 3 // Digital pin for confirmation input
|
|
#define numReadings 10 // Number of readings to average for the moving average
|
|
|
|
int leftReadings[numReadings]; // Array to store the left sensor readings
|
|
int rightReadings[numReadings]; // Array to store the right sensor readings
|
|
int leftReadIndex = 0; // Index of the current left sensor reading
|
|
int rightReadIndex = 0; // Index of the current right sensor reading
|
|
int leftTotal = 0; // Sum of the left sensor readings
|
|
int rightTotal = 0; // Sum of the right sensor readings
|
|
int leftAverage = 0; // Average of the left sensor readings
|
|
int rightAverage = 0; // Average of the right sensor readings
|
|
|
|
void setup() {
|
|
Serial.begin(9600);
|
|
pinMode(leftSensorPin, INPUT);
|
|
pinMode(rightSensorPin, INPUT);
|
|
pinMode(relayPin, OUTPUT);
|
|
pinMode(confirmationPin, INPUT);
|
|
|
|
for (int i = 0; i < numReadings; i++) {
|
|
leftReadings[i] = 0; // Initialize the left sensor readings array
|
|
rightReadings[i] = 0; // Initialize the right sensor readings array
|
|
}
|
|
}
|
|
|
|
void loop() {
|
|
if (digitalRead(confirmationPin) == HIGH) {
|
|
// Process left sensor readings
|
|
leftTotal = leftTotal - leftReadings[leftReadIndex];
|
|
int leftRawValue = analogRead(leftSensorPin);
|
|
int leftDistance = 123.221814 * exp(-0.005895 * leftRawValue);
|
|
leftTotal = leftTotal + leftDistance;
|
|
leftReadings[leftReadIndex] = leftDistance;
|
|
leftReadIndex = (leftReadIndex + 1) % numReadings;
|
|
leftAverage = leftTotal / numReadings;
|
|
|
|
// Process right sensor readings
|
|
rightTotal = rightTotal - rightReadings[rightReadIndex];
|
|
int rightRawValue = analogRead(rightSensorPin);
|
|
int rightDistance = 123.221814 * exp(-0.005895 * rightRawValue);
|
|
rightTotal = rightTotal + rightDistance;
|
|
rightReadings[rightReadIndex] = rightDistance;
|
|
rightReadIndex = (rightReadIndex + 1) % numReadings;
|
|
rightAverage = rightTotal / numReadings;
|
|
|
|
// Print the raw values and the averaged distances to the Serial Monitor
|
|
Serial.print("Left Raw Value: ");
|
|
Serial.print(leftRawValue);
|
|
Serial.print(", Left Distance: ");
|
|
Serial.print(leftAverage);
|
|
Serial.println(" cm");
|
|
|
|
Serial.print("Right Raw Value: ");
|
|
Serial.print(rightRawValue);
|
|
Serial.print(", Right Distance: ");
|
|
Serial.print(rightAverage);
|
|
Serial.println(" cm");
|
|
|
|
// Activate relay if left distance is greater than or equal to right distance
|
|
if (leftAverage >= rightAverage) {
|
|
digitalWrite(relayPin, HIGH);
|
|
} else {
|
|
digitalWrite(relayPin, LOW);
|
|
}
|
|
|
|
delay(250); // Approximately 250 ms delay
|
|
} else {
|
|
digitalWrite(relayPin, LOW); // Ensure the relay is off when not in measurement mode
|
|
}
|
|
}
|