{
  "nbformat": 4,
  "nbformat_minor": 0,
  "metadata": {
    "colab": {
      "provenance": [],
      "collapsed_sections": []
    },
    "kernelspec": {
      "name": "python3",
      "display_name": "Python 3"
    },
    "language_info": {
      "name": "python"
    },
    "accelerator": "GPU"
  },
  "cells": [
    {
      "cell_type": "markdown",
      "source": [
        "<h1>Tugas kelompok sisken</h1>\n",
        "<p>\n",
        "Anggota:\n",
        "Nanda Riangga Damanik (19/442385/PA/19134)<br>\n",
        "Gabriel Possenti (19/442374/PA/19123)<br>\n",
        "Aziz<br>\n",
        "Ivander Achmad W. (19/442377/PA/19126)<br>\n",
        "\n",
        "</p>"
      ],
      "metadata": {
        "id": "NVGE7Ov689Yv"
      }
    },
    {
      "cell_type": "code",
      "source": [
        "!pip install slycot\n",
        "!pip install control"
      ],
      "metadata": {
        "id": "rh1dEWqiX2eC",
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "outputId": "84e97c6d-94d0-421e-8095-e729c9a34db8"
      },
      "execution_count": 1,
      "outputs": [
        {
          "output_type": "stream",
          "name": "stdout",
          "text": [
            "Looking in indexes: https://pypi.org/simple, https://us-python.pkg.dev/colab-wheels/public/simple/\n",
            "Collecting slycot\n",
            "  Downloading slycot-0.5.0.0.tar.gz (3.0 MB)\n",
            "\u001b[K     |████████████████████████████████| 3.0 MB 31.2 MB/s \n",
            "\u001b[?25h  Installing build dependencies ... \u001b[?25l\u001b[?25hdone\n",
            "  Getting requirements to build wheel ... \u001b[?25l\u001b[?25hdone\n",
            "    Preparing wheel metadata ... \u001b[?25l\u001b[?25hdone\n",
            "Requirement already satisfied: numpy in /usr/local/lib/python3.7/dist-packages (from slycot) (1.21.6)\n",
            "Building wheels for collected packages: slycot\n",
            "  Building wheel for slycot (PEP 517) ... \u001b[?25l\u001b[?25hdone\n",
            "  Created wheel for slycot: filename=slycot-0.5.0-cp37-cp37m-linux_x86_64.whl size=2021298 sha256=ef6b71965ab3a32f21fe1daea1abbd06cfbfa806c45e9a65c6796ef179774339\n",
            "  Stored in directory: /root/.cache/pip/wheels/7f/2e/54/e2cb7cf358f956ed0efa9923ab4f5aa353a941422c55b25531\n",
            "Successfully built slycot\n",
            "Installing collected packages: slycot\n",
            "Successfully installed slycot-0.5.0\n",
            "Looking in indexes: https://pypi.org/simple, https://us-python.pkg.dev/colab-wheels/public/simple/\n",
            "Collecting control\n",
            "  Downloading control-0.9.2.tar.gz (398 kB)\n",
            "\u001b[K     |████████████████████████████████| 398 kB 31.3 MB/s \n",
            "\u001b[?25hRequirement already satisfied: numpy in /usr/local/lib/python3.7/dist-packages (from control) (1.21.6)\n",
            "Requirement already satisfied: scipy in /usr/local/lib/python3.7/dist-packages (from control) (1.7.3)\n",
            "Requirement already satisfied: matplotlib in /usr/local/lib/python3.7/dist-packages (from control) (3.2.2)\n",
            "Requirement already satisfied: kiwisolver>=1.0.1 in /usr/local/lib/python3.7/dist-packages (from matplotlib->control) (1.4.4)\n",
            "Requirement already satisfied: cycler>=0.10 in /usr/local/lib/python3.7/dist-packages (from matplotlib->control) (0.11.0)\n",
            "Requirement already satisfied: python-dateutil>=2.1 in /usr/local/lib/python3.7/dist-packages (from matplotlib->control) (2.8.2)\n",
            "Requirement already satisfied: pyparsing!=2.0.4,!=2.1.2,!=2.1.6,>=2.0.1 in /usr/local/lib/python3.7/dist-packages (from matplotlib->control) (3.0.9)\n",
            "Requirement already satisfied: typing-extensions in /usr/local/lib/python3.7/dist-packages (from kiwisolver>=1.0.1->matplotlib->control) (4.1.1)\n",
            "Requirement already satisfied: six>=1.5 in /usr/local/lib/python3.7/dist-packages (from python-dateutil>=2.1->matplotlib->control) (1.15.0)\n",
            "Building wheels for collected packages: control\n",
            "  Building wheel for control (setup.py) ... \u001b[?25l\u001b[?25hdone\n",
            "  Created wheel for control: filename=control-0.9.2-py2.py3-none-any.whl size=403205 sha256=b7dbf248d6db2ffa075018978d031e1b619234352ae5cc429fa52fd4ae0e9685\n",
            "  Stored in directory: /root/.cache/pip/wheels/48/ef/c2/929bb5c59a1328df00a0561d0e68cd7c8537f33f5ce0ce741b\n",
            "Successfully built control\n",
            "Installing collected packages: control\n",
            "Successfully installed control-0.9.2\n"
          ]
        }
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "tlHXy4vZR402",
        "outputId": "27da5aad-c71a-4dfb-91c4-04e9c4fd091b"
      },
      "outputs": [
        {
          "output_type": "stream",
          "name": "stdout",
          "text": [
            "2790.4639871211475\n",
            "<class 'numpy.ndarray'>\n"
          ]
        }
      ],
      "source": [
        "%matplotlib inline\n",
        "\n",
        "import numpy as np\n",
        "import seaborn as sns\n",
        "import matplotlib.pyplot as plt\n",
        "import control.matlab as control\n",
        "import time\n",
        "\n",
        "def step_info(t,yout):\n",
        "    print(\"% overshoot : \",(yout.max()/yout[-1]-1)*100,'%')\n",
        "    print(\"Rise time : \",t[next(i for i in range(0,len(yout)-1) if yout[i]>yout[-1]*.90)]-t[0])\n",
        "    print(\"%Settling time : \",t[next(len(yout)-i for i in range(2,len(yout)-1) if abs(yout[-i]/yout[-1])>1.02)]-t[0])\n",
        "\n",
        "s = control.tf('s')\n",
        "J = 0.01\n",
        "b = 0.1\n",
        "K = 0.01\n",
        "R = 1\n",
        "L = 0.5\n",
        "\n",
        "num_motor = [K]\n",
        "den_motor = [J*L, J*R+b*L, R*b+K*K]\n",
        "\n",
        "motor = control.tf(num_motor, den_motor)\n",
        "\n",
        "# Constant\n",
        "c1=2\n",
        "c2=2 \n",
        "w_max = 1 \n",
        "w_min = 0.1 \n",
        "particles=50 \n",
        "iterations=100\n",
        "var=3 \n",
        "e_max = 1 \n",
        "e_min=0.1\n",
        "\n",
        "# Declaration\n",
        "v = np.zeros([particles, var])\n",
        "x = np.zeros([particles, var])\n",
        "xp = np.zeros([particles, var])\n",
        "xg = np.zeros([var])\n",
        "ITAEp = np.zeros([particles])\n",
        "ITAE = np.zeros([particles])\n",
        "best_value = np.zeros([999999])\n",
        "\n",
        "\n",
        "# Search limit\n",
        "lim_min = 0\n",
        "lim_max = 2500\n",
        "\n",
        "# limization steps\n",
        "steps = 0\n",
        "\n",
        "# Initialization\n",
        "for m in range(particles):\n",
        "    for n in range(var):\n",
        "        v[m,n]=0\n",
        "        x[m,n]=lim_min+np.random.rand()*(lim_max-lim_min)\n",
        "        xp[m,n]=x[m,n]\n",
        "\n",
        "    # Model Parameters\n",
        "    Kp = x[m,0]\n",
        "    Ki = x[m,1]\n",
        "    Kd = x[m,2]\n",
        "    # Simulation Model\n",
        "    pid = control.tf([Kd,Kp,Ki],[0,1,0])\n",
        "    motor_cl = control.feedback(motor * pid, 1)\n",
        "    [y,t] = control.step(motor_cl)\n",
        "    # TIAE (Objective Function)\n",
        "    total = 0.0\n",
        "    T = len(y)\n",
        "    for t in range(T):\n",
        "        total = total + t*abs(y[t]-1)\n",
        "    ITAE[m] = total\n",
        "\n",
        "print(ITAE[0])\n",
        "print(type(ITAE))"
      ]
    },
    {
      "cell_type": "code",
      "source": [
        "prev_best = min(ITAE)\n",
        "loc = np.searchsorted(ITAE, min(ITAE))\n",
        "xg[0] = x[loc,0]\n",
        "xg[1] = x[loc,1]\n",
        "xg[2] = x[loc,2]"
      ],
      "metadata": {
        "id": "xY1bhLrhz5tu"
      },
      "execution_count": 3,
      "outputs": []
    },
    {
      "cell_type": "code",
      "source": [
        "start = time.time()\n",
        "\n",
        "for i in range(iterations):\n",
        "  e = e_max - ((e_max - e_min)*i)/iterations\n",
        "  w = w_min + ((iterations - i)*(w_max - w_min))/iterations\n",
        "  for m in range(particles):\n",
        "    for n in range(var):\n",
        "      v[m,n] = w*v[m,n] + c1*np.random.rand()*(xp[m,n]-x[m,n]) + c2*np.random.rand()*(xg[n]-x[m,n])\n",
        "      x[m,n] = x[m,n] + e*v[m,n];\n",
        "      # Constrain\n",
        "      if x[m,n] < lim_min:\n",
        "        x[m,n] = lim_min\n",
        "      if x[m,n] > lim_max:\n",
        "        x[m,n] = lim_max\n",
        "    \n",
        "    Kp = x[m,0]\n",
        "    Kp = x[m,1]\n",
        "    Kp = x[m,2]\n",
        "    pid = control.tf([Kd, Kp, Ki],[0,1,0])\n",
        "    motor_cl = control.feedback(motor * pid, 1)\n",
        "    [y,t] = control.step(motor_cl)\n",
        "\n",
        "    total = 0\n",
        "    T = len(y)\n",
        "    for t in range(T):\n",
        "      total = total + (t*abs(y[t]-1))\n",
        "    \n",
        "    ITAEp[m] = total\n",
        "\n",
        "    if ITAEp[m] < ITAE[m]:\n",
        "      ITAE[m] = ITAEp[m]\n",
        "      xp[m,0] = x[m,0]\n",
        "      xp[m,1] = x[m,1]\n",
        "      xp[m,2] = x[m,2]\n",
        "    \n",
        "\n",
        "    now_best = min(ITAE)\n",
        "    loc = np.searchsorted(ITAE, min(ITAE))\n",
        "\n",
        "    if now_best < prev_best:\n",
        "      prev_best = now_best\n",
        "      xg[0] = x[loc,0]\n",
        "      xg[1] = x[loc,1]\n",
        "      xg[2] = x[loc,2]\n",
        "    \n",
        "    steps = steps + 1\n",
        "    best_value[steps] = prev_best\n",
        "\n",
        "end = time.time()\n",
        "\n",
        "print(\"Time elapsed: \", end - start, \"s\")"
      ],
      "metadata": {
        "id": "0fC4tNGBxRx9",
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "outputId": "772570c9-dd1f-4b1d-c299-6bf5ec7b3265"
      },
      "execution_count": 4,
      "outputs": [
        {
          "output_type": "stream",
          "name": "stdout",
          "text": [
            "Time elapsed:  228.38785576820374 s\n"
          ]
        }
      ]
    },
    {
      "cell_type": "code",
      "source": [
        "# Final testing\n",
        "ITAE_min = prev_best\n",
        "Kp = xg[0]\n",
        "Ki = xg[1]\n",
        "Kd = xg[2]\n",
        "\n",
        "pid = control.tf([Kd, Kp, Ki],[0,1,0])\n"
      ],
      "metadata": {
        "id": "4EysKchgx--6"
      },
      "execution_count": 5,
      "outputs": []
    },
    {
      "cell_type": "code",
      "source": [
        "motor_cl = control.feedback(motor * pid, 1)\n",
        "motor_l = control.feedback(motor, 1)\n",
        "\n"
      ],
      "metadata": {
        "id": "WZ1DM3Y7i2m0"
      },
      "execution_count": 6,
      "outputs": []
    },
    {
      "cell_type": "code",
      "source": [
        "(t,y) = control.step(motor_l)\n",
        "\n",
        "plt.plot(t,y)\n",
        "plt.xlabel('motor_l')\n",
        "plt.grid()"
      ],
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/",
          "height": 280
        },
        "id": "DTiYuZi00l7C",
        "outputId": "bbb8b18a-65b3-4cfe-a354-75556ee0a332"
      },
      "execution_count": 7,
      "outputs": [
        {
          "output_type": "display_data",
          "data": {
            "text/plain": [
              "<Figure size 432x288 with 1 Axes>"
            ],
            "image/png": "[encoded data, 11780 characters]\n"
          },
          "metadata": {
            "needs_background": "light"
          }
        }
      ]
    },
    {
      "cell_type": "code",
      "source": [
        "(t,y) = control.step(motor_cl)\n",
        "\n",
        "plt.plot(t,y)\n",
        "plt.xlabel('motor_cl')\n",
        "plt.grid()"
      ],
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/",
          "height": 280
        },
        "id": "dxMJTkSX2DsB",
        "outputId": "c8a7eee2-b2ba-43a2-b64e-a00f0312e964"
      },
      "execution_count": 8,
      "outputs": [
        {
          "output_type": "display_data",
          "data": {
            "text/plain": [
              "<Figure size 432x288 with 1 Axes>"
            ],
            "image/png": "[encoded data, 6736 characters]\n"
          },
          "metadata": {
            "needs_background": "light"
          }
        }
      ]
    },
    {
      "cell_type": "code",
      "source": [
        "(t,y) = control.step(motor_l/(s**2))\n",
        "\n",
        "plt.plot(t,y)\n",
        "plt.xlabel('motor_l')\n",
        "plt.grid()"
      ],
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/",
          "height": 280
        },
        "id": "nEa8Y4Xy2PZu",
        "outputId": "2dbc182a-8300-4285-9b33-fcdc2e228b6a"
      },
      "execution_count": 9,
      "outputs": [
        {
          "output_type": "display_data",
          "data": {
            "text/plain": [
              "<Figure size 432x288 with 1 Axes>"
            ],
            "image/png": "[encoded data, 11624 characters]\n"
          },
          "metadata": {
            "needs_background": "light"
          }
        }
      ]
    },
    {
      "cell_type": "code",
      "source": [
        "(t,y) = control.step(motor_cl/(s**2))\n",
        "\n",
        "plt.plot(t,y)\n",
        "plt.xlabel('motor_cl')\n",
        "plt.grid()"
      ],
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/",
          "height": 280
        },
        "id": "qMQ5T9Vo2V4O",
        "outputId": "03dc4e63-5a72-4f6e-c4b3-d84884101188"
      },
      "execution_count": 10,
      "outputs": [
        {
          "output_type": "display_data",
          "data": {
            "text/plain": [
              "<Figure size 432x288 with 1 Axes>"
            ],
            "image/png": "[encoded data, 13652 characters]\n"
          },
          "metadata": {
            "needs_background": "light"
          }
        }
      ]
    },
    {
      "cell_type": "code",
      "source": [
        "def step_info(t,yout):\n",
        "    try:\n",
        "      print(\"% overshoot : \",(yout.max()/yout[-1]-1)*100,'%')\n",
        "    except:\n",
        "      print(\"% overshoot : -\")\n",
        "    try:\n",
        "      print(\"Rise time : \",t[next(i for i in range(0,len(yout)-1) if yout[i]>yout[-1]*.90)]-t[0])\n",
        "    except:\n",
        "      print(\"Rise time : -\")\n",
        "    try:\n",
        "      print(\"%Settling time : \",t[next(len(yout)-i for i in range(2,len(yout)-1) if abs(yout[-i]/yout[-1])>1.02)]-t[0])\n",
        "    except:\n",
        "      print(\"%Settling time : -\")\n",
        "\n",
        "y,t = control.step(motor_l)\n",
        "step_info(t,y)\n",
        "\n",
        "print(\"\")\n",
        "\n",
        "y,t = control.step(motor_cl)\n",
        "step_info(t,y)"
      ],
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "Uv3JS6zCRaf6",
        "outputId": "4c56bc20-f94e-43ce-9f1f-697da03d2231"
      },
      "execution_count": 11,
      "outputs": [
        {
          "output_type": "stream",
          "name": "stdout",
          "text": [
            "% overshoot :  0.0 %\n",
            "Rise time :  1.1315389526151183\n",
            "%Settling time : -\n",
            "\n",
            "% overshoot :  0.15210430606766856 %\n",
            "Rise time :  0.005695236394454569\n",
            "%Settling time : -\n"
          ]
        }
      ]
    }
  ]
}