A cada iteración del bucle de control se debe calcular primero las velocidades angulares de las ruedas derecha (conectada en el puerto A) e izquie...

A cada iteración del bucle de control se debe calcular primero las velocidades angulares de las ruedas derecha (conectada en el puerto A) e izquierda (conectada en el puerto B). Esto se hace a partir de la estimación con los valores de las posiciones obtenidos a partir de los encoders. Considerando el modelo cinemático diferencial del robot, a partir de las velocidades angulares se calcula la velocidad angular y lineal del robot, obteniendo posteriormente una estimación de la posición X−Y y de la orientación theta del robot, de forma que se puede calcular a continuación el control cinemático del robot. A continuación, considerando el modelo cinemático inverso del robot, se puede calcular las velocidades lineales de referencia para las ruedas, y a partir de éstas, las velocidades angulares de referencia. Una vez se tienen las velocidades angulares de referencia se puede establecer el control abordado en el apartado 5.2. Aunque se podría plantear un control más complejo, en este algoritmo se ha implementado un control proporcional. Lo único que falta es proporcionar las acciones de control a los motores, escribir los valores en un fichero de texto y verificar el periodo de muestreo.