{
"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
}