race1-SENDIT-Copy1.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "from picamera.array import PiYUVArray, PiRGBArray\n",
    "from picamera import PiCamera\n",
    "\n",
    "from scipy.signal import find_peaks, butter, filtfilt\n",
    "\n",
    "import time\n",
    "import matplotlib.pyplot as plt\n",
    "import skimage as ski\n",
    "import numpy as np\n",
    "\n",
    "# Camera resolution\n",
    "\n",
    "res = (640, 480)\n",
    "\n",
    "CAMERA_CENTER = res[0] // 2\n",
    "       \n",
    "from pwm import PWM"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Setup servo and motor PWM signal to control the steering angle and the speed.\n",
    "\n",
    "The values specified period and duty cycle ar in nanoseconds.\n",
    "\n",
    "Servo and motor controlled follow the following protocol:\n",
    "  1. Control signal is read 50 times per second by each device (servo and speed controller). Thus period is 20 ms => 20000000 ns\n",
    "  2. To send the minimum value (full left for servo and break for motor) the duty cycle should be 1 ms => 1000000 ns\n",
    "  3. Maximum value corresponds to a duty cycle of 2 ms => 2000000 ns\n",
    "  \n",
    "However, these devices are not perfect and you can set a duty cycle outside this range and still get a response. I would recoment to play with it and find are the limits in your case. Pay attention that servo is limited by the mechanical construction how much it can turn the wheels. Do not set these limits to a value which servo cannot execute as it will try to do that and eventually burn."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Enable servo\n",
    "SERVO_MIDDLE = 1500000\n",
    "\n",
    "servo = PWM(1)\n",
    "servo.period = 20000000\n",
    "servo.duty_cycle = SERVO_MIDDLE\n",
    "servo.enable = True"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Enable servo\n",
    "MOTOR_BRAKE = 1000000\n",
    "\n",
    "motor = PWM(0)\n",
    "motor.period = 20000000\n",
    "motor.duty_cycle = MOTOR_BRAKE\n",
    "motor.enable = True"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [],
   "source": [
    "motor.duty_cycle = MOTOR_BRAKE "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## The self driving code\n",
    "\n",
    "Quick explanation on how this algorithm works:\n",
    "  1. Initialize the camera, use a frame rate of 10 to 20 fps\n",
    "  2. Get the coefficients of the butterworth filter of order 3 and cut-off frequency of 0.1 (change cut of frequency if the peak functions is not working)\n",
    "  3. For each frame captured from the camera:\n",
    "    1. Get the blak and white part of the image\n",
    "    2. Select line 195 to use for lane detection (this number depends on your camera angle)\n",
    "    3. Filter the line and find the peaks that are greater than 160. White parts of the image have higher values than the black. The threshold of 160 depends on the ambient light, lower if the room is darker\n",
    "    4. Find the peaks left and right of the camera center, these peaks should corresponds to the white lines delimiting the track,\n",
    "    5. Compute the center point between these lines. This is where we want our car\n",
    "    6. Compute the error by substracting the center point from camera middle and multiply by 5000. 5000 is our P gain.\n",
    "    7. Adjust the duty cycle of the servo such that the"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "313 [ 41 585 615]\n",
      "313 [ 40 586 615]\n",
      "313 [ 41 585 614]\n",
      "313 [ 43 583 613]\n",
      "310 [ 39 581 610]\n",
      "309 [ 39 579 609]\n",
      "310 [ 41 580 611]\n",
      "310 [ 40 580 610]\n",
      "309 [ 37 582 609]\n",
      "314 [ 35 594]\n",
      "331 [37]\n",
      "351 [13 41]\n",
      "368 [ 8 40]\n",
      "394 [45]\n",
      "397 [48]\n",
      "398 [49]\n",
      "400 [51]\n",
      "400 [51]\n",
      "398 [49]\n",
      "399 [50]\n",
      "394 [45]\n",
      "401 [52]\n",
      "400 [51]\n",
      "406 [57]\n",
      "413 [64]\n",
      "417 [68]\n",
      "436 [87]\n",
      "450 [101]\n",
      "471 [112 122]\n",
      "498 [132 149]\n",
      "527 [155 178]\n",
      "559 [183 210]\n",
      "598 [214 231 249]\n",
      "636 [248 267 287]\n",
      "660 [271 291 311]\n",
      "319 [299 320 340]\n",
      "303 [324 340 356]\n",
      "303 [325 351]\n",
      "325 [314 336]\n",
      "321 [332 353]\n",
      "356 [368 387]\n",
      "379 [391 409]\n",
      "398 [411 427]\n",
      "394 [407 424]\n",
      "369 [382 402]\n",
      "328 [341 360]\n",
      "327 [298 314]\n",
      "294 [281]\n",
      "276 [246 263]\n",
      "263 [233 250]\n",
      "268 [238 255]\n",
      "285 [253 272]\n",
      "317 [284 304]\n",
      "327 [318 337]\n",
      "336 [346 364]\n",
      "355 [365 382]\n",
      "356 [367 384]\n",
      "342 [353 371]\n",
      "314 [325 344]\n",
      "323 [293 312]\n",
      "237 [226]\n",
      "no line\n",
      "237 []\n",
      "no line\n",
      "237 []\n",
      "no line\n",
      "237 []\n",
      "428 [638]\n",
      "410 [632]\n",
      "390 [625]\n",
      "364 [614]\n",
      "351 [601]\n",
      "329 [579]\n",
      "282 [532 549]\n",
      "102 [352 374 396 412 434 456 637]\n",
      "310 [560 597]\n",
      "463 [ 41  63  84 135 213]\n",
      "no line\n",
      "463 []\n",
      "no line\n",
      "463 []\n",
      "no line\n",
      "463 []\n",
      "no line\n",
      "463 []\n",
      "no line\n",
      "463 []\n",
      "no line\n",
      "463 []\n",
      "no line\n",
      "463 []\n",
      "268 [44 58]\n",
      "382 [127 144 160]\n",
      "471 [196 216 236]\n",
      "529 [241 260 279]\n",
      "551 [269 286 301]\n",
      "548 [273 298]\n",
      "529 [256 279]\n",
      "497 [227 247]\n",
      "453 [186 203]\n",
      "405 [145 155]\n",
      "361 [111]\n",
      "330 [80]\n",
      "342 [ 61 623]\n",
      "335 [ 49 622]\n",
      "342 [ 47 637]\n",
      "357 [47]\n",
      "374 [47]\n",
      "401 [54]\n",
      "430 [62]\n",
      "437 [69]\n",
      "353 [ 73 633]\n",
      "343 [ 67 620]\n",
      "336 [ 64 609 637]\n",
      "324 [ 10  26  54 594 630]\n",
      "316 [ 20  46 586 621]\n",
      "309 [ 39 580 615]\n",
      "304 [ 31 577 607]\n",
      "305 [ 33 578 607]\n",
      "306 [ 33 579 609]\n",
      "306 [ 32 580 608]\n",
      "309 [ 37 581 609]\n",
      "310 [ 38 582 610]\n",
      "312 [ 41 584 611]\n",
      "315 [ 44 586 613]\n",
      "316 [ 44 588 614]\n",
      "317 [ 46 588 614]\n",
      "315 [ 44 587 612]\n",
      "313 [ 42 585 613]\n",
      "314 [ 43 585 612]\n",
      "314 [ 43 585 613]\n",
      "314 [ 41 588 616]\n",
      "313 [ 40 587 615]\n",
      "313 [ 42 584 613]\n",
      "311 [ 41 581 610]\n",
      "311 [ 38 584 613]\n",
      "311 [ 39 583 613]\n",
      "309 [ 39 580 610]\n",
      "310 [ 40 581 611]\n",
      "310 [ 39 581 610]\n",
      "312 [ 37 587 602]\n",
      "324 [35]\n",
      "345 [11 40]\n",
      "367 [ 9 44]\n",
      "395 [52]\n",
      "403 [60]\n",
      "409 [66]\n",
      "408 [65]\n",
      "409 [66]\n",
      "413 [70]\n",
      "414 [71]\n",
      "420 [77]\n",
      "428 [85]\n",
      "433 [90]\n",
      "447 [ 95 104]\n",
      "464 [111 121]\n",
      "479 [123 136]\n",
      "499 [141 156]\n",
      "522 [159 179]\n",
      "543 [178 200]\n",
      "567 [196 224]\n",
      "594 [221 251]\n",
      "618 [241 258 275]\n",
      "632 [256 273 289]\n",
      "653 [274 293 310]\n",
      "634 [291 320]\n",
      "657 [290 314]\n",
      "633 [268 290]\n",
      "598 [236 255]\n",
      "424 [195 213 635]\n",
      "395 [149 164 626]\n",
      "362 [100 110 614 632]\n",
      "327 [ 55 600 622]\n",
      "300 [ 17 583 612]\n",
      "265 [562 602]\n",
      "219 [533 585]\n",
      "169 [501 566]\n",
      "127 [480 490 553]\n",
      "124 [477 482 546]\n",
      "139 [492 552]\n",
      "157 [510 564]\n",
      "265 [  1 530 576]\n",
      "286 [ 17 556 594]\n",
      "306 [ 33 580 611]\n",
      "326 [ 50 602 627]\n",
      "337 [ 61 614]\n",
      "345 [ 74 617]\n",
      "349 [ 82 616]\n",
      "348 [ 87 610]\n",
      "328 [603]\n",
      "300 [590 633]\n",
      "271 [578 631]\n",
      "232 [558 629]\n",
      "213 [539 634]\n",
      "183 [509 517]\n",
      "149 [475 489]\n",
      "121 [447 466 617]\n",
      "90 [416 439 571]\n",
      "70 [396 423 536 544]\n",
      "45 [371 401 504 515]\n",
      "36 [362 387 484 494]\n",
      "29 [355 378 476 482]\n",
      "33 [359 380 478 484]\n",
      "49 [375 392 490]\n",
      "75 [401 418 505]\n",
      "107 [433 450 524]\n",
      "149 [475 486 550]\n",
      "197 [523 582]\n",
      "289 [ 11 568 607]\n",
      "315 [ 27 604 632]\n",
      "342 [ 52 633]\n",
      "396 [81 91]\n",
      "466 [123 144]\n",
      "526 [160 185]\n",
      "567 [182 204]\n",
      "564 [182 201]\n",
      "411 [ 30 169 184 638]\n",
      "393 [146 157 630]\n",
      "371 [118 126 617 636]\n",
      "350 [ 93 608 629]\n",
      "326 [ 60 592 619]\n",
      "305 [ 32 579 611]\n",
      "289 [ 14 565 604]\n",
      "280 [ 11 549 596]\n",
      "274 [ 10 539 585]\n",
      "289 [11]\n",
      "316 [23]\n",
      "341 [30]\n",
      "377 [47]\n",
      "393 [63]\n",
      "410 [80]\n",
      "431 [ 94 101]\n",
      "457 [110 127]\n",
      "476 [125 146]\n",
      "503 [150 173]\n",
      "539 [172 191 209]\n",
      "573 [207 225 243]\n",
      "605 [231 253 275]\n",
      "637 [265 286 307]\n",
      "327 [294 316 338]\n",
      "310 [321 342 362]\n",
      "331 [343 364 383]\n",
      "357 [369 390 408]\n",
      "418 [431 455]\n",
      "478 [491 505 525]\n",
      "523 [536 554 574]\n",
      "539 [552 568 585]\n",
      "537 [550 575]\n",
      "291 [ 37 546 564]\n",
      "275 [542 555]\n",
      "252 [534 542]\n",
      "227 [526]\n",
      "220 [538]\n",
      "254 [572]\n",
      "293 [611]\n",
      "no line\n",
      "293 []\n",
      "no line\n",
      "293 []\n",
      "no line\n",
      "293 []\n",
      "no line\n",
      "293 []\n",
      "no line\n",
      "293 []\n",
      "no line\n",
      "293 []\n",
      "no line\n",
      "293 []\n",
      "no line\n",
      "293 []\n",
      "no line\n",
      "293 []\n",
      "no line\n",
      "293 []\n",
      "no line\n",
      "293 []\n",
      "no line\n",
      "293 []\n",
      "no line\n",
      "293 []\n",
      "no line\n",
      "293 []\n"
     ]
    }
   ],
   "source": [
    "# Run a track detection algorithm on a single horizontal line.\n",
    "# Uses YUV420 image format as the Y component corresponds to image intensity (gray image)\n",
    "# and thus there is no need to convert from RGB to BW\n",
    "\n",
    "RUN_TIMER = 17 # seconds\n",
    "history=[]\n",
    "\n",
    "camera = PiCamera()\n",
    "        \n",
    "# Check the link below for the combinations between mode and resolution\n",
    "# https://picamera.readthedocs.io/en/release-1.13/fov.html#sensor-modes\n",
    "camera.sensor_mode = 7\n",
    "camera.resolution = res\n",
    "camera.framerate = 10\n",
    "\n",
    "# Initialize the buffer and start capturing\n",
    "rawCapture = PiYUVArray(camera, size=res)\n",
    "stream = camera.capture_continuous(rawCapture, format=\"yuv\", use_video_port=True)\n",
    "\n",
    "# Measure the time needed to process 300 images to estimate the FPS\n",
    "t = time.time()\n",
    "\n",
    "# To filter the noise in the image we use a 3rd order Butterworth filter\n",
    "\n",
    "# Wn = 0.02, the cut-off frequency, acceptable values are from 0 to 1\n",
    "b, a = butter(3, 0.1)\n",
    "\n",
    "line_pos    = CAMERA_CENTER\n",
    "first_frame = True\n",
    "\n",
    "# start car\n",
    "motor.duty_cycle = MOTOR_BRAKE + 120000\n",
    "track_width = 0\n",
    "MOTOR_MAX = 310000\n",
    "MOTOR_STRAIGHT = MOTOR_MAX\n",
    "SPEED_COMFY = 240000\n",
    "MOTOR_TURN_INIT = 150000\n",
    "MOTOR_TURN = MOTOR_TURN_INIT\n",
    "TURN_COMFY = 250000\n",
    "turn_ratio_start = 1.9\n",
    "turn_ratio_end = 1.3\n",
    "inc = 0.1\n",
    "turn_ratio = turn_ratio_start\n",
    "for f in stream:\n",
    "    if first_frame:\n",
    "        first_frame = False\n",
    "        # Reset the buffer for the next image\n",
    "        rawCapture.truncate(0)\n",
    "        continue\n",
    "    \n",
    "    # Stop after RUN_TIMER seconds\n",
    "    if (time.time() - t) > RUN_TIMER:\n",
    "        break\n",
    "        \n",
    "    # Get the intensity component of the image (a trick to get black and white images)\n",
    "    I = f.array[:, :, 0]\n",
    "    \n",
    "    # Reset the buffer for the next image\n",
    "    rawCapture.truncate(0)\n",
    "    \n",
    "    # Select a horizontal line in the middle of the image\n",
    "    L = I[195, :]\n",
    "\n",
    "    # Smooth the transitions so we can detect the peaks \n",
    "    Lf = filtfilt(b, a, L)\n",
    "    history.append(Lf)\n",
    "    \n",
    "    # Find peaks which are higher than 0.5\n",
    "    p = find_peaks(Lf, height=160)\n",
    "      \n",
    "    peaks = p[0]\n",
    "    \n",
    "    \n",
    "    line_left   = None\n",
    "    line_right  = None\n",
    "    peaks_left  = peaks[peaks < CAMERA_CENTER]\n",
    "    peaks_right = peaks[peaks > CAMERA_CENTER]\n",
    "    \n",
    "    # state machine\n",
    "    \n",
    "    if peaks_left.size:\n",
    "        line_left = peaks_left.max()\n",
    "\n",
    "    if peaks_right.size:\n",
    "        line_right = peaks_right.min()\n",
    "        \n",
    "    if line_left and line_right:\n",
    "        line_pos    = (line_left + line_right ) // 2\n",
    "        track_width = line_right - line_left\n",
    "        motor.duty_cycle = MOTOR_BRAKE + MOTOR_STRAIGHT\n",
    "        if(MOTOR_STRAIGHT >= SPEED_COMFY):\n",
    "            MOTOR_STRAIGHT -= 10000\n",
    "        MOTOR_TURN = MOTOR_TURN_INIT\n",
    "        turn_ratio = turn_ratio_start\n",
    "    elif line_left and not line_right:\n",
    "#         track_width = 400\n",
    "        line_pos    = line_left + int(track_width/turn_ratio)\n",
    "#         line_pos    = line_left + int(track_width / 2)\n",
    "        motor.duty_cycle = MOTOR_BRAKE + MOTOR_TURN\n",
    "        MOTOR_STRAIGHT = MOTOR_MAX\n",
    "        if(MOTOR_TURN <= TURN_COMFY):\n",
    "            MOTOR_TURN += 10000\n",
    "        if(turn_ratio >= turn_ratio_end):\n",
    "            turn_ratio -= inc\n",
    "    elif not line_left and line_right:\n",
    "#         track_width = 400\n",
    "        line_pos    = line_right - int(track_width/turn_ratio)\n",
    "#         line_pos    = line_right - int(track_width / 2)\n",
    "        motor.duty_cycle = MOTOR_BRAKE + MOTOR_TURN\n",
    "        MOTOR_STRAIGHT = MOTOR_MAX\n",
    "        if(MOTOR_TURN <= TURN_COMFY):\n",
    "            MOTOR_TURN += 10000\n",
    "        if(turn_ratio >= turn_ratio_end):\n",
    "            turn_ratio -= inc\n",
    "    else:\n",
    "        track_width = 400\n",
    "        print(\"no line\")\n",
    "        turn_ratio = turn_ratio_start\n",
    "        \n",
    "    print(line_pos, peaks)\n",
    "\n",
    "        \n",
    "    DUTY_CYCLE = SERVO_MIDDLE + 5000 * (CAMERA_CENTER - line_pos)\n",
    "    if DUTY_CYCLE > 2000000:\n",
    "        DUTY_CYCLE = 2000000\n",
    "    if DUTY_CYCLE < 1000000:\n",
    "        DUTY_CYCLE = 1000000\n",
    "        \n",
    "    servo.duty_cycle =  DUTY_CYCLE\n",
    "        \n",
    "#Initialize lines position\n",
    "#Check which lines are closer them in the next frame\n",
    "\n",
    "        \n",
    "        #print(line_pos)\n",
    "        \n",
    "motor.duty_cycle = MOTOR_BRAKE    \n",
    "    \n",
    "\n",
    "# Release resources\n",
    "stream.close()\n",
    "rawCapture.close()\n",
    "camera.close()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Release resources\n",
    "stream.close()\n",
    "rawCapture.close()\n",
    "camera.close()\n",
    "motor.duty_cycle = MOTOR_BRAKE   "
   ]
  }
 ],
 "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.7.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}