#include <CAN.h>


#define sensor_01_ECHO_PIN 6
#define sensor_01_TRIG_PIN 7
#define sensor_02_ECHO_PIN 4
#define sensor_02_TRIG_PIN 5
#define sensor_03_ECHO_PIN 2
#define sensor_03_TRIG_PIN 3

void setup() {
  Serial.begin(9600);

  // Set trigger pins as OUTPUT
  pinMode(sensor_01_TRIG_PIN, OUTPUT);
  pinMode(sensor_02_TRIG_PIN, OUTPUT);
  pinMode(sensor_03_TRIG_PIN, OUTPUT);

  // Set echo pins as INPUT
  pinMode(sensor_01_ECHO_PIN, INPUT);
  pinMode(sensor_02_ECHO_PIN, INPUT);
  pinMode(sensor_03_ECHO_PIN, INPUT);
}

long readUltrasonicDistance(int triggerPin, int echoPin) {
  digitalWrite(triggerPin, LOW);
  delayMicroseconds(2);
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin, LOW);

  long duration = pulseIn(echoPin, HIGH);
  long distance = duration * 0.034 / 2; // Convert to distance in cm
  return distance;
}

void loop() {
  long distance1 = readUltrasonicDistance(sensor_01_TRIG_PIN, sensor_01_ECHO_PIN);
  long distance2 = readUltrasonicDistance(sensor_02_TRIG_PIN, sensor_02_ECHO_PIN);
  long distance3 = readUltrasonicDistance(sensor_03_TRIG_PIN, sensor_03_ECHO_PIN);

  Serial.print("Sensor 1 Distance: ");
  Serial.println(distance1);
  Serial.print("Sensor 2 Distance: ");
  Serial.println(distance2);
  Serial.print("Sensor 3 Distance: ");
  Serial.println(distance3);

  delay(500);
}