Un par de apuntes.
Entiendo por el código que el sensor ultrasónico se desplaza con el brazo. Por lo tanto, si no hay forma de conocer la posición de los brazos en el momento de recoger el contenedor, o no tenemos un sensor de distancia fijo en el camión, no sabremos en qué momento el contenedor vuelve a estar a la distancia inicial (estará en todo momento a distancia cero del brazo, mientras no lo soltemos.
En cuanto al map, tal como está no hace nada, ya que no se está asignando el valor devuelto por la función a nada. Si se borra esa línea no afectará en nada al funcionamiento actual.
Y para finalizar, este es un ejemplo perfecto en el que está plenamente justificado el esfuerzo de hacer una máquina de estados y eliminar delays, para seguir atendiendo sensores o botones de parada, etc...