En uno de los grupos de Telegram sobre Arduino en los que participo, un compañero ha solicitado ayuda con un problema de sus estudios de mecatrónica.
Los requisitos vienen a ser los siguientes:
- Hay un robot en una zona negra circular
- El exterior de la zona donde está el robot es blanca
- Dentro de la zona negra hay piezas
- El robot debe empujar esas piezas fuera de la zona negra
- Cuenta con dos sensores:
- De distancia, para detectar objetos
- De color, para saber si está en la zona negra o la zona blanca
Y este es el algoritmo en pseudocódigo que me ha salido en 10 minutos:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 |
ALGORITMO DE RESOLUCION VARIABLE AVANCE MAXIMO = RADIO DEL CIRCULO DE LA ZONA NEGRA VARIABLE REALIZAR CAMBIO DIRECCION = FALSE BUCLE POR SIEMPRE MIENTRAS DENTRO DE LA ZONA NEGRA VARIABLE DE DISTANCIA AVANZADA = 0 MIENTRAS DETECTOR DE OBJETO = FALSE GIRAR 5º DETECTAR OBJETO FIN MIENTRAS AVANZAR REALIZAR CAMBIO DIRECCION = TRUE FIN MIENTRAS SI FUERA DE LA ZONA NEGRA && REALIZAR CAMBIO DIRECCION GIRA A LA DERECHA UN NÚMERO ALEATORIO ENTRE 90 Y 180º REALIZAR CAMBIO DIRECCION = FALSE FIN SI SI VARIABLE DE DISTANCIA AVANZADA < AVANCE MAXIMO AVANZA 1 VARIABLE DE DISTANCIA AVANZADA = VARIABLE DE DISTANCIA AVANZADA + 1 FIN SI SI (VARIABLE DE DISTANCIA AVANZADA >= AVANCE MAXIMO) && FUERA DE LA ZONA NEGRA REALIZAR CAMBIO DIRECCION = TRUE FIN SI FIN BUCLE POR SIEMPRE |
Consta de dos partes principales:
- No salirse de la zona negra y volver en caso de salirse
- Empujar la pieza hasta sacarla de la zona negra
La parte de empujar la pieza fuera de la zona negra es la más sencilla. Hacemos una especie de escaneo girando 5º cada vez y avanzamos hasta que estemos fuera de la zona negra:
|
1 2 3 4 5 6 7 8 9 |
MIENTRAS DETECTOR DE OBJETO = FALSE GIRAR 5º DETECTAR OBJETO FIN MIENTRAS AVANZAR |
Y todo el resto del algoritmo se usa para evitar que el robot salga de la zona negra o que vuelva en caso de que se haya salido. En caso de estar dentro de la zona negra vamos a buscar una pieza y empujarla:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 |
MIENTRAS DENTRO DE LA ZONA NEGRA VARIABLE DE DISTANCIA AVANZADA = 0 MIENTRAS DETECTOR DE OBJETO = FALSE GIRAR 5º DETECTAR OBJETO FIN MIENTRAS AVANZAR REALIZAR CAMBIO DIRECCION = TRUE FIN MIENTRAS |
El resto de condiciones sirven para hacer un giro que nos permita volver a la zona segura, avanzar hasta un máximo que sería el radio del círculo y que en condiciones ideales nos dejaría en el centro, y hacer un nuevo giro en caso de que hayamos avanzado la distancia prevista pero sigamos fuera de la zona negra:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 |
SI FUERA DE LA ZONA NEGRA && REALIZAR CAMBIO DIRECCION GIRA A LA DERECHA UN NÚMERO ALEATORIO ENTRE 90 Y 180º REALIZAR CAMBIO DIRECCION = FALSE FIN SI SI VARIABLE DE DISTANCIA AVANZADA < AVANCE MAXIMO AVANZA 1 VARIABLE DE DISTANCIA AVANZADA = VARIABLE DE DISTANCIA AVANZADA + 1 FIN SI SI (VARIABLE DE DISTANCIA AVANZADA >= AVANCE MAXIMO) && FUERA DE LA ZONA NEGRA REALIZAR CAMBIO DIRECCION = TRUE FIN SI |
Le he pedido que me pase una captura de la implementación que haga.