 
 void loop()
 {
-    long range1 = GetRange1();
-    long range2 = GetRange2();
+    long rangeFront = GetRange(1);
+    long rangeBack = GetRange(2);
 
     uint8_t actualServoPos = ServoMotor.read();
     if(servoPos == actualServoPos)

}


long GetRange1()
{
    long range = sonar1.ping_cm();

    // fallback if return value is out of range
    if(range == 0) { range = maxDistance; }

    return range;
}

long GetRange2()
{
    long range = sonar2.ping_cm();

    // fallback if return value is out of range
    if(range == 0) { range = maxDistance; }

    return range;
}

        delay(5);
    }

    SendDistance(rangeFront);
    //SendDistance(rangeBack);

    nh.spinOnce();
}
