// pin definitions
#define trig_pin_LS 23
#define trig_pin_RS 21
// left wheel pins
#define range_LS 26  // LS echo pin - 16 on the actual project
// right motor pins
#define range_RS 18// RS echo pin
// pin 2 will flash for debug only 
#define LED 2  
#define SPEED_SOUND 0.0343  // in cm/us
// trigger config
const int trig_freq = 16; // 16Hz
const int trig_led_Channel_1 = 0;
const int trig_led_Channel_2 = 2;
const int resolution = 16;
// distance cal variables
long period_LS;
long distance_LS;
long period_RS;
long distance_RS;
// function prototypes
long distance_LRS();
long distance_RRS();
void setup() {
  Serial.begin(115200); // uart setup - debugging 
  // set up the input pins
  pinMode(range_LS, INPUT);
  pinMode(range_RS, INPUT);
}

void loop(){
  // debugging 
  delay(500);
  digitalWrite(LED,HIGH);
  delay(500);
  digitalWrite(LED,LOW);

  // convert the time into a distance
  distance_LS =  distance_LRS();
  distance_RS =  distance_RRS();

  // Prints the distance in the Serial Monitor
  Serial.print("Distance for left sensor(cm): ");
  Serial.println(distance_LS);
  Serial.print("Distance for right sensor(cm): ");
  Serial.println(distance_RS);
}
// distance calculator
// The right wheel works with the left sensor and vice-versa
long distance_LRS(){
  // trigger signal for left sensor setup 
  ledcSetup(trig_led_Channel_1, trig_freq, resolution); //set pwn freq and resulution
  ledcAttachPin(trig_pin_LS, trig_led_Channel_1); //select pwm output pin and channel
  ledcWrite(trig_led_Channel_1, 11); // set dutycycle to 11/65536 (10u out of 62.5ms)
  // Read in pulse from left sensor echo pin
  period_LS = pulseIn(range_LS, HIGH);
  // return the distance of object from left sensor
  return ((period_LS * SPEED_SOUND) / 2);
}
long distance_RRS(){
  // trigger signal for right sensor setup 
  ledcSetup(trig_led_Channel_2, trig_freq, resolution); //set pwn freq and resulution
  ledcAttachPin(trig_pin_RS, trig_led_Channel_2); //select pwm output pin and channel
  ledcWrite(trig_led_Channel_2, 11); // set dutycycle to 11/65536 (10u out of 62.5ms)
  // Read in pulse from right sensor echo pin
  period_RS  = pulseIn(range_RS, HIGH);
  // return the distance of object from left sensor
  return ((period_RS * SPEED_SOUND) / 2);
}