Giro y avance incorrecto

¡Hola a todos!.
A ver si alguien puede ayudarme con algo muy simple pero que el robot no ejecuta correctamente. Para el programa de atravesar el campo de obstáculos quiero implementar un programa que detecte un obstáculo, retroceda, gire a un lado, avance una distancia y gire en sentido contrario para recolocar al robot. El tema es que detecta el obstáculo, retrocede y gira, pero al avanzar…lo hace !en diagonal!.. y ni parando cada acción es capaz de hacerlo correctamente. Adjunto captura. Gracias de antemano