Práctica 1 (parte 3)

Después de implementar el control con derivada para que el robot zigzaguee menos y de subir un poco las líneas del error para el giro, he implementado otras dos líneas por encima de las que ya tenía, de forma que puedo medir el error de lo que vendrá y anticiparse un poco más.
Con el error de estas líneas consigo saber hacia dónde irá la próxima curva y lo cerrada que es. De esta forma puedo adaptar la velocidad y el giro del robot.

Ahora la función que dice cuánto debe girar el robot, depende del error de giro de las dos líneas del principio, de la derivada de este error y del error de las nuevas líneas.
La velocidad del robot depende de la línea vertical que tenía desde un principio y del error de las nuevas líneas. De esta forma si lo que se aproxima es una curva muy cerrada, (hay mucho error en la anticipación) el robot empezará a ir más lento o a frenar.
Además he definido una serie de circunstancias para definir cuánto debe contribuir cada uno de los errores para el giro. Si las dos líneas superiores de error (las nuevas que tienen que dar la anticipación) no consiguen ver el camino, es porque el robot probablemente está en una curva muy cerrada. Lo que interesa es que el robot gire muy deprisa para perder el menor tiempo posible, por lo que la contribución al giro del error de las dos líneas inferiores (las del principio) es muy grande.
Cuando el robot va en línea recta, la contribución de este error es menor pero se tienen también en cuenta el error de las líneas superiores, para que el robot empiece a girar un poco antes de estar en la curva, y la derivada, para que no zigzaguee demasiado.
Cuando el robot va lento suele ser porque está en una curva no muy cerrada. Como me interesa que resuelva estas de forma rápida, cuando el robot va lento, la contribución de las líneas superiores al giro es mayor que cuando va deprisa (en línea recta).