{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "1ebf527e",
   "metadata": {},
   "source": [
    "# ShieldController\n",
    "\n",
    "The `ShieldController` class is most suitable for the majority of users and use cases. Implementing a controller is done following a few steps.\n",
    "\n",
    "1. Create a class that inherits from `ShieldController`\n",
    "1. Implement the `controller` method and, optionally, the `variables` method.\n",
    "1. Execute your controller class' `run` method, specifying the frequency and runtime.\n",
    "\n",
    "This tutorial will run an experiment on the `AeroShield` with your controller. We will discuss each step in more detail below. The steps in this tutorial apply to all shield classes, however, the controller will need to be tuned differently to work.\n",
    "\n",
    "Optionally, you can plot the results afterwards or add a `LivePlotter` instance to the run to see the data live. See the plotting examples for more information on plotters.\n",
    "\n",
    "First, we import the `AeroShield` and `ShieldController` classes, as well as the `numpy` package."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "bb44055c-a3ec-4f7d-8e4e-79ddd515c10f",
   "metadata": {
    "tags": []
   },
   "outputs": [],
   "source": [
    "%%capture\n",
    "import numpy as np\n",
    "\n",
    "from automationshield import AeroShield, ShieldController"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "15bb25b5",
   "metadata": {},
   "source": [
    "## Simple Controller Example: Using the `controller` Method."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "e7c6dcb9",
   "metadata": {},
   "outputs": [],
   "source": [
    "class PotController(ShieldController):\n",
    "    def controller(self, t: float, dt: float, ref: float, pot: float, sensor: float) -> float:\n",
    "        return pot"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ad2529e0",
   "metadata": {},
   "source": [
    "Above is a very simple example to illustrate the working of the `controller` method. Beside the `self` parameter, pointing to the class instance, there are several inputs to the method:\n",
    "\n",
    "* `t` (s): The time since the start of the experiment run.\n",
    "* `dt` (s): The time step size of the current iteration. This should match the frequency set when running most of the time, but this is not guaranteed. There are usually a few instances during a run where `dt` is larger.\n",
    "* `ref` (-): In most scenarios for the `AeroShield`, this will be the reference angle, though you're free to do with it what you want in your controller.\n",
    "* `pot` (-): The potentiometer value scaled on the interval [0, 100].\n",
    "* `sensor` (°): For the `AeroShield`, this is the current angle of the pendulum.\n",
    "\n",
    "**Whichever controller you build, the signature of the method must always contain these variables in this order.** The return value of the controller method is sent to the Arduino as the motor value. The motor value is assumed to be in percent, and as a result, will be saturated between 0 and 100 before being converted to a bit value and sent to the Arduino. In the example above, you can see that that is the potentiometer value. Also note how all other variables go unused.\n",
    "\n",
    "To run this controller on the Aeroshield, we create an instance of our `PotController` class and call it `pot_controller`. We create an instance of `AeroShield` called `aero_shield` and give it to the controller upon creation. By separately creating the `AeroShield` instance, we can more easily modify it (e.g. manually setting the port) before giving it to the controller.\n",
    "\n",
    "Then, we call the `run` method on the controller. You must specify the frequency (`freq`) (Hz), and runtime (`cycles`) expressed as a number of cycles the loop should be run. The runtime is seconds is then roughly equal to `cycles/freq`.\n",
    "\n",
    "When complete, `run` returns the results of the experiment in a `numpy` array. By default, the array contains 5 columns: `[t, ref, pot, angle, motor]`. They are identical to the values passed to `controller`, except for the motor value, which is saturated (limited between [0, 100]) in case the controller outputs a motor value that is too high. You'll also notice the cycles being counted in the terminal."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "24172863",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(1000, 5)"
      ]
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "aero_shield = AeroShield()\n",
    "pot_controller = PotController(aero_shield)\n",
    "results = pot_controller.run(freq=200, cycles=1000)\n",
    "\n",
    "results.shape"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9ed34752",
   "metadata": {},
   "source": [
    "The above controller is very similar to the controller we implemented in the `AeroShield` example. It also doesn't look all that much shorter or simpler. However, a lot more is happening in the background here:\n",
    "\n",
    "* The frequency of the loop is controlled;\n",
    "* The results are automatically saved and returned in a convenient format.\n",
    "\n",
    "And we haven't talked about the option to plot results in real time (see the `LivePlotter` example for that). The convenience of using the `ShieldController` class might become more clear with a slightly more complex example."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "cd207179",
   "metadata": {},
   "source": [
    "## Adding Persistent Variables\n",
    "\n",
    "The code block below implements a PID controller. The `controller` method calculates the proportional, integral and differential componenents of the controller, and sums them to get a motor value.\n",
    "\n",
    "This controller also implements a `variables` method. This method does nothing more than declare a few variables in the class instance which can be used by the controller. In this example, we define the proportional, integral and differential gains as well as the total error and previous error. It's not necessary to define the gains in the `variables` method, but it helps clean up the controller implementation and prevents the variables to be assigned again every time the `controller` method is called. The total and previous errors are different. Their values must be initialised outside of the controller (so they can be set to 0) and persist beyond the scope of the `controller` method (so we can remember the value from the previous time we ran the controller). That is why we define them as instance variables to the class in the `variables` method. We do this by giving them the `self.` prefix. This method is run upon creation of the instance, making sure the variables exist in the scope of the class instance. It is advised to prefix all user-defined variables with `var_`, to avoid overriding variables from the base class.\n",
    "\n",
    "A second thing is happening in the `variables` method. We are adding `var_total_error` as a tracked variable by calling `add_tracked_variable` with the variable name (as a string) as argument. This will add a column to the results we discussed earlier for `var_total_error` and return it at the end of the experiment. By default, one column is added per variable. If more columns are needed, you can specify that through the `size` keyword argument."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "0253d73a-9c09-4c36-9f11-8bed01f06d1a",
   "metadata": {
    "tags": []
   },
   "outputs": [],
   "source": [
    "class PIDController(ShieldController):\n",
    "    def variables(self) -> None:\n",
    "        # preface every variable defined in subclass with var_ to avoid overrides.\n",
    "        self.var_kp = 1\n",
    "        self.var_ki = .25\n",
    "        self.var_kd = .25\n",
    "\n",
    "        self.var_total_error = 0\n",
    "        self.var_prev_error = 0\n",
    "\n",
    "        self.add_tracked_variable(\"var_total_error\")\n",
    "\n",
    "    def controller(self, t: float, dt: float, ref: float, pot: float, sensor: float) -> float:\n",
    "        error = ref - sensor\n",
    "        self.var_total_error += error * dt\n",
    "\n",
    "        proportional = self.var_kp * error\n",
    "        integral = self.var_kp/self.var_ki * self.var_total_error\n",
    "\n",
    "        if dt > 0:\n",
    "            differential = self.var_kp*self.var_kd*(error - self.var_prev_error)/dt\n",
    "        else:\n",
    "            differential = 0\n",
    "\n",
    "        motor = proportional + integral + differential\n",
    "\n",
    "        self.var_prev_error = error\n",
    "\n",
    "        return motor\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "02e19cd2",
   "metadata": {},
   "source": [
    "We run this controller the same way we did before, except we also provide a reference this time. we set the reference at a constant 45° throughout the experiment (in this case, instead of creating an array, we could also simply provide a single number). Because we added `var_total_error` as a tracked variable, the `results` array now features an extra column, totaling 6."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "62c0d949-c36c-406b-85c4-41635718c432",
   "metadata": {
    "tags": []
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(1000, 6)"
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "pid_controller = PIDController(AeroShield())\n",
    "results = pid_controller.run(freq=200, cycles=1000, ref=np.ones(1000)*45)\n",
    "\n",
    "results.shape"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e961bb12-bece-4f5b-b3ae-691b08cb7f27",
   "metadata": {},
   "source": [
    "## Calculating the Reference using a Callback\n",
    "\n",
    "Sometimes, you don't just want a present reference. A common example is setting the reference as a function of the potentiometer value. This could be done in the `controller` method, but then you would have to change the controller each time you want a different reference. It is more convenient to define a function to calculate the reference each cycle instead.\n",
    "\n",
    "In the example below, we define a function `calc_ref_from_pot`, which calculates the reference value as a function of the potentiometer. In this case, it returns an angle between 45° and 135° depending on the potentiometer value. The function must take three inputs: the cycle count, the elapsed time of the experiment and the potentiometer value."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "1ad1a878-3468-441b-8cfa-b830c2493071",
   "metadata": {},
   "outputs": [],
   "source": [
    "def calc_ref_from_pot(cntr: int, t: float, pot: float) -> float:\n",
    "    return 45 + .9 * pot"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7d01c7f8",
   "metadata": {},
   "source": [
    "For even more complex references, for example when you want to switch between different types of references during an experiment, you can define a reference class. The class must expose a `__call__` method with the same signature as the callback function defined above. Here, we defined a reference class that initially sets the reference as a function of the potentiometer and after 500 cycles switches to a preset reference."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "0a0d1b92",
   "metadata": {},
   "outputs": [],
   "source": [
    "class MyReference:\n",
    "    def __init__(self, ref_list: list[float]) -> None:\n",
    "        self.ref = ref_list\n",
    "        self.i = -1\n",
    "\n",
    "    def __call__(self, cntr: int, t: float, pot: float) -> float:\n",
    "        if cntr > 500:\n",
    "            self.i += 1\n",
    "            return self.ref[self.i]\n",
    "\n",
    "        return 45 + .9 * pot"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c3ab783c-acfe-4638-a34f-736a1a4c401b",
   "metadata": {},
   "source": [
    "The callback function or class can be provided to the `run` method like any other reference."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "id": "3e3fcc39-dd14-4df1-94c7-7c5310fbf6bd",
   "metadata": {},
   "outputs": [],
   "source": [
    "pid_controller = PIDController(AeroShield())\n",
    "\n",
    "# callback function\n",
    "results = pid_controller.run(freq=200, cycles=1000, ref=calc_ref_from_pot)\n",
    "\n",
    "# class\n",
    "my_ref = MyReference(np.arange(500)/5)\n",
    "results = pid_controller.run(freq=200, cycles=1000, ref=my_ref)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "55571b94-d706-4eef-a50a-21fc5c295da1",
   "metadata": {},
   "source": [
    "An added benefit of using functions to calculate the ref over doing so in the `controller` method is that now, the calculated reference is included in the `results` array."
   ]
  }
 ],
 "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.10.14"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}