race1-statemachine.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 79,
   "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": 80,
   "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": 81,
   "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": 82,
   "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": 83,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "317 [ 33  68 567]\n",
      "317 [ 32  67 568]\n",
      "317 [ 33  67 568]\n",
      "315 [ 34  68 563]\n",
      "315 [ 32  66 564]\n",
      "314 [ 33  66 563]\n",
      "313 [ 31  64 563]\n",
      "314 [ 33  67 562]\n",
      "312 [ 31  65 560]\n",
      "310 [ 29  63 558]\n",
      "311 [ 29  64 558]\n",
      "309 [ 30  64 554]\n",
      "310 [ 30  63 557]\n",
      "309 [ 29  63 556]\n",
      "309 [ 28  62 557]\n",
      "309 [ 30  63 556]\n",
      "309 [ 28  61 558]\n",
      "310 [ 30  64 557]\n",
      "311 [ 33  66 557]\n",
      "310 [ 31  65 555]\n",
      "311 [ 33  68 554]\n",
      "311 [ 33  68 554]\n",
      "310 [ 31  67 554]\n",
      "310 [ 29  65 556]\n",
      "312 [ 66 559]\n",
      "312 [ 67 558]\n",
      "312 [ 66 558]\n",
      "314 [ 66 562]\n",
      "312 [ 28  65 560]\n",
      "313 [ 31  66 560]\n",
      "311 [ 28  64 559]\n",
      "311 [ 29  64 559]\n",
      "304 [ 47 561]\n",
      "300 [557]\n",
      "290 [547]\n",
      "288 [545]\n",
      "281 [538]\n",
      "268 [525]\n",
      "253 [510]\n",
      "243 [500]\n",
      "222 [479 486]\n",
      "203 [460 470]\n",
      "185 [442 455]\n",
      "175 [432 445]\n",
      "167 [424 441]\n",
      "158 [415 433]\n",
      "152 [409 425]\n",
      "149 [406 423]\n",
      "137 [394 415]\n",
      "135 [392 413]\n",
      "122 [379 401]\n",
      "113 [370 396]\n",
      "111 [368 391]\n",
      "105 [362 387]\n",
      "100 [357 382]\n",
      "88 [345 369]\n",
      "93 [350 374]\n",
      "86 [343 369 537 564 584]\n",
      "81 [338 361 544 561 584]\n",
      "78 [335 359 554]\n",
      "74 [331 356]\n",
      "72 [329 355]\n",
      "69 [326 350]\n",
      "69 [326 348]\n",
      "66 [323 346 617]\n",
      "64 [321 343 615]\n",
      "65 [322 344]\n",
      "67 [324 346]\n",
      "66 [323 346]\n",
      "70 [327 349]\n",
      "71 [328 348]\n",
      "75 [332 353]\n",
      "75 [332 353]\n",
      "76 [333 353]\n",
      "78 [335 357]\n",
      "84 [341 361]\n",
      "94 [351 367]\n",
      "102 [359 374]\n",
      "114 [371 386]\n",
      "128 [385 398]\n",
      "146 [403 414]\n",
      "163 [420 429]\n",
      "223 [  8 439]\n",
      "235 [ 13 458]\n",
      "249 [ 20 479]\n",
      "264 [ 29 500 606 631]\n",
      "280 [ 39 521 601 611]\n",
      "292 [ 47 538]\n",
      "308 [ 59 557]\n",
      "320 [ 72 569]\n",
      "330 [ 39  80 580]\n",
      "333 [ 42  85 581]\n",
      "336 [ 45  89 583]\n",
      "337 [ 46  91 583]\n",
      "336 [ 46  92 581]\n",
      "335 [ 47  92 579]\n",
      "329 [ 42  86 573]\n",
      "325 [ 40  82 569]\n",
      "324 [ 38  78 570]\n",
      "319 [ 34  72 566]\n",
      "315 [ 30  66 565]\n",
      "312 [ 28  62 562]\n",
      "311 [ 30  62 561]\n",
      "309 [ 29  62 557]\n",
      "311 [ 31  65 558]\n",
      "318 [ 27  63 573]\n",
      "323 [29 68]\n",
      "328 [27 73]\n",
      "335 [25 80]\n",
      "350 [26 95]\n",
      "359 [ 21 104]\n",
      "374 [ 17 119]\n",
      "389 [ 10 134]\n",
      "399 [144]\n",
      "408 [147 153]\n",
      "413 [ 10 148 158]\n",
      "420 [ 29 152 165]\n",
      "422 [ 47 155 167]\n",
      "428 [ 65 159 173]\n",
      "429 [ 70 160 174]\n",
      "436 [ 86 168 181]\n",
      "439 [ 93 171 184]\n",
      "433 [ 99 167 178]\n",
      "402 [ 95 162 173 632]\n",
      "394 [ 93 156 164 624]\n",
      "385 [ 93 154 617]\n",
      "376 [ 87 144 609]\n",
      "366 [ 81 132 601]\n",
      "352 [ 67 113 591]\n",
      "340 [ 55  96 585]\n",
      "329 [ 46  84 575]\n",
      "320 [ 39  73 568]\n",
      "312 [ 34  66 558]\n",
      "306 [ 27  59 554]\n",
      "302 [ 25  55 550]\n",
      "302 [ 20  51 554]\n",
      "302 [ 22  52 553]\n",
      "301 [ 21  50 553]\n",
      "299 [ 20  49 550]\n",
      "300 [ 22  52 549]\n",
      "299 [ 22  51 548]\n",
      "304 [ 27  56 553]\n",
      "308 [ 31  61 555]\n",
      "308 [ 32  62 554]\n",
      "309 [ 33  63 556]\n",
      "311 [ 33  64 558]\n",
      "312 [ 36  65 560]\n",
      "315 [562]\n",
      "311 [558 623]\n",
      "308 [555 626]\n",
      "297 [544 623]\n",
      "284 [531 624]\n",
      "265 [512 621]\n",
      "248 [495]\n",
      "230 [477]\n",
      "216 [463]\n",
      "200 [447 457]\n",
      "186 [433 447]\n",
      "175 [422 438]\n",
      "167 [414 428]\n",
      "160 [407 423]\n",
      "155 [402 418]\n",
      "151 [398 414]\n",
      "137 [384 409]\n",
      "134 [381 400]\n",
      "127 [374 396]\n",
      "124 [371 392]\n",
      "118 [365 389]\n",
      "117 [364 389]\n",
      "111 [358 381]\n",
      "109 [356 379]\n",
      "102 [349 372]\n",
      "101 [348 371]\n",
      "97 [344 366]\n",
      "97 [344 366]\n",
      "98 [345 366]\n",
      "98 [345 367]\n",
      "97 [344 365]\n",
      "95 [342 364]\n",
      "95 [342 363]\n",
      "96 [343 365]\n",
      "95 [342 363]\n",
      "94 [341 362]\n",
      "94 [341 363]\n",
      "96 [343 364]\n",
      "93 [340 360]\n",
      "97 [344 365]\n",
      "100 [347 366]\n",
      "102 [349 370]\n",
      "103 [350 370 451]\n",
      "105 [352 370 439 478 504]\n",
      "109 [356 373 456 498 527 541]\n",
      "114 [361 379 500 524 541 559]\n",
      "113 [360 381 552]\n",
      "120 [367 382]\n",
      "129 [376 390]\n",
      "138 [385 396]\n",
      "151 [398 407]\n",
      "167 [414 421]\n",
      "185 [432]\n",
      "226 [  4 448]\n",
      "232 [  2 463]\n",
      "247 [ 15 479]\n",
      "262 [  2  24 500]\n",
      "276 [  9  34 519]\n",
      "291 [ 17  45 538]\n",
      "307 [ 27  58 556]\n",
      "317 [ 35  69 566]\n",
      "327 [ 41  79 575]\n",
      "334 [ 46  86 582]\n",
      "337 [ 48  89 585]\n",
      "339 [ 50  91 587]\n",
      "338 [ 52  92 585]\n",
      "336 [ 51  90 582]\n",
      "333 [ 51  88 579]\n",
      "329 [ 47  82 576]\n",
      "325 [ 43  78 572]\n",
      "320 [ 38  72 568]\n",
      "316 [ 38  70 562]\n",
      "311 [ 33  65 558]\n",
      "309 [ 31  62 557]\n",
      "308 [ 30  61 555]\n",
      "309 [ 32  64 555]\n",
      "307 [ 29  59 555]\n",
      "308 [ 27  61 556]\n",
      "309 [ 24  62 557]\n",
      "309 [ 17  62 556]\n",
      "308 [  9  60 556 582 605]\n",
      "309 [  1  60 559 584 607]\n",
      "310 [ 60 560]\n",
      "311 [ 60 562]\n",
      "310 [ 63 557]\n",
      "311 [ 65 558]\n",
      "310 [ 64 557]\n",
      "311 [ 65 557]\n",
      "300 [ 42 559]\n",
      "300 [558]\n",
      "294 [552]\n",
      "286 [544]\n",
      "274 [532]\n",
      "264 [522]\n",
      "254 [512]\n",
      "243 [501]\n",
      "233 [491]\n",
      "221 [479]\n",
      "208 [466 471]\n",
      "192 [450 460]\n",
      "181 [439 452]\n",
      "175 [433 447]\n",
      "164 [422 437]\n",
      "160 [418 432]\n",
      "159 [417 432]\n",
      "162 [420 433]\n",
      "168 [426 439]\n",
      "177 [435 446]\n",
      "187 [445 452]\n",
      "198 [456]\n",
      "235 [  1 470]\n",
      "244 [  8 480]\n",
      "254 [ 15 493]\n",
      "266 [  3  24 509]\n",
      "278 [  7  34 522]\n",
      "287 [ 12  42 533]\n",
      "297 [ 17  49 546]\n",
      "305 [ 22  55 556]\n",
      "315 [ 31  65 566]\n",
      "324 [ 39  74 574]\n",
      "329 [ 40  81 577]\n",
      "330 [ 42  84 577]\n",
      "332 [ 45  87 577]\n",
      "333 [ 48  90 577]\n",
      "326 [ 40  81 571]\n",
      "326 [ 39  79 573]\n",
      "324 [ 40  78 570]\n",
      "320 [ 36  73 568]\n",
      "316 [ 33  68 565]\n",
      "314 [ 32  67 562]\n",
      "309 [ 28  63 556]\n",
      "309 [ 29  64 554]\n",
      "308 [ 30  65 552]\n",
      "308 [ 30  65 552]\n",
      "307 [ 27  61 554]\n",
      "310 [ 25  64 556]\n",
      "307 [ 62 552]\n",
      "309 [ 64 554]\n",
      "309 [ 63 556]\n",
      "310 [ 28  63 557]\n",
      "308 [ 29  63 554]\n",
      "308 [ 28  62 555]\n",
      "309 [ 27  62 556]\n",
      "305 [ 41  52 559]\n",
      "302 [555]\n",
      "296 [549]\n",
      "294 [547]\n",
      "283 [536]\n",
      "270 [523]\n",
      "258 [511]\n",
      "246 [499]\n",
      "228 [481]\n",
      "206 [459 471]\n",
      "190 [443 458]\n",
      "177 [430 445]\n",
      "167 [420 437]\n",
      "159 [412 431]\n",
      "152 [405 425]\n",
      "149 [402 418]\n",
      "139 [392 411]\n",
      "133 [386 408]\n",
      "132 [385 407]\n",
      "119 [372 393]\n",
      "112 [365 391]\n",
      "110 [363 386]\n",
      "105 [358 383]\n",
      "103 [356 379]\n",
      "92 [345 371]\n",
      "95 [348 371]\n",
      "89 [342 367 528 552 578]\n",
      "88 [341 365 539 582]\n",
      "86 [339 361 548 588]\n",
      "81 [334 357]\n",
      "79 [332 356]\n",
      "77 [330 355]\n",
      "74 [327 351]\n",
      "75 [328 349]\n",
      "75 [328 351]\n",
      "73 [326 349]\n",
      "72 [325 346]\n",
      "74 [327 348]\n",
      "77 [330 351]\n",
      "76 [329 350]\n",
      "82 [335 356]\n",
      "85 [338 357]\n",
      "87 [340 360]\n",
      "89 [342 361]\n",
      "91 [344 366]\n",
      "97 [350 367]\n",
      "108 [361 376]\n",
      "121 [374 387]\n",
      "135 [388 400]\n",
      "149 [402 414]\n",
      "167 [420 428]\n",
      "219 [  1 438 441]\n",
      "233 [ 11 456]\n",
      "247 [ 21 473]\n",
      "259 [ 27 492 638]\n",
      "278 [ 40 516 600 615]\n",
      "293 [ 48 538]\n",
      "307 [ 60 554]\n",
      "316 [ 66 566]\n",
      "325 [ 37  77 574]\n",
      "331 [ 42  84 578]\n",
      "332 [ 43  87 578]\n",
      "336 [ 46  90 582]\n",
      "334 [ 44  89 579]\n",
      "332 [ 43  87 578]\n",
      "331 [ 44  88 575]\n",
      "331 [ 43  84 578]\n",
      "326 [ 40  81 571]\n",
      "321 [ 36  75 568]\n",
      "317 [ 34  70 565]\n",
      "314 [ 30  64 565]\n",
      "311 [ 28  61 562]\n",
      "310 [ 29  60 560]\n",
      "308 [ 30  62 555]\n",
      "311 [ 29  63 559 614 631]\n",
      "312 [27 64]\n",
      "317 [27 69]\n",
      "324 [28 76]\n",
      "334 [26 86]\n",
      "348 [ 26 100]\n",
      "365 [ 25 117]\n",
      "384 [ 20 136]\n",
      "402 [ 15 148 154]\n",
      "414 [  5 155 166]\n",
      "419 [160 171]\n",
      "426 [ 14 165 178]\n",
      "431 [ 30 167 183]\n",
      "436 [ 52 173 188]\n",
      "437 [ 64 173 189]\n",
      "441 [ 79 177 193]\n",
      "446 [ 94 182 198]\n",
      "447 [ 97 181 199]\n",
      "441 [ 99 179 193]\n",
      "441 [108 179 193]\n",
      "410 [105 175 185 635]\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 = 25 # 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",
    "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 + 250000\n",
    "        \n",
    "    elif line_left and not line_right:\n",
    "#         track_width = 400\n",
    "        line_pos    = line_left + int(track_width / 2)\n",
    "        motor.duty_cycle = MOTOR_BRAKE + 150000\n",
    "        \n",
    "    elif not line_left and line_right:\n",
    "#         track_width = 400\n",
    "        line_pos    = line_right - int(track_width / 2)\n",
    "        motor.duty_cycle = MOTOR_BRAKE + 150000\n",
    "        \n",
    "    else:\n",
    "        track_width = 400\n",
    "        print(\"no line\")\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": 73,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Release resources\n",
    "stream.close()\n",
    "rawCapture.close()\n",
    "camera.close()\n",
    "motor.duty_cycle = MOTOR_BRAKE   "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "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.7.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}