PID-Controller.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[<matplotlib.lines.Line2D at 0x1eb39bb8a90>]"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "image/png": "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",
      "text/plain": [
       "<Figure size 576x576 with 2 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "# -----------\n",
    "# User Instructions\n",
    "#\n",
    "# Implement a P controller by running 100 iterations\n",
    "# of robot motion. The steering angle should be set\n",
    "# by the parameter tau so that:\n",
    "#\n",
    "# steering = -tau_p * CTE - tau_d * diff_CTE - tau_i * int_CTE\n",
    "#\n",
    "# where the integrated crosstrack error (int_CTE) is\n",
    "# the sum of all the previous crosstrack errors.\n",
    "# This term works to cancel out steering drift.\n",
    "#\n",
    "# Only modify code at the bottom! Look for the TODO.\n",
    "# ------------\n",
    "\n",
    "import random\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "\n",
    "# ------------------------------------------------\n",
    "# \n",
    "# this is the Robot class\n",
    "#\n",
    "\n",
    "class Robot(object):\n",
    "    def __init__(self, length=20.0):\n",
    "        \"\"\"\n",
    "        Creates robot and initializes location/orientation to 0, 0, 0.\n",
    "        \"\"\"\n",
    "        self.x = 0.0\n",
    "        self.y = 0.0\n",
    "        self.orientation = 0.0\n",
    "        self.length = length\n",
    "        self.steering_noise = 0.0\n",
    "        self.distance_noise = 0.0\n",
    "        self.steering_drift = 0.0\n",
    "\n",
    "    def set(self, x, y, orientation):\n",
    "        \"\"\"\n",
    "        Sets a robot coordinate.\n",
    "        \"\"\"\n",
    "        self.x = x\n",
    "        self.y = y\n",
    "        self.orientation = orientation % (2.0 * np.pi)\n",
    "\n",
    "    def set_noise(self, steering_noise, distance_noise):\n",
    "        \"\"\"\n",
    "        Sets the noise parameters.\n",
    "        \"\"\"\n",
    "        # makes it possible to change the noise parameters\n",
    "        # this is often useful in particle filters\n",
    "        self.steering_noise = steering_noise\n",
    "        self.distance_noise = distance_noise\n",
    "\n",
    "    def set_steering_drift(self, drift):\n",
    "        \"\"\"\n",
    "        Sets the systematical steering drift parameter\n",
    "        \"\"\"\n",
    "        self.steering_drift = drift\n",
    "\n",
    "    def move(self, steering, distance, tolerance=0.001, max_steering_angle=np.pi / 4.0):\n",
    "        \"\"\"\n",
    "        steering = front wheel steering angle, limited by max_steering_angle\n",
    "        distance = total distance driven, most be non-negative\n",
    "        \"\"\"\n",
    "        if steering > max_steering_angle:\n",
    "            steering = max_steering_angle\n",
    "        if steering < -max_steering_angle:\n",
    "            steering = -max_steering_angle\n",
    "        if distance < 0.0:\n",
    "            distance = 0.0\n",
    "\n",
    "        # apply noise\n",
    "        steering2 = random.gauss(steering, self.steering_noise)\n",
    "        distance2 = random.gauss(distance, self.distance_noise)\n",
    "\n",
    "        # apply steering drift\n",
    "        steering2 += self.steering_drift\n",
    "\n",
    "        # Execute motion\n",
    "        turn = np.tan(steering2) * distance2 / self.length\n",
    "\n",
    "        if abs(turn) < tolerance:\n",
    "            # approximate by straight line motion\n",
    "            self.x += distance2 * np.cos(self.orientation)\n",
    "            self.y += distance2 * np.sin(self.orientation)\n",
    "            self.orientation = (self.orientation + turn) % (2.0 * np.pi)\n",
    "        else:\n",
    "            # approximate bicycle model for motion\n",
    "            radius = distance2 / turn\n",
    "            cx = self.x - (np.sin(self.orientation) * radius)\n",
    "            cy = self.y + (np.cos(self.orientation) * radius)\n",
    "            self.orientation = (self.orientation + turn) % (2.0 * np.pi)\n",
    "            self.x = cx + (np.sin(self.orientation) * radius)\n",
    "            self.y = cy - (np.cos(self.orientation) * radius)\n",
    "\n",
    "    def __repr__(self):\n",
    "        return '[x=%.5f y=%.5f orient=%.5f]' % (self.x, self.y, self.orientation)\n",
    "\n",
    "############## ADD / MODIFY CODE BELOW ####################\n",
    "# ------------------------------------------------------------------------\n",
    "#\n",
    "# run - does a single control run\n",
    "\n",
    "robot = Robot()\n",
    "robot.set(0, 1, 0)\n",
    "\n",
    "\n",
    "def run(robot, tau_p, tau_d, tau_i, n=100, speed=1.0):\n",
    "    x_trajectory = []\n",
    "    y_trajectory = []\n",
    "    # TODO: your code here\n",
    "    prev_CTE = robot.y\n",
    "    int_CTE = 0\n",
    "    for i in range(n):\n",
    "        CTE = robot.y\n",
    "        diff_CTE = CTE - prev_CTE\n",
    "        int_CTE += CTE                                                #to correct the drifting in steering \n",
    "        steer = - tau_p * CTE - tau_d * diff_CTE - tau_i * int_CTE\n",
    "        prev_CTE = CTE\n",
    "        robot.move(steer,speed)\n",
    "        \n",
    "        x_trajectory.append(robot.x)\n",
    "        y_trajectory.append(robot.y)\n",
    "    return x_trajectory, y_trajectory\n",
    "\n",
    "\n",
    "x_trajectory, y_trajectory = run(robot, 0.2, 3.0, 0.004)\n",
    "n = len(x_trajectory)\n",
    "\n",
    "fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(8,8))\n",
    "ax1.plot(x_trajectory, y_trajectory, 'g', label='PID controller')\n",
    "ax1.plot(x_trajectory, np.zeros(n), 'r', label='reference')\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3.9.12 ('base')",
   "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.9.12"
  },
  "orig_nbformat": 4,
  "vscode": {
   "interpreter": {
    "hash": "e572caa66b9d49951af5480c4e65bd874b4d163b9e077ed4369f2912d83d117b"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}