add a virtual delay to the main.ino (needs testing)

main/main.ino

@@ -57,14 +57,23 @@ void loop() {
   
 }
 
+void turnOnLight(int trigPin, int time1, int time2) {
+  VirtualDelay delay1;
+  VirtualDelay delay2;
+  delay1.start(time1);
+  if (delay1.elapsed()) {
+    digitalWrite(trigPin, HIGH);
+    delay2.start(time2);
+    if (delay2.elapsed()) {
+      digitalWrite(trigPin, LOW);
+    }
+  }
+}
+
 void usSensor(int trigPin, int echoPin, int sensorNum) {
   int ledPin = 0;
   
-  digitalWrite(trigPin, LOW);
-  delay(2);
-  digitalWrite(trigPin, HIGH);
-  delay(100);
-  digitalWrite(trigPin, LOW);
+  turnOnLight(trigPin, 2, 100);
 
   duration = pulseIn(echoPin, HIGH);
   int distance = duration * (0.034/2);
@@ -91,9 +100,7 @@ void usSensor(int trigPin, int echoPin, int sensorNum) {
 
   if (distance < 30) {
    
-   digitalWrite(ledPin, HIGH);
-   delay(distance * 15);   
-   digitalWrite(ledPin, LOW);  
+   turnOnLight(ledPin, distance * 15, 0);
    //delay(distance * 15);   
   
    Serial.print("Obstacle detected: Sensor ");