LineFollower.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "77acbdf3-fc9f-4808-ab8b-c48d0711b238",
   "metadata": {},
   "source": [
    "# Siguelíneas\n",
    "\n",
    "Se trata de hacer que el robot siga una línea trazada en el suelo. Para detectar la línea, se usará la [matriz de sensores](https://www.pololu.com/product/3577) que proporciona el fabricante para este modelo de robot:\n",
    "\n",
    "<img src=\"https://a.pololu-files.com/picture/0J9642.1200.jpg\" alt=\"Sensores\" style=\"width: 450px;\"/>\n",
    "\n",
    "Se trata de una placa con cinco sensores de reflectividad, que devuelven un valor numérico relacionado con la luz reflejada por el material que tienen debajo.\n",
    "\n",
    "Para que el seguimiento de la línea sea robusto, es importante que esta tenga bastante contraste con el color del suelo, pudiendo ser tanto una línea oscura sobre un suelo claro, como una línea clara sobre un suelo oscuro."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "de4481fa-cf23-4654-a3b3-a4e58b952bad",
   "metadata": {},
   "source": [
    "## Calibración de los sensores\n",
    "\n",
    "Para que el sensor detecte la línea lo mejor posible, es necesario calibrarlo previamente. La calibración consiste en registrar los valores mínimo y máximo de reflectividad esperados durante el funcionamiento. Para ello, se desplaza el robot lateralmente sobre la línea, haciendo que todos los sensores pasen sobre la misma, mientras los cinco sensores van midiendo la reflectividad y registrando los valores mínimo y máximo. Una vez determinados estos valores, se escalan para que la máxima reflectividad de cada sensor corresponda a un valor de cero, y la mínima a un valor de 1000.\n",
    "\n",
    "La biblioteca [Balboa32U4](https://pololu.github.io/balboa-32u4-arduino-library/class_balboa32_u4_line_sensors.html) contiene las funciones necesarias para esto. [Aquí](./Files/QTRCalibrate.ino) se puede descargar un ejemplo de código para la calibración. Este programa realiza la calibración y almacena los resultados en la [EEPROM](https://docs.arduino.cc/learn/built-in-libraries/eeprom) del robot, que es una memoria no volátil, para poder cargarlos posteriormente desde el programa de control."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3cd314e9-b859-43f2-8ae7-434be5b5789e",
   "metadata": {},
   "source": [
    "## Estimación de la posición de la línea\n",
    "\n",
    "Una vez calibrados los sensores, la posición de la línea $y_l$ se obtiene como un valor combinado de las lecturas $r_i$ de los sensores:\n",
    "\n",
    "$$\n",
    "    y_l = 1000\\frac{r_2 + 2r_3 + 3r_4 + 4r_5}{r_1 + r_2 + r_3 + r_4 + r_5}\n",
    "$$\n",
    "\n",
    "La variable $y_l$ tomará un valor entre 0 y 4000, dependiendo de la posición de la línea respecto a la matriz de sensores, siendo 2000 el valor cuando el robot está centrado sobre la línea. Por lo tanto, este será el valor deseado de la lectura. [Este programa](./Files/QTRRead.ino) muestra cómo cargar la calibración almacenada previamente en la EEPROM, y cómo obtener el error de seguimiento a partir de la lectura de los sensores. Si están bien calibrados, el error debería ser cero cuando el robot está centrado."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "95973f8e-a38f-45ee-94c8-2b84de0b0a3a",
   "metadata": {},
   "source": [
    "## Actuación sobre la dirección\n",
    "\n",
    "A diferencia del control de dirección mostrado [aquí](./Steering.ipynb), en este caso nuestro objetivo no es conseguir una velocidad angular sobre el eje vertical, sino mantener el error de seguimiento cerca de cero. Pero el principio utilizado para hacer girar al robot sigue siendo el mismo: aplicar un $\\Delta V$ a los motores, de forma que se mantiene aproximadamente intacto el momento neto sobre el chasis:\n",
    "\n",
    "$$\n",
    "    V_r = V + \\Delta V \\qquad V_l = V - \\Delta V\n",
    "$$\n",
    "\n",
    "donde $V$ es, de nuevo, el voltaje común obtenido en el LQR, y $\\Delta V$ es un ajuste de voltaje que vendrá determinado por el controlador de seguimiento de trayectoria."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "6add4f8b-ed4b-4e1e-8896-0c1cdae5920c",
   "metadata": {},
   "source": [
    "## Diseño del controlador\n",
    "\n",
    "En este caso se utilizará un sencillo controlador PD. Su entrada será el error en $y_l$, y devolverá como salida una corrección de voltaje $\\Delta V$, resultado de aplicar las ganancias correspondientes $K_p$ y $K_d$ al error y su derivada temporal. El error en la posición lateral se define como la diferencia entre la posición deseada, que en nuestro caso es igual a 2000, y la que tenemos realmente, $y_l$, así que el controlador se puede resumir de la siguiente manera:\n",
    "\n",
    "$$\n",
    "\\begin{align}\n",
    "    \\varepsilon &= 2000 - y_l \\\\\n",
    "    \\Delta V &= K_p \\varepsilon + K_d \\dot\\varepsilon\n",
    "\\end{align}\n",
    "$$\n",
    "\n",
    "La aplicación práctica es igual que en el caso del control de dirección: se mide la posición sobre la línea, se calcula el error respecto a la posición deseada, y se deriva en el tiempo. Luego, se calcula el $\\Delta V$ usando las ganancias del PD y se aplica al voltaje medio obtenido por el LQR, sumándolo al motor derecho y restándolo al izquierdo."
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.11.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}