Considera un circuito de Arduino al que se le han conectado 2 motores DC a través deun módulo L298N o un módulo L293D, un LED rgb y un sensor ultrasónico HC - SR04montado sobre un servomotor SG09?

Considera un circuito de Arduino al que se le han conectado 2 motores DC a través de un módulo L298N o un módulo L293D, un LED rgb y un sensor ultrasónico HC - SR04 montado sobre un servomotor SG09. Considerando la configuración anterior, escribe el programa de Arduino que permita avanzar al vehículo en tanto frente a él no exista un obstáculo, en caso de haber un obstáculo deberá detenerse y decidir si debe girar a la derecha o a la izquierda, considerando cuál de esos lados se encuentra libre para avanzar. Mientras el vehículo está avanzando hacia adelante el LED se enciende en verde, si se detiene se enciende en azul y si avanza en reversa (cuando hay obstáculos al frente, izquierda y derecha) se enciende en rojo. Alguien ayúdeme a hacer este programa : c, tenía que hacer 5 de tarea y este y otro que ahorita voy a publicar, son los únicos que me salen mal : c.