{ "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": [ "\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": "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": [ "\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": "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": [ "\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", "" ] }, { "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": [ "\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", "" ] }, { "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": [ "\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", "" ] }, { "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 }