FollowLine: controlador con dos PID

Con el fin de mejorar la entrada y salida de curva, los principales problemas que tení­a hasta el momento, he decidido incorporar un segundo PID. De este modo, tendrá un controlador para el estado de curva y otro para el estado de recta, con sus correspondientes constantes ajustadas. El gran desafío es como definir correctamente que es curva y que recta. Bien, tras darle algunas vueltas, creo que la opción más sencilla es comprobar si tres puntos tomados a distintas alturas de la lí­nea están alineados o no. En caso de que no lo estén, asumiré que el robot se encuentra en una curva o cualquier otro estado que no sea un recta. La forma de calcular si los puntos están alineados es sencilla. Teniendo tres puntos A, B y C estos pertenecen a la misma línea si: distancia(A,B) + distancia(B,C) = distancia(A,C). Además, hay que añadir una pequeña tolerancia para que funcione correctamente.

Tras ajustar las constantes de cada controlador manualmente, mediante prueba y error, el robot se comporta muy bien en las dos situaciones, permitiendome subir la velocidad. Sin embargo las entradas y salidas de curva siguen siendo deficientes. Revisando el código, me he dado cuenta de que dentro de cada controlador tenía dos variables que conservaban su valor entre frames. Esto es correcto, el problema es que cuando el robot encuentra una nueva curva, por ejemplo, el valor de dichas variables no es el de por defecto (0), si no que tienen un valor que proviene de la curva anterior. Por suerte tiene fácil solución, con resetear las variables del controlador que no está en uso en cada iteración es suficiente.

Una vez arreglado ese detalle, el comportamiento mejora, pero todaví­a es muy brusco en algunas partes. Para suavizarlo he decidido incorporar un buffer de diez unidades donde almaceno el estado del robot (curva o recta) en cada iteración. De este modo puedo establecer la regla de que pase a usarse el controlador de recta sólo si anteriormente se han detectado al menos diez rectas. En cuanto en el buffer entre una curva el controlador seleccionado sería el de curva. 

Aunque parece algo sencillo, he notado una gran mejoría en el comportamiento al incorporarlo. Ahora mismo las entradas y salidas de curva son mucho más suaves. Por otra parte, al incluir dos controladores he suprimido las reglas de control que había introducido antes, ya que realmente no estaban aportando mucho. Tras las mejoras el código es mucho más limpio y claro, a la vez que el rendimiento del robot ha mejorado. He podido completar una vuelta en 1min40s aunque he rebajado un poco la velocidad para ganar más seguridad. Con la estrategia que he venido siguiendo creo que estoy cerca del límite y no creo que pueda conseguir mejorar mucho más mi tiempo. Posiblemente mediante otro camino se pueda, pero no se me ocurre como.