{ "cells": [ { "cell_type": "markdown", "metadata": {}, "source": [ "# Steering Control\n", "\n", "## Preliminaries\n", "\n", "The next lines setup some things and import the various libraries required to run this notebook.\n", "\n", "Don't forget to initialize the server with:\n", "\n", " sudo pyctrl_start_server -m pyctrl.rc.drive\n" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Preliminaries\n", "\n", "The next lines setup some things and import the various libraries required to run this notebook." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "%matplotlib notebook\n", "import numpy as np\n", "import matplotlib.pyplot as plt\n", "import math\n", "import time\n", "\n", "from pyctrl.block import ShortCircuit, Wrap\n", "from pyctrl.block.system import Differentiator, Gain, Feedback, System, Sum, Subtract\n", "from pyctrl.block import Interp, Logger, Constant\n", "from pyctrl.system.tf import PID\n", "\n", "from pyctrl.client import Controller\n", "HOST, PORT = \"192.168.8.1\", 9999\n", "mip = Controller(host = HOST, port = PORT)\n", "\n", "mip.reset()\n", "print(mip.info('all'))" ] }, { "cell_type": "markdown", "metadata": { "collapsed": true }, "source": [ "## Closed-loop Velocity Control\n", "\n", "In closed-loop control, the control input, in this case the voltage controlled by the `pwm` signal, is also produced by an algorithm, the **controller**. However, in addition to the **reference signal**, in this case a velocity, the controller also responds to a **measurement**, in this case the velocity, which is **fedback** into the signal producing the measurement as shown in the following block-diagram:\n", "\n", "\n", "\n", "With the purpose of analyzing the resulting closed-loop system assume that the motor and the controller are both well represented by the linear models:\n", "\n", "$$\n", "\\begin{aligned}\n", "v(x) &= g \\, x, &\n", "x(e) &= k \\, e\n", "\\end{aligned}\n", "$$\n", "where $e$ represents the **error** signal in the above diagram." ] }, { "cell_type": "markdown", "metadata": { "collapsed": true }, "source": [ "## Closed-loop Position Control\n", "\n", "Feedback can be used to control any quantity which can be measured. The underlying algorithm is often based on the exact same diagram you studied before. For example, if one would like to control the position of the MIP rather than its velocity, then one could use position feedback as in the following diagram:\n", "\n", "" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Steering control\n", "\n", "As you studied before, the relation between the `pwm` inputs, the forward velocity, $v$, and the angular velocity, $\\omega$, can be represented by the relations:\n", "\n", "$$\n", "\\begin{aligned}\n", " \\omega &= \\frac{r}{d} (\\omega_r - \\omega_l), \\\\\n", " v &= \\frac{r}{2} (\\omega_r + \\omega_l)\n", "\\end{aligned}\n", "$$\n", "\n", "where $r$ is the radius of the wheels and $d$ is the distance between the wheels. The above relationships can be represented by the block diagram:\n", "\n", "\n", "\n", "Because the angular velocity, $\\omega$, is the derivative of the heading angle, $\\theta$, steering control can be performed based on the following diagram:\n", "\n", "\n", "\n", "where $\\bar{\\theta}$ is the reference angle and $\\overline{\\mathrm{pwm}}$ is a open-loop control input for the forward velocity. Of course one could also close the loop on the forward velocity as well.\n", "\n", "The signal corresponding to the heading angle $\\theta$ in the MIP is the signal `yaw`." ] }, { "cell_type": "code", "execution_count": null, "metadata": { "collapsed": true }, "outputs": [], "source": [ "mip.reset()" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "collapsed": true }, "outputs": [], "source": [ "T = 5\n", "K = 10" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# Add reference signal\n", "mip.add_signals('ref_yaw')\n", "\n", "# add differentiators\n", "mip.add_signals('velocity1','velocity2')\n", "mip.add_filter('velocity1', Differentiator(), ['clock', 'encoder1'],['velocity1'])\n", "mip.add_filter('velocity2', Differentiator(), ['clock', 'encoder2'],['velocity2'])\n", "\n", "# Closed-loop controllers\n", "mip.add_filter('Controller1',\n", " Feedback(block = Gain(gain = K)),\n", " ['yaw','ref_yaw'],\n", " ['pwm_yaw'])\n", "\n", "mip.add_filter('Sum1',\n", " Subtract(),\n", " ['pwm_yaw','ref_pwm1'],\n", " ['pwm1'])\n", "\n", "mip.add_filter('Sum2',\n", " Sum(),\n", " ['pwm_yaw','ref_pwm2'],\n", " ['pwm2'])\n", "\n", "# add logger\n", "mip.add_sink('logger', Logger(auto_reset = True),\n", " ['clock', \n", " 'encoder1', 'encoder2', \n", " 'velocity1','velocity2',\n", " 'pwm1','pwm2',\n", " 'yaw'])\n", "\n", "# add a timer to stop the controller\n", "mip.add_timer('stop',\n", " Constant(value = 0),\n", " None, ['is_running'],\n", " period = T, repeat = False)\n", "\n", "print(mip.info('all'))" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "collapsed": true }, "outputs": [], "source": [ "ref_pwm1 = 30\n", "ref_pwm2 = 30\n", "ref_yaw = 0.5\n", "\n", "mip.set_signal('ref_pwm1', ref_pwm1)\n", "mip.set_signal('ref_pwm2', ref_pwm2)\n", "mip.set_signal('ref_yaw', ref_yaw)\n", "with mip:\n", " mip.join()\n", "log = mip.get_sink('logger', 'log')" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Calculate the average heading angle and its standard deviation:" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "clock = log['clock']\n", "mean_yaw = log['yaw'][clock > clock[0] + 1].mean()\n", "std_yaw = log['yaw'][clock > clock[0] + 1].std()\n", "print('yaw: mean = {:5.3f}, std = {:5.3f}%'.format(mean_yaw,100*std_yaw/mean_yaw))" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Plot the heading angle:" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "scrolled": true }, "outputs": [], "source": [ "plt.figure()\n", "plt.plot(log['clock'], log['yaw'], 'b',\n", " [clock[0],clock[-1]], [ref_yaw,ref_yaw], 'b--',\n", " [clock[0],clock[-1]], [mean_yaw,mean_yaw], 'b')\n", "plt.xlabel('time (s)')\n", "plt.ylabel('yaw (rad)')\n", "plt.title('yaw x time')\n", "plt.grid()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Plot *pwm versus time*:" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "scrolled": true }, "outputs": [], "source": [ "plt.figure()\n", "plt.plot(clock, log['pwm1'],\n", " clock, log['pwm2'])\n", "plt.ylabel('pwm (%)')\n", "plt.xlabel('time (s)')\n", "plt.title('pwm x time')\n", "plt.grid()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Plot *velocities versus time*:" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "scrolled": false }, "outputs": [], "source": [ "plt.figure()\n", "plt.plot(log['clock'], log['velocity1'], 'b',\n", " log['clock'], log['velocity2'], 'r')\n", "plt.xlabel('time (s)')\n", "plt.ylabel('velocity (Hz)')\n", "plt.title('velocity x time')\n", "plt.grid()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "#### Reference signal #1" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "collapsed": true }, "outputs": [], "source": [ "# Interpolated input signals\n", "T = 5\n", "ts = [0, T]\n", "yaws = 2*np.pi*np.array([0, 2])" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "scrolled": true }, "outputs": [], "source": [ "plt.figure()\n", "plt.plot(ts, yaws)\n", "plt.ylabel('position (cycles)')\n", "plt.xlabel('time (s)')\n", "plt.grid()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "#### Reference signal #2" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "collapsed": true }, "outputs": [], "source": [ "# Interpolated input signals\n", "T = 10\n", "ts = [0, T/4, T/2, 3*T/4, T]\n", "yaws = 2*np.pi*np.array([0, 1, 0, 1, 0])" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "scrolled": true }, "outputs": [], "source": [ "plt.figure()\n", "plt.plot(ts, yaws)\n", "plt.ylabel('position (cycles)')\n", "plt.xlabel('time (s)')\n", "plt.grid()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "#### Reference signal #3" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "collapsed": true }, "outputs": [], "source": [ "# Interpolated input signals\n", "T = 10\n", "ts = [0, T/4, T/4, 2*T/4, 2*T/4, 3*T/4, 3*T/4, T, T]\n", "yaws = 2*np.pi*np.array([0, 0, 1/4, 1/4, 1/2, 1/2, 3/4, 3/4, 1])" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "scrolled": true }, "outputs": [], "source": [ "plt.figure()\n", "plt.plot(ts, yaws)\n", "plt.ylabel('position (cycles)')\n", "plt.xlabel('time (s)')\n", "plt.grid()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Run the controller:" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "collapsed": true }, "outputs": [], "source": [ "K=10" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# Add reference signal\n", "mip.add_signals('ref_yaw')\n", "\n", "# add filters to interpolate data\n", "mip.add_filter('ref_yaw',\n", " Interp(fp = ts, xp = yaws, period = T),\n", " ['clock'],\n", " ['ref_yaw'])\n", "\n", "# add differentiators\n", "mip.add_signals('velocity1','velocity2')\n", "mip.add_filter('velocity1', Differentiator(), ['clock', 'encoder1'],['velocity1'])\n", "mip.add_filter('velocity2', Differentiator(), ['clock', 'encoder2'],['velocity2'])\n", "\n", "# Closed-loop controllers\n", "mip.add_filter('wrap', Wrap(), ['yaw'], ['continuous_yaw'])\n", "\n", "mip.add_filter('Controller1',\n", " Feedback(block = Gain(gain = K)),\n", " ['continuous_yaw','ref_yaw'],\n", " ['pwm_yaw'])\n", "\n", "mip.add_filter('Sum1',\n", " Subtract(),\n", " ['pwm_yaw','ref_pwm1'],\n", " ['pwm1'])\n", "\n", "mip.add_filter('Sum2',\n", " Sum(),\n", " ['pwm_yaw','ref_pwm2'],\n", " ['pwm2'])\n", "\n", "# add logger\n", "mip.add_sink('logger', Logger(auto_reset = True),\n", " ['clock', \n", " 'encoder1', 'encoder2', \n", " 'velocity1','velocity2',\n", " 'pwm1','pwm2',\n", " 'continuous_yaw', 'ref_yaw'])\n", "\n", "# add a timer to stop the controller\n", "mip.add_timer('stop',\n", " Constant(value = 0),\n", " None, ['is_running'],\n", " period = T, repeat = False)\n", "\n", "print(mip.info('all'))" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "collapsed": true }, "outputs": [], "source": [ "ref_pwm1 = 30\n", "ref_pwm2 = 30\n", "\n", "mip.set_signal('ref_pwm1', ref_pwm1)\n", "mip.set_signal('ref_pwm2', ref_pwm2)\n", "mip.set_source('clock', reset=True)\n", "mip.set_filter('wrap', reset=True)\n", "mip.set_filter('ref_yaw', reset=True)\n", "with mip:\n", " mip.join()\n", "log = mip.get_sink('logger', 'log')" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Plot the heading angle:" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "scrolled": true }, "outputs": [], "source": [ "plt.figure()\n", "plt.plot(log['clock'], log['continuous_yaw'], 'b',\n", " log['clock'], log['ref_yaw'], 'b--')\n", "plt.xlabel('time (s)')\n", "plt.ylabel('yaw (rad)')\n", "plt.title('yaw x time')\n", "plt.grid()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Plot *pwm versus time*:" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "scrolled": true }, "outputs": [], "source": [ "plt.figure()\n", "plt.plot(log['clock'], log['pwm1'],\n", " log['clock'], log['pwm2'])\n", "plt.ylabel('pwm (%)')\n", "plt.xlabel('time (s)')\n", "plt.title('pwm x time')\n", "plt.grid()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Plot *velocities versus time*:" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "collapsed": true, "scrolled": false }, "outputs": [], "source": [ "plt.figure()\n", "plt.plot(log['clock'], log['velocity1'], 'b',\n", " log['clock'], log['velocity2'], 'r')\n", "plt.xlabel('time (s)')\n", "plt.ylabel('velocity (Hz)')\n", "plt.title('velocity x time')\n", "plt.grid()" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "collapsed": true }, "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.6.2" } }, "nbformat": 4, "nbformat_minor": 1 }