{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "1ebf527e",
   "metadata": {},
   "source": [
    "# AeroController\n",
    "\n",
    "The `AeroController` 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 `AeroController`\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 will run an experiment on the AeroShield with your controller. We will discuss each step in more detail below. 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 `AeroController` classes, as well as the `numpy` package."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "bb44055c-a3ec-4f7d-8e4e-79ddd515c10f",
   "metadata": {
    "tags": []
   },
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "\n",
    "from aeroshield import AeroShield, AeroController"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ad2529e0",
   "metadata": {},
   "source": [
    "## Simple Controller Example: Using the `controller` Method.\n",
    "\n",
    "Below 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, 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, 1).\n",
    "* `angle` [°]: 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. In the example below, 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 as the values passed to `controller`, except for the motor value, which is saturated (limited between [0, 256) ). You'll also notice the cycles being counted in the terminal."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "81826059",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "999\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "(1000, 5)"
      ]
     },
     "execution_count": 2,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "class PotController(AeroController):\n",
    "    def controller(self, t: float, dt: float, ref: float, pot: float, angle: float) -> float:\n",
    "        return pot\n",
    "\n",
    "\n",
    "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 `AeroController` class might become more clear with a slightly more complex example."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "cd207179",
   "metadata": {},
   "source": [
    "## Complex Controller Example: Adding 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 defined 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": 3,
   "id": "0253d73a-9c09-4c36-9f11-8bed01f06d1a",
   "metadata": {
    "tags": []
   },
   "outputs": [],
   "source": [
    "class PIDController(AeroController):\n",
    "    def variables(self) -> None:\n",
    "        # preface every variable defined in subclass with var_ to avoid overwrites.\n",
    "        self.var_kp = 2.5\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, angle: float) -> float:\n",
    "        error = ref - angle\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",
    "        try:\n",
    "            differential = self.var_kp*self.var_kd*(error - self.var_prev_error)/dt\n",
    "        except ZeroDivisionError:\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": 4,
   "id": "62c0d949-c36c-406b-85c4-41635718c432",
   "metadata": {
    "tags": []
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "999\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "(1000, 6)"
      ]
     },
     "execution_count": 4,
     "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"
   ]
  }
 ],
 "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.13"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}