Práctica P3: Programación de un coche autónomo - ori1710/Robotica_servicio GitHub Wiki
Objetivo
El objetivo de esta práctica es implementar la lógica de un algoritmo de navegación para un vehículo automatizado. El vehículo debe encontrar una plaza de aparcamiento y aparcar correctamente. Para realizar la práctica, he hecho una máquina de estados que se distribuye de la siguiente forma:
- Encontrar un hueco libre
- Acercarse al hueco libre
- Empezar maniobras de aparcamiento
- Posicionarse bien y aparcar
Encontrar hueco libre
Para esta parte solo tenemos que ir avanzando hasta encontrar un hueco libre, en nuestro caso a la derecha, que sea lo suficientemente grande para que entre el coche y haya un espacio suficiente entre los coches que tengamos delante y atrás.
Para saber que estamos ante un hueco libre, tenemos que ver que nos dice el laser. Uno de los tres laser que tenemos es el RightLaser que nos permite ver todo a nuestro lado derecho, así que si vemos entre 60 y 120 grados podemos ver si hay espacio suficiente. Si dentro de ese rango el valor mínimo es mayor a 5 metros diremos que esta libre el hueco (comprobando también que el valor esta dentro del rango valido de datos).
Una vez encontrado hueco pasamos al siguiente estado que es el de acercarnos al hueco disponible.
Acercarse al hueco libre
En este paso, sin importar si hay coches o no, lo que haremos sera girar a la derecha y acercarnos lo mayor posible a la derecha, si hay un coche delante del hueco lo que hará sera, una vez este a 1,5 metros del coche girara a la izquierda, pero si no hay coche delante del hueco,cuando este a 7,5 metros del coche que está detrás del hueco libre girara a la izquierda, aunque si no hay coche detrás, la condicion cambai a lo siguiente al final:
if min(rightLaser) < 1.5 or (min(backLaser) > 7.5 and math.isfinite(min(backLaser)))
Para entrar al siguiente estado, primero tuve que comprobar si hay un coche delante y el laser trasero no devuelve infinito para que asi se quedara en la misma etapa y lograra acercarse al coche delantero y una vez estuviera a 1.2 metros, pasa a la siguiente etapa, si no cumple este primer requisito, ya compruebo si el coche trasero está a 9.4 metros, si esta el coche medio recto y que el coche delantero este a 1.2 metros de separación o que el coche trasero se encuentre a 10 metros para poder pasar al siguiente estado.
if min(frontLaser) < 6.0 and math.isinf(min(backLaser)):
if min(rightLaser) < 1.2:
HAL.setV(0.0)
HAL.setW(0.0)
state = 2
elif min(backLaser) > 9.4 and (abs(dif) < 7 or abs(dif) > 353) and (min(rightLaser) < 1.2 or min(backLaser) > 10.0):
HAL.setV(0.0)
HAL.setW(0.0)
state = 2
Empezar maniobras de aparcamiento
Para entrar en el espacio libre lo dividimos en dos partes: giro a 135 grados a la derecha yendo hacia atrás hasta que el coche no este a 135 grados más o menos no pasa al siguiente estado, y luego giro a la izquierda al máximo hasta que este recto o este a 0,8 metros de un coche.
Posicionarse bien y aparcar
En la última etapa solo se enfoca en quedar centrado en el caso que haya dos carros y que si sólo hay un coche, ya sea delante o detrás, que quede a 2.5 metros de este, dejando espacio suficiente para que todos los coches puedan maniobrar con facilidad.
A continuación dejo tres videos, uno para cada caso, todos quedan bien aparcados pero uno queda más cerca de la acera que los otros pero sigue aparcando dentro de lo que se considera la calle, además en todos los escenarios logra aparacar en un tiempo de 1:30.