race1-SENDIT.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": 2,
   "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": 3,
   "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": 4,
   "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": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "501.0 [ 40  62 540]\n",
      "334.0 [ 40  62 540]\n"
     ]
    },
    {
     "ename": "ValueError",
     "evalue": "Unknown format code 'd' for object of type 'float'",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mValueError\u001b[0m                                Traceback (most recent call last)",
      "\u001b[0;32m<ipython-input-5-50c1ab71febd>\u001b[0m in \u001b[0;36m<module>\u001b[0;34m\u001b[0m\n\u001b[1;32m    128\u001b[0m         \u001b[0mDUTY_CYCLE\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0;36m1000000\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m    129\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m--> 130\u001b[0;31m     \u001b[0mservo\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mduty_cycle\u001b[0m \u001b[0;34m=\u001b[0m  \u001b[0mDUTY_CYCLE\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m    131\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m    132\u001b[0m \u001b[0;31m#Initialize lines position\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;32m~/leroy_jenkins/workshops/pwm.py\u001b[0m in \u001b[0;36mduty_cycle\u001b[0;34m(self, value)\u001b[0m\n\u001b[1;32m    100\u001b[0m     \u001b[0;32mdef\u001b[0m \u001b[0mduty_cycle\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mself\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mvalue\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m    101\u001b[0m         \u001b[0;32mwith\u001b[0m \u001b[0mopen\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mpath\u001b[0m \u001b[0;34m+\u001b[0m \u001b[0;34m'/duty_cycle'\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;34m'w'\u001b[0m\u001b[0;34m)\u001b[0m \u001b[0;32mas\u001b[0m \u001b[0mf\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m--> 102\u001b[0;31m             \u001b[0mf\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mwrite\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m'{:d}'\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mformat\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mvalue\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m    103\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m    104\u001b[0m     \u001b[0;34m@\u001b[0m\u001b[0mproperty\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;31mValueError\u001b[0m: Unknown format code 'd' for object of type 'float'"
     ]
    }
   ],
   "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 = 2\n",
    "next_turn = 0\n",
    "line_pos = 200\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",
    "    if peaks_left.size:\n",
    "        line_left = peaks_left.max()\n",
    "\n",
    "    if peaks_right.size:\n",
    "        line_right = peaks_right.min()\n",
    "\n",
    "    # first turn (U turn)\n",
    "    if line_left and line_right:\n",
    "        if next_turn == 0:\n",
    "            line_pos    = (line_left + line_right ) // 1.2 # stay right\n",
    "            next_turn += 1\n",
    "        elif next_turn == 1:\n",
    "            line_pos    = (line_left + line_right ) // 1.8 # stay left\n",
    "            next_turn += 1\n",
    "        elif next_turn == 2:\n",
    "            line_pos    = (line_left + line_right ) // 1.2 # stay right\n",
    "            next_turn += 1\n",
    "        elif next_turn == 3:\n",
    "            line_pos    = (line_left + line_right ) // 1.2 # stay right\n",
    "            next_turn = 0;       \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",
    "    elif line_left and not line_right:\n",
    "        line_pos    = line_left + int(track_width/turn_ratio)\n",
    "        motor.duty_cycle = MOTOR_BRAKE + MOTOR_TURN\n",
    "        MOTOR_STRAIGHT = MOTOR_MAX\n",
    "        if(MOTOR_TURN <= TURN_COMFY):\n",
    "            MOTOR_TURN += 10000\n",
    "    elif not line_left and line_right:\n",
    "        line_pos    = line_right - int(track_width/turn_ratio)\n",
    "        motor.duty_cycle = MOTOR_BRAKE + MOTOR_TURN\n",
    "        MOTOR_STRAIGHT = MOTOR_MAX\n",
    "        if(MOTOR_TURN <= TURN_COMFY):\n",
    "            MOTOR_TURN += 10000\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 - round(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 =  round(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": 21,
   "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
}