{ "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 and\n", "the associated equations of motion, as shown below:\n", "\n", "[missing]\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$. In the diagram above, the force exerted by the spring is a nonlinear function of the head position due to the way it is attached. The measured output of the system corresponds to the displacement of the spring with a velocity dependent perturbation. 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", "$$" ] }, { "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\n", "\n", "# Import nonlinear I/O model of the disk drive dynamics\n", "from diskdrive import diskdrive\n", "print(diskdrive)\n", "print(\"Params:\", diskdrive.params)" ] }, { "cell_type": "markdown", "id": "missing-asian", "metadata": {}, "source": [ "## Exercises" ] }, { "cell_type": "markdown", "id": "competitive-terrain", "metadata": {}, "source": [ "\n", "(a) Compute the linearization of the dynamics about the equilibrium point corresponding\n", "to $\\theta_\\text{e} = 15^\\circ$.\n", "" ] }, { "cell_type": "code", "execution_count": null, "id": "naughty-barrel", "metadata": {}, "outputs": [], "source": [ "# Add code for your solution here, including comments explaining your answer" ] }, { "cell_type": "markdown", "id": "instant-lancaster", "metadata": {}, "source": [ "\n", "(b) Plot the step response of the linearized, open-loop system and compute the rise time and settling time.\n", "" ] }, { "cell_type": "code", "execution_count": null, "id": "unusual-haiti", "metadata": {}, "outputs": [], "source": [ "# Add code for your solution here, including comments explaining your answer" ] }, { "cell_type": "markdown", "id": "stuffed-premiere", "metadata": {}, "source": [ "\n", "(c) Design a state feedback controller for the system that stabilizes the system about $\\theta_\\text{e} = 45^\\circ$ and sets the closed loop eigenvalues to $\\lambda_{1,2} = −1 \\pm \\sqrt{3} i$. Plot the step response for the closed loop system and compute the rise time, settling time, and steady state error.\n", "" ] }, { "cell_type": "code", "execution_count": null, "id": "living-objective", "metadata": {}, "outputs": [], "source": [ "# Add code for your solution here, including comments explaining your answer" ] }, { "cell_type": "markdown", "id": "vulnerable-afghanistan", "metadata": {}, "source": [ "\n", "(d) Compute the transfer function $H_{yu}$ for the open system around the equilibrium point and sketch the frequency response of the open loop system.\n", "" ] }, { "cell_type": "code", "execution_count": null, "id": "nonprofit-coral", "metadata": {}, "outputs": [], "source": [ "# Add code for your solution here, including comments explaining your answer" ] }, { "cell_type": "markdown", "id": "fifty-bibliography", "metadata": {}, "source": [ "\n", "(e) Design a frequency domain compensator that provides tracking with less than 10% error up to 1 rad/sec and has a phase margin of at least $45^\\circ$. Demonstrate that your controller meets these requirements by showing Bode, Nyquist, and step response plots, and compute the rise time, settling time, and steady state error for the system using your controller design.\n", "" ] }, { "cell_type": "code", "execution_count": null, "id": "driving-therapy", "metadata": {}, "outputs": [], "source": [ "# Add code for your solution here, including comments explaining your answer" ] }, { "cell_type": "code", "execution_count": null, "id": "sexual-asian", "metadata": {}, "outputs": [], "source": [ "P * C" ] }, { "cell_type": "code", "execution_count": null, "id": "institutional-bulgarian", "metadata": {}, "outputs": [], "source": [ "# The code below will generate the plots required to validate your controller\n", "# assuming that you define L to be the loop transfer function for the system\n", "#\n", "# You do not need to modify this code; just run as is\n", "\n", "# Plot the frequency response\n", "ct.bode_plot(L)\n", "\n", "# Compute stability margins\n", "gm, pm, wcg, wcp = ct.margin(L)\n", "print(\"Phase margin =\", pm)\n", "print(\"Percent error at 1 rad/sec = \", abs(1/(1 + L(1j))) * 100)\n", "\n", "# Check the Nyquist plot to make sure everything looks OK\n", "plt.figure()\n", "count = ct.nyquist_plot(L)\n", "print(\"Nyquist encirclements =\", count)\n", "\n", "# Compute the transfer function for the steady state system\n", "G_closed = L.feedback(1)\n", "\n", "# Generate a step response to show that everything is OK\n", "plt.figure()\n", "time, output = ct.step_response(G_closed)\n", "plt.plot(time, output)\n", "plt.gca().set(xlim=(0,5))\n", "\n", "plt.xlabel(\"Time $t$ [s]\")\n", "plt.ylabel(\"Position $y$ [m]\")\n", "plt.title(\"Step response for closed loop, frequency domain controller\")\n", "\n", "# Compute and print properties of the step response\n", "results = ct.step_info(G_closed)\n", "print(\"\")\n", "print(\"Rise time:\", results['RiseTime'])\n", "print(\"Settling time:\", results['SettlingTime'])\n", "print(\"Steady state error (percent):\", abs(results['SteadyStateValue'] - 1) * 100)" ] }, { "cell_type": "markdown", "id": "rocky-hobby", "metadata": {}, "source": [ "\n", "(f) Create simulations of the full nonlinear system with the linear controllers designed in parts (c) and (e) and plot the response of the system from an initial position of 0 mm at $t = 0$, to 0.4 mm at $t = 30$ ms, to 1.2 mm at $t = 90$ ms, to 0.8 mm at $t = 120$ ms.\n", "" ] }, { "cell_type": "code", "execution_count": null, "id": "finite-reconstruction", "metadata": {}, "outputs": [], "source": [ "# Add code for your solution here, including comments explaining your answer" ] }, { "cell_type": "code", "execution_count": null, "id": "utility-community", "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 output y, and computes tau as its output. For this template,\n", "# we assume that C_ss is a state space representation of a dynamic controller.\n", "\n", "def controller_update(t, states, inputs, params):\n", " r = inputs[0]\n", " y = inputs[1]\n", " return C_ss.A @ states + C_ss.B * (r - y)\n", "\n", "def controller_output(t, states, inputs, params):\n", " r = inputs[0]\n", " y = inputs[1]\n", " return C_ss.C @ states + C_ss.D * (r - y)\n", "\n", "controller = ct.NonlinearIOSystem(\n", " controller_update, controller_output, name='controller',\n", " inputs=['r', 'y'], outputs=['tau'], states=C_ss.nstates)\n", "\n", "# Create the complete control system\n", "ctrl_sys = ct.interconnect(\n", " (diskdrive, controller), name='ctrl_sys',\n", " inputs=['r'], outputs=['y'])\n", "\n", "# Create a reference trajectory to track\n", "time = np.linspace(0, 5, 500)\n", "ref = np.concatenate((\n", " np.ones(50) * 0,\n", " np.ones(150) * 0.4,\n", " np.ones(150) * 1.2,\n", " np.ones(150) * 0.8,\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": "seven-uncertainty", "metadata": {}, "outputs": [], "source": [] } ], "metadata": { "language_info": { "name": "python" } }, "nbformat": 4, "nbformat_minor": 5 }