jetbot_control.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Road Following - Live demo"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Load Trained Model"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Load libraries"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import torchvision\n",
    "import torch\n",
    "import torch.nn.functional as F\n",
    "from efficientnet_pytorch import EfficientNet\n",
    "import time\n",
    "from datetime import datetime, timedelta\n",
    "\n",
    "from IPython.display import display\n",
    "import ipywidgets\n",
    "import traitlets\n",
    "from jetbot import Camera, bgr8_to_jpeg\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Load pretrained model. It has 3 components: 1 encoder and 2 decoders."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "encoder = EfficientNet.from_name('efficientnet-b0')\n",
    "encoder._fc = torch.nn.Identity()\n",
    "regressor = torch.nn.Linear(1280, 2)\n",
    "classifier = torch.nn.Linear(1280, 2)\n",
    "\n",
    "MODEL_PATH='multi_task_CNN/'\n",
    "encoder.load_state_dict(torch.load(MODEL_PATH+'backbone_0.pth'))\n",
    "regressor.load_state_dict(torch.load(MODEL_PATH+'head_xy_0.pth'))\n",
    "classifier.load_state_dict(torch.load(MODEL_PATH+'head_stop_0.pth'))\n",
    "\n",
    "# Transfer to GPU from CPU, use only half floats for efficiency\n",
    "device = torch.device('cuda')\n",
    "encoder = encoder.to(device)\n",
    "encoder = encoder.eval().half()\n",
    "regressor = regressor.to(device)\n",
    "regressor = regressor.eval().half()\n",
    "classifier = classifier.to(device)\n",
    "classifier = classifier.eval().half()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Creating the Pre-Processing Function"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "1. Convert from HWC layout to CHW layout\n",
    "3. Transfer the data from CPU to GPU\n",
    "4. Add a batch dimension\n",
    "\n",
    "Note: EfficientNet has built in standardization for the inputs!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "import torchvision.transforms as transforms\n",
    "import torch.nn.functional as F\n",
    "import cv2\n",
    "import PIL.Image\n",
    "import numpy as np\n",
    "\n",
    "def preprocess(image):\n",
    "    image = PIL.Image.fromarray(image)\n",
    "    image = transforms.functional.to_tensor(image).to(device).half()\n",
    "    return image[None, ...]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Start camera, max FPS is 30. If the Python kernel is shut down without closing the camera instance, a system reset will be needed to use it again (see last cell of notebook)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "camera = Camera(fps=30)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Create robot instance which will drive the motors."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 30,
   "metadata": {},
   "outputs": [],
   "source": [
    "from jetbot import Robot, bgr8_to_jpeg\n",
    "\n",
    "robot = Robot()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Check that system time is correct! Docker uses universal time, so need to add 2h."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "datetime.datetime(2021, 8, 13, 17, 34, 3, 96728)"
      ]
     },
     "execution_count": 7,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "datetime.now()+timedelta(hours=2)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Class logging the stops."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    " \n",
    "LOG_PATH = \"/workspace/temp_logs/\"\n",
    "LOG_NAME = \"stop_log.txt\"\n",
    "\n",
    "class StopLogger:\n",
    "    def __init__(self, path=LOG_PATH+LOG_NAME, start_stop=0, n_stops=16, tz_offset=2):\n",
    "        self.path=path\n",
    "        self.n_stops=n_stops\n",
    "        self.time_start=\"NaN\"\n",
    "        self.current_stop=start_stop-1\n",
    "        self.tz_offset=tz_offset\n",
    "    def start_stop(self):\n",
    "        now=datetime.now()+timedelta(hours=self.tz_offset)\n",
    "        self.time_start=now.strftime(\"%d/%m/%Y %H:%M:%S.%f\")\n",
    "        self.current_stop=(self.current_stop+1)%self.n_stops\n",
    "    def log_stop(self):\n",
    "        with open(self.path, 'a') as f:\n",
    "            now=datetime.now()+timedelta(hours=self.tz_offset)\n",
    "            line=self.time_start+\";\"+now.strftime(\"%d/%m/%Y %H:%M:%S.%f\")+\";\"+str(self.current_stop)\n",
    "            f.write(line+\"\\n\")\n",
    "            self.time_start=\"NaN\"\n",
    "        "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The control loop methods."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 46,
   "metadata": {},
   "outputs": [],
   "source": [
    "angle = 0.0\n",
    "angle_last = 0.0\n",
    "angle_sum=0\n",
    "frame_count=0\n",
    "\n",
    "def process_frame(frame_dict, check_stop, P_thresh, base_speed, K_p, K_i, K_d, bias,\n",
    "                  max_K_d, max_PID, logger, save_pics=False):\n",
    "    '''\n",
    "    frame_dict : dictionary with a frame from the camera\n",
    "    angle : estimated angle to take (error)\n",
    "    angle_last : estimated angle to take (error) in previous step\n",
    "    angle_sum : accumulated angle (error) over all previous steps\n",
    "    K_p : proportinal gain constant\n",
    "    K_i : integral gain constant\n",
    "    K_d : derivative gain constant\n",
    "    bias : steady state bias term\n",
    "    ---------------\n",
    "    return : boolean indicating whether it stopped for measurements in this frame\n",
    "    '''   \n",
    "    global angle, angle_last, angle_sum, frame_count\n",
    "    \n",
    "    image = preprocess(frame_dict['new'])\n",
    "    features=encoder(image)\n",
    "    angle,xy=estimate_angle(features)\n",
    "    \n",
    "    PID=PID_control(angle, angle_last, angle_sum, K_p, K_i, K_d, bias, max_K_d, max_PID)\n",
    "    angle_sum += angle\n",
    "    angle_last = angle\n",
    "\n",
    "    # Constrain speed to be [0,1] for both wheels\n",
    "    robot.left_motor.value = max(min(base_speed + PID, 1.0), 0.0)\n",
    "    robot.right_motor.value = max(min(base_speed - PID, 1.0), 0.0)\n",
    "    \n",
    "    if save_pics:\n",
    "        save(frame_dict['new'],xy,frame_count)\n",
    "        frame_count+=1\n",
    "    \n",
    "    if check_stop:\n",
    "        P_move=estimate_P_move(features)\n",
    "        if P_move < P_thresh:\n",
    "            robot.stop()\n",
    "            angle_last=0.0\n",
    "            if logger is not None:\n",
    "                logger.start_stop()\n",
    "                print(\"stop nr: %d, P_move=%.4f\"%(logger.current_stop,P_move))\n",
    "                time.sleep(10)\n",
    "                logger.log_stop()\n",
    "            else:\n",
    "                time.sleep(10)\n",
    "            return True\n",
    "    \n",
    "    return False\n",
    "        \n",
    "        \n",
    "def estimate_P_move(features):\n",
    "    y = classifier(features)\n",
    "    return float(F.softmax(y, dim=1).flatten()[0])\n",
    "\n",
    "def estimate_angle(features):\n",
    "    xy = regressor(features).detach().float().cpu().numpy().flatten()\n",
    "    x = xy[0]\n",
    "    y = (xy[1]+0.5) / 2.0\n",
    "    return np.arctan2(x, y), xy\n",
    "    \n",
    "def PID_control(angle, angle_last, angle_sum, K_p, K_i, K_d, bias, max_K_d, max_PID):\n",
    "    \n",
    "    # proportional control\n",
    "    P= angle * K_p\n",
    "    # integral control\n",
    "    I = K_i * angle_sum + bias\n",
    "    # derivative control\n",
    "    D = max(min((angle - angle_last) * K_d, max_K_d),-max_K_d)\n",
    "    # Put upper limit on steering from proportional and derivative control\n",
    "    # to smooth turns.\n",
    "    return max(min( P + D, max_PID),-max_PID) + I\n",
    "\n",
    "def save(image,xy,count):\n",
    "    snapshot = image.copy()\n",
    "    snapshot = cv2.circle(snapshot, (int(xy[0]*112.0+112.0), int(xy[1]*112.0+112.0)), 8, (0, 0, 255), 3)\n",
    "    image_path = os.path.join(DATASET_DIR, str(count) + '.jpg')\n",
    "    with open(image_path, 'wb') as f:\n",
    "        f.write(bgr8_to_jpeg(snapshot))\n",
    "    \n",
    "#execute({'new': camera.value})"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Method for measuring how long the different parts of the control loop take."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "def benchmark(frame_dict):\n",
    "    t_start = time.time()\n",
    "    image = preprocess(frame_dict['new'])\n",
    "    t_pre = time.time()\n",
    "    \n",
    "    features=encoder(image)\n",
    "    t_feat = time.time()\n",
    "    xy = regressor(features).detach().float().cpu().numpy().flatten()\n",
    "    t_xy = time.time()\n",
    "    p=float(F.softmax(classifier(features), dim=1).flatten()[0])\n",
    "    t_p = time.time()\n",
    "    return {\"t_pre\":t_pre-t_start, \"t_feat\":t_feat-t_pre, \"t_xy\":t_xy-t_feat, \"t_p\":t_p-t_xy }\n",
    "    "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Method for testing the control loop without moving the robot. Used at every startup before starting operations to see if camera etc. is okay."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [],
   "source": [
    "import IPython\n",
    "def predict(change, K_p=0.1, K_i=0, K_d=0.1,\n",
    "                         bias=0, max_K_d=0.7, max_PID=0.25):\n",
    "    global out, angle, angle_last, angle_sum\n",
    "    image = preprocess(change['new'])\n",
    "    \n",
    "    p = classifier(encoder(image))\n",
    "    p=float(F.softmax(p, dim=1).flatten()[0])\n",
    "    \n",
    "    xy = regressor(encoder(image)).detach().float().cpu().numpy().flatten()\n",
    "    x = xy[0]\n",
    "    y = (0.5 - xy[1]) / 2.0\n",
    "    y_alt=(xy[1] + 0.5) / 2.0\n",
    "    angle_base=np.arctan2(x, xy[1])\n",
    "    angle=np.arctan2(x, y)\n",
    "    angle_alt=np.arctan2(x, y_alt)\n",
    "    pid_base=PID_control(angle_base, angle_last, angle_sum, K_p, K_i, K_d, bias, max_K_d, max_PID)\n",
    "    pid=PID_control(angle, angle_last, angle_sum, K_p, K_i, K_d, bias, max_K_d, max_PID)\n",
    "    pid_alt=PID_control(angle_alt, angle_last, angle_sum, K_p, K_i, K_d, bias, max_K_d, max_PID)\n",
    "    \n",
    "    out0.update(IPython.display.Pretty('P: %.2f ; x: %.2f ; y: %.2f -> angle: %.2f ; pid: %.2f' % (p, x, xy[1], angle_base*180/3.14,pid_base)))\n",
    "    out.update(IPython.display.Pretty('P: %.2f ; x: %.2f ; y: %.2f -> angle: %.2f ; pid: %.2f' % (p, x, y, angle*180/3.14,pid)))\n",
    "    out2.update(IPython.display.Pretty('P: %.2f ; x: %.2f ; y: %.2f -> angle: %.2f ; pid: %.2f' % (p, x, y_alt, angle_alt*180/3.14,pid_alt)))\n",
    "    \n",
    "\n",
    "    "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Code snippet used for calibrating the bias term for PID. This will move the robot!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "'\\n#old bias before fixed back-wheel was -0.0332\\nbase_speed=0.25\\nbias= 0.03\\nrobot.left_motor.value = base_speed + bias\\nrobot.right_motor.value = base_speed - bias\\ntime.sleep(3)\\nrobot.stop()\\n'"
      ]
     },
     "execution_count": 12,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "'''\n",
    "#old bias before fixed back-wheel was -0.0332\n",
    "base_speed=0.25\n",
    "bias= 0.03\n",
    "robot.left_motor.value = base_speed + bias\n",
    "robot.right_motor.value = base_speed - bias\n",
    "time.sleep(3)\n",
    "robot.stop()\n",
    "'''"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Widget to check what the robot sees. It displays the camera feed."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "\"image_widget = ipywidgets.Image()\\n\\ntraitlets.dlink((camera, 'value'), (image_widget, 'value'), transform=bgr8_to_jpeg)\\n\\ndisplay(image_widget)\""
      ]
     },
     "execution_count": 13,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "'''image_widget = ipywidgets.Image()\n",
    "\n",
    "traitlets.dlink((camera, 'value'), (image_widget, 'value'), transform=bgr8_to_jpeg)\n",
    "\n",
    "display(image_widget)'''"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Code snippet for initital check before starting to use the robot. It takes time to initialize the CNNs on the GPU, so wait until the display changes from \"Start\" to the estimated control values."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "P: 1.00 ; x: 0.02 ; y: -0.18 -> angle: 175.30 ; pid: 0.25"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "P: 1.00 ; x: 0.02 ; y: 0.34 -> angle: 2.57 ; pid: 0.01"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "P: 1.00 ; x: 0.02 ; y: 0.16 -> angle: 5.53 ; pid: 0.02"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "out0 = display(IPython.display.Pretty('Start'), display_id=True)\n",
    "out = display(IPython.display.Pretty('Start'), display_id=True)\n",
    "out2 = display(IPython.display.Pretty('Start'), display_id=True)\n",
    "camera.observe(predict, names='value')\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If estimates look fine, turn it off with camera.unobserve() so the actual deployment can be started."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [],
   "source": [
    "camera.unobserve(predict, names='value')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Robot deployment"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Set parameters, initialize logger object."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 61,
   "metadata": {},
   "outputs": [],
   "source": [
    "actions_before_break=15\n",
    "break_s=0.2\n",
    "has_stopped=False\n",
    "check_stop=True\n",
    "counter=0\n",
    "base_speed=0.24\n",
    "bias=0.03\n",
    "start_boost=0.08\n",
    "start=True\n",
    "logger=StopLogger(start_stop=9)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Start moving the robot. The outer loop defines how long the robot should operate. Every iteration is one cycle with \"actions_before_break\" frames processed. Finishing one round around ESAT 00.08 takes around 70 iterations."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 62,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "stop nr: 9, P_move=0.0011\n",
      "stop nr: 10, P_move=0.0023\n",
      "stop nr: 11, P_move=0.0038\n",
      "stop nr: 12, P_move=0.0010\n",
      "stop nr: 13, P_move=0.0000\n",
      "stop nr: 14, P_move=0.0000\n",
      "stop nr: 15, P_move=0.0000\n",
      "stop nr: 0, P_move=0.0001\n",
      "stop nr: 1, P_move=0.0002\n",
      "stop nr: 2, P_move=0.0003\n",
      "stop nr: 3, P_move=0.0016\n",
      "stop nr: 4, P_move=0.0003\n",
      "stop nr: 5, P_move=0.0001\n",
      "stop nr: 6, P_move=0.0000\n",
      "stop nr: 7, P_move=0.0000\n",
      "stop nr: 8, P_move=0.0002\n",
      "stop nr: 9, P_move=0.0138\n",
      "stop nr: 10, P_move=0.0050\n",
      "stop nr: 11, P_move=0.0001\n",
      "stop nr: 12, P_move=0.0005\n",
      "stop nr: 13, P_move=0.0000\n",
      "stop nr: 14, P_move=0.0002\n",
      "stop nr: 15, P_move=0.0000\n",
      "stop nr: 0, P_move=0.0006\n",
      "stop nr: 1, P_move=0.0015\n",
      "stop nr: 2, P_move=0.0068\n",
      "stop nr: 3, P_move=0.0014\n",
      "done\n"
     ]
    }
   ],
   "source": [
    "for i in range(400):\n",
    "    for i in range(actions_before_break):\n",
    "        # extra speed when starting to overcome static friction\n",
    "        if start:\n",
    "            base_speed_adj=base_speed+start_boost\n",
    "            start=False\n",
    "        else:\n",
    "            base_speed_adj=base_speed\n",
    "        # if robot stops to take measurements do not stop again for m cycles\n",
    "        if process_frame({'new': camera.value},check_stop, P_thresh=0.015,\n",
    "                         base_speed=base_speed_adj, K_p=0.17, K_i=0, K_d=0.2,\n",
    "                         bias=bias, max_K_d=0.4, max_PID=0.33, logger=logger):\n",
    "            check_stop=False\n",
    "            counter=0\n",
    "            start=True\n",
    "        else:\n",
    "            counter+=1\n",
    "        if counter > 30:\n",
    "            check_stop=True\n",
    "\n",
    "    robot.stop()\n",
    "    angle_last=0.0\n",
    "    time.sleep(break_s)\n",
    "    start=True\n",
    "print(\"done\")   "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If robot control loop needs to be terminated early, the motors will continue at their last speed setting. To stop the motors robot.stop() needs to be used."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 45,
   "metadata": {},
   "outputs": [],
   "source": [
    "robot.stop()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Benchmarking"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 27,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "preprocessing: 0.00041s, 0.047 of total\n",
      "feature extraction: 0.00828s, 0.933 of total\n",
      "regression: 0.00010s, 0.011 of total\n",
      "classification: 0.00008s, 0.009 of total\n"
     ]
    }
   ],
   "source": [
    "time_sum_dict={\"t_pre\":0, \"t_feat\":0, \"t_xy\":0, \"t_p\":0 }\n",
    "total_time_sum=0\n",
    "for i in range(100):\n",
    "    time_dict=benchmark({'new': camera.value})\n",
    "    for key in time_dict.keys():\n",
    "        time_sum_dict[key]+=time_dict[key]\n",
    "        total_time_sum+=time_dict[key]\n",
    "\n",
    "print(\"preprocessing: {:.5f}s, {:.3f} of total\".format(time_sum_dict[\"t_pre\"]/1000,time_sum_dict[\"t_pre\"]/total_time_sum))\n",
    "print(\"feature extraction: {:.5f}s, {:.3f} of total\".format(time_sum_dict[\"t_feat\"]/1000,time_sum_dict[\"t_feat\"]/total_time_sum))\n",
    "print(\"regression: {:.5f}s, {:.3f} of total\" .format(time_sum_dict[\"t_xy\"]/1000,time_sum_dict[\"t_xy\"]/total_time_sum))\n",
    "print(\"classification: {:.5f}s, {:.3f} of total\".format(time_sum_dict[\"t_p\"]/1000,time_sum_dict[\"t_p\"]/total_time_sum))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Code for recording what the robot sees + marking predictions."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 47,
   "metadata": {},
   "outputs": [],
   "source": [
    "import os\n",
    "DATASET_DIR = 'demo'\n",
    "\n",
    "# we have this \"try/except\" statement because these next functions can throw an error if the directories exist already\n",
    "try:\n",
    "    os.makedirs(DATASET_DIR)\n",
    "except FileExistsError:\n",
    "    print('Directories not created because they already exist')\n",
    "actions_before_break=15\n",
    "break_s=0.2\n",
    "has_stopped=False\n",
    "check_stop=True\n",
    "counter=0\n",
    "base_speed=0.22\n",
    "bias=0.03\n",
    "start_boost=0.08\n",
    "start=True\n",
    "logger=None"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 48,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "done\n"
     ]
    }
   ],
   "source": [
    "for i in range(100):\n",
    "    for i in range(actions_before_break):\n",
    "        # extra speed when starting to overcome static friction\n",
    "        if start:\n",
    "            base_speed_adj=base_speed+start_boost\n",
    "            start=False\n",
    "        else:\n",
    "            base_speed_adj=base_speed\n",
    "        # if robot stops to take measurements do not stop again for m cycles\n",
    "        if process_frame({'new': camera.value},check_stop, P_thresh=0.015,\n",
    "                         base_speed=base_speed_adj, K_p=0.17, K_i=0, K_d=0.2,\n",
    "                         bias=bias, max_K_d=0.4, max_PID=0.33, logger=None, save_pics=True):\n",
    "            check_stop=False\n",
    "            counter=0\n",
    "            start=True\n",
    "        else:\n",
    "            counter+=1\n",
    "        if counter > 30:\n",
    "            check_stop=True\n",
    "\n",
    "    robot.stop()\n",
    "    angle_last=0.0\n",
    "    time.sleep(break_s)\n",
    "    start=True\n",
    "print(\"done\")   \n",
    "!zip -r -q demo.zip {DATASET_DIR}"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### CAMERA NEEDS TO BE CLOSED\n",
    "If the Python kernel is shut down without closing the camera instance, a system reset will be needed to use it again."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 28,
   "metadata": {},
   "outputs": [],
   "source": [
    "camera.stop()"
   ]
  }
 ],
 "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.8.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}