Gain Schedulling PID Equation 1-v2.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "#Importing Dependencies\n",
    "import numpy as np\n",
    "from scipy.integrate import odeint\n",
    "from matplotlib import pyplot as plt\n",
    "from ipywidgets import interactive\n",
    "\n",
    "\n",
    "%matplotlib inline"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "ierror = 0\n",
    "prev_error = 0\n",
    "#PID Module that takes input: y(Model Output),SP(Set Point), params(Tuning parameters) and delta_t(Time Interval)\n",
    "#and returns: x(Model Input)\n",
    "def PID(v, SP, params, delta_t):\n",
    "    global ierror\n",
    "    global prev_error\n",
    "    error = SP - v\n",
    "    ierror = ierror + error*delta_t\n",
    "    derror = (error - prev_error)/delta_t\n",
    "    \n",
    "    #kp,ki,kd = get_parameters()\n",
    "    kp=params[\"kp\"]\n",
    "    ki=params[\"ki\"]\n",
    "    kd=params[\"kd\"]\n",
    "    \n",
    "    x = kp*error + ki*ierror + kd*derror\n",
    "    prev_error = error\n",
    "    return x, error"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Equation\n",
    "===\n",
    "\n",
    "The system is governed by the following equation\n",
    "\n",
    "$$ \\ddot{y} + 2\\dot{y} + y = (t - 0.5)x $$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "def model(y_cap, t, params):\n",
    "    \"\"\"\n",
    "        Given a vector <y, y1> returns <y1, y2> according to equation\n",
    "        above.\n",
    "    \"\"\"\n",
    "    y, y1 = y_cap # y, dy/dt\n",
    "    x = params\n",
    "\n",
    "    y2 = -2*y1 - y + (t - 0.5)*x\n",
    "    dy_cap = y1, y2\n",
    "\n",
    "    return dy_cap"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "def get_normalized_values():\n",
    "    '''\n",
    "        Returns normalized values of Kp, Kd and alpha according to some Logic, currently it returns constant values.\n",
    "    '''\n",
    "    kp_ = 0.5\n",
    "    kd_ = 0.5\n",
    "    alpha = 2\n",
    "    return kp_, kd_, alpha\n",
    "\n",
    "def get_pid_parameters():\n",
    "    '''\n",
    "        Returns actual Kp, Ki and Kd values by using normalized values of Kp,Kd and alpha in\n",
    "        the euations given in Research paper. Currently it returns constant values.\n",
    "    '''\n",
    "    ku = 4.7\n",
    "    tu = 3.3\n",
    "    kpmax = 0.6*ku\n",
    "    kpmin = 0.32*ku\n",
    "    kdmax = 0.15*ku*tu\n",
    "    kdmin = 0.08*ku*tu\n",
    "    kp_, kd_, alpha = get_normalized_values()\n",
    "    kp = (kpmax - kpmin)*kp_ + kpmin\n",
    "    kd = (kdmax - kdmin)*kd_ + kdmin\n",
    "    ki = kp**2/(alpha*kd)\n",
    "    params = {'kp':kp, 'ki':ki, 'kd':kd}\n",
    "    return params\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [],
   "source": [
    "#Here we Simulate out Model+PID controller over time-step of 0.1 Seconds\n",
    "def simulation(kp=2.8,ki=1.7,kd=1.12):\n",
    "    #PID Tuning parameters\n",
    "    params = {'kp':kp,'ki':ki,'kd':kd}\n",
    "    \n",
    "    #Time-Span with time-step of 0.1 Seconds\n",
    "    t = np.linspace(0, 14, 140)\n",
    "    delta_t = t[1] - t[0]\n",
    "    \n",
    "    #Initialising y0\n",
    "    y0 = 0\n",
    "    #Storing y(t) responses\n",
    "    y = np.ones(len(t))*y0\n",
    "    #Storing y'(t) reponses\n",
    "    y1 = np.ones(len(t))*y0\n",
    "    \n",
    "    #Storing x(t) responses\n",
    "    x = np.zeros(len(t))\n",
    "    \n",
    "    #Storing e(t) responses\n",
    "    e = np.zeros(len(t))\n",
    "\n",
    "    #Step-Adding through set points\n",
    "    SP = np.ones(len(t))\n",
    "    \n",
    "    #Simulating the PID+Model system over time-step of 0.1s\n",
    "    for i in range(len(t)-1):\n",
    "        #Here we can also call a function to input PID parameters at each time-step\n",
    "        #params = get_pid_parameters()\n",
    "        x[i+1], e[i+1] = PID(y[i], SP[i], params, delta_t)\n",
    "        #Using odeint we are returned two values y(t) and y'(t) we store them in y, y1 respectively\n",
    "        z = odeint(model, [y[i], y1[i]], t[i:i+2], args=(x[i+1],))\n",
    "        y1[i+1] = z[-1][1]\n",
    "        y[i+1] = z[-1][0]\n",
    "    \n",
    "    #Plotting\n",
    "    plt.figure(figsize=(7, 7), dpi= 80, facecolor='w', edgecolor='k')\n",
    "    plt.subplot(2,1,1)\n",
    "    plt.plot(t, y, 'r-', label='y(t)')\n",
    "    plt.plot(t, SP, 'b--', label='SP')\n",
    "    plt.legend(loc='best')\n",
    "    plt.subplot(2,1,2)\n",
    "    plt.plot(t, e, 'k', label='e(t)')\n",
    "    plt.ylabel('Error')\n",
    "    plt.xlabel('Time')\n",
    "    plt.legend(loc='best')\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "09d40c2e8c8f46eeba63c7375d122d82",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "interactive(children=(FloatSlider(value=2.8, description='kp', max=200.0, min=-20.0), FloatSlider(value=1.7, d…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "interactive_plt = interactive(simulation, kp=(-20.0,200.0), ki=(-20.0,20.0), kd=(-20.0,20.0))\n",
    "interactive_plt"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "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.7.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}