{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "hairy-humidity",
   "metadata": {},
   "source": [
    "# Servo Mechanism Control System\n",
    "\n",
    "## Student name, today's date\n",
    "\n",
    "Consider a simple mechanism for positioning a mechanical arm whose equations of motion are given by\n",
    "\n",
    "$$\n",
    "J \\ddot \\theta = -b \\dot\\theta - k r\\sin\\theta + \\tau_\\text{m},\n",
    "$$\n",
    "\n",
    "which can be written in state space form as\n",
    "\n",
    "$$\n",
    "\\frac{d}{dt} \\begin{bmatrix} \\theta \\\\ \\dot\\theta \\end{bmatrix} =\n",
    "  \\begin{bmatrix} \\dot\\theta \\\\ -k r \\sin\\theta / J - b\\dot\\theta / J \\end{bmatrix}\n",
    "  + \\begin{bmatrix} 0 \\\\ 1/J \\end{bmatrix} \\tau_\\text{m}.\n",
    "$$\n",
    "\n",
    "The system consists of a spring loaded arm that is driven by a  motor. The motor applies a torque that twists the arm against a linear spring and moves the end of the arm across a rotating platter. The input to the system is the motor torque $\\tau_\\text{m}$. The force exerted by the spring is a nonlinear function of the head position due to the way it is attached. The  output of the system corresponds to the displacement of the spring with a velocity dependent perturbation and the angular velocity:\n",
    "\n",
    "$$\n",
    "y = l \\theta + \\epsilon \\dot\\theta\n",
    "$$\n",
    "\n",
    "The system parameters are given by\n",
    "\n",
    "$$\n",
    "k = 1,\\quad J = 100,\\quad b = 10,\n",
    "\\quad r = 1,\\quad l = 2,\\quad \\epsilon = 0.01.\n",
    "$$\n",
    "and we assume that time is measured in sec and distance in m."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "invalid-carnival",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Import standard packages needed for this exercise\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "import control as ct"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3e476db9",
   "metadata": {},
   "source": [
    "The system dynamics are available in the file `servomech.py`, available from the course website.  This file defines three systems:\n",
    "* `servomech_fullstate`: servomechanism dynamics, with states as the output\n",
    "* `servomech_sensor`: sensor system, takes states and outputs position y\n",
    "* `servomech`: input/output system from tau_m to y"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "27bb3c38",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Import nonlinear I/O model of the disk drive dynamics\n",
    "from servomech import *\n",
    "print(servomech_fullstate)           # servomechanism with full state output\n",
    "print(\"\\n\", servomech_sensor)        # servomechanism sensor (state -> y)\n",
    "print(\"\\n\", servomech)               # servomechanism with position output, y\n",
    "\n",
    "print(\"\\nParams:\", servomech.params)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "missing-asian",
   "metadata": {},
   "source": [
    "## Exercise parts"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "competitive-terrain",
   "metadata": {},
   "source": [
    "<font color='blue'>\n",
    "(a) Compute the linearization of the dynamics about the equilibrium point corresponding\n",
    "to $\\theta_\\text{e} = 15^\\circ$.\n",
    "</font>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3579ba3d",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Add code for your solution here, including comments explaining your answer\n",
    "# Your output should print the A, B, C, and D matrices for the linearized system"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "instant-lancaster",
   "metadata": {},
   "source": [
    "<font color='blue'>\n",
    "(b) Plot the step response of the linearized, open-loop system and compute the rise time and settling time.\n",
    "</font>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ab4e56a0",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Add code for your solution here, including comments explaining your answer\n",
    "# Your output should be a properly labeled plot of the open loop step response"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "stuffed-premiere",
   "metadata": {},
   "source": [
    "<font color='blue'>\n",
    "(c) Design a feedback controller for the system that that allows the system to track a desired position $y_\\text{d}$  and sets the closed loop eigenvalues to $\\lambda_{1,2} = −10 \\pm 10 i$.  Plot the step response for the closed loop system and compute the rise time, settling time, and steady state error.\n",
    "</font>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1c4a11e1",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Add code for your solution here, including comments explaining your answer\n",
    "# Your answer should show the form of the state feedback along with the gains\n",
    "# for the system, a plot of the step response, and the values of the rise time,\n",
    "# settling time, and steady state error."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e0176710",
   "metadata": {},
   "source": [
    "<font color='blue'>\n",
    "(d) Plot the frequency response (Bode plot) of the closed loop system.  Use the frequency response to compute the steady state error for a step input and the bandwidth of the system.\n",
    "</font>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "45b9f5a7",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Add code for your solution here, including comments explaining your answer\\n\",\n",
    "# Your answer should include a plot of the frequency response (Bode plot) along\\n\",\n",
    "# with a calculation of the steady state error and bandwidth.\""
   ]
  },
  {
   "cell_type": "markdown",
   "id": "rocky-hobby",
   "metadata": {},
   "source": [
    "<font color='blue'>\n",
    "(e) Create simulations of the full nonlinear system with the linear controllers  designed in part (c) and plot the response of the system from an initial position of 0 m at $t = 0$, to 1 m at $t = 0.5$, to 3 cm at $t = 1$, to 2 m at $t = 1.5$ ms.\n",
    "</font>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "15e65605",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Template [use this as a starting point for your own code]\n",
    "#\n",
    "# Define a controller that takes as its inputs the reference value r and\n",
    "# the process state x = (theta, thetadot), and computes tau as its output.  For\n",
    "# this template, we assume that `controller` is the I/O representation of this\n",
    "# linear controller.\n",
    "#\n",
    "# Controller definition (your code goes here)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "utility-community",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Create the complete control system\n",
    "ctrl_sys = ct.interconnect(\n",
    "    (servomech_fullstate, controller, servomech_sensor), name='ctrl_sys',\n",
    "    inputs=['r'], outputs=['y'])\n",
    "\n",
    "# Create a reference trajectory to track\n",
    "time = np.linspace(0, 2.5, 250)\n",
    "ref = np.concatenate((\n",
    "    np.ones(50) * 0,\n",
    "    np.ones(50) * 1,\n",
    "    np.ones(50) * 3,\n",
    "    np.ones(100) * 2,\n",
    "    ))\n",
    "\n",
    "# Create the system response and plot the results\n",
    "time, outputs = ct.input_output_response(ctrl_sys, time, ref)\n",
    "plt.plot(time, outputs)\n",
    "\n",
    "# Plot the reference trajectory\n",
    "plt.plot(time, ref, 'k--');\n",
    "\n",
    "# Label the plot\n",
    "plt.xlabel(\"Time $t$ [s]\")\n",
    "plt.ylabel(\"Position $y$ [m]\")\n",
    "plt.title(\"Trajectory tracking with full nonlinear dynamics\");"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "074427a3",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "language_info": {
   "name": "python"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}