User:Kannix/RPI-Cam

From OpenStreetMap Wiki
Jump to navigation Jump to search

intro:

This setup shall take photos at different angles on slow moving vehicles (eg. canal-boats) to be shared at mapillary:

  • motorised panning camera head
  • GPS EXIF tags

parts:

  • Raspberry PI EUR 35,-
  • Camera Board EUR 25,-
  • ULN2003 stepper motor and driver board Arduino EUR 4,50
  • Dupont Jumper Cables
  • Krick RABOESCH Ruderhebel T-Form Nylon 5mm Durchmesser - rb107-41 EUR 4,-
  • GPS-device
  • push-button switch
  • scrap metal, tiny screws, double sided tape etc ...

some prerequisites:

apt-get update
apt-get upgrade
apt-get install raspi-config
apt-get install python
apt-get install rpi.gpio
apt-get install python-picamera
apt-get install gpsd gpsd-clients
apt-get install python-dateutil

code:

Note: Pins are numbered according BCM-scheme!

#!/usr/bin/env python
# -*- coding: utf-8 -*-

from time import sleep
import RPi.GPIO as GPIO
import picamera
import gps
import math
from datetime import datetime
from dateutil import parser

# Class Motor is written by Stephen C Phillips.
# It is in the public domain, so you can do what you like with it
# but a link to http://scphillips.com would be nice.
# It works on the Raspberry Pi computer with the standard Debian Wheezy OS and
# the 28BJY-48 stepper motor with ULN2003 control board.

class Motor(object):
    def __init__(self, pins, mode=3):
        """Initialise the motor object.

        pins -- a list of 4 integers referring to the GPIO pins that the IN1, IN2
                IN3 and IN4 pins of the ULN2003 board are wired to
        mode -- the stepping mode to use:
                1: wave drive (not yet implemented)
                2: full step drive
                3: half step drive (default)

        """
        self.P1 = pins[0]
        self.P2 = pins[1]
        self.P3 = pins[2]
        self.P4 = pins[3]
        self.mode = mode
        self.deg_per_step = 5.625 / 64  # for half-step drive (mode 3)
        self.steps_per_rev = int(360 / self.deg_per_step)  # 4096
        self.step_angle = 0  # Assume the way it is pointing is zero degrees
        for p in pins:
            GPIO.setup(p, GPIO.OUT)
            GPIO.output(p, 0)

    def _set_rpm(self, rpm):
        """Set the turn speed in RPM."""
        self._rpm = rpm
        # T is the amount of time to stop between signals
        self._T = (60.0 / rpm) / self.steps_per_rev

    # This means you can set "rpm" as if it is an attribute and
    # behind the scenes it sets the _T attribute
    rpm = property(lambda self: self._rpm, _set_rpm)

    def move_to(self, angle):
        """Take the shortest route to a particular angle (degrees)."""
        # Make sure there is a 1:1 mapping between angle and stepper angle
        target_step_angle = 8 * (int(angle / self.deg_per_step) / 8)
        steps = target_step_angle - self.step_angle
        steps = (steps % self.steps_per_rev)
        if steps > self.steps_per_rev / 2:
            steps -= self.steps_per_rev
            print "moving " + `steps` + " steps"
            print "moving " + `(steps * self.deg_per_step)` + " degree"
            if self.mode == 2:
                self._move_acw_2(-steps / 8)
            else:
                self._move_acw_3(-steps / 8)
        else:
            print "moving " + `steps` + " steps"
            print "moving " + `(steps * self.deg_per_step)` + " degree"
            if self.mode == 2:
                self._move_cw_2(steps / 8)
            else:
                self._move_cw_3(steps / 8)
        self.step_angle = target_step_angle

    def __clear(self):
        GPIO.output(self.P1, 0)
        GPIO.output(self.P2, 0)
        GPIO.output(self.P3, 0)
        GPIO.output(self.P4, 0)

    def _move_acw_2(self, big_steps):
        self.__clear()
        for i in range(big_steps):
            GPIO.output(self.P3, 0)
            GPIO.output(self.P1, 1)
            sleep(self._T * 2)
            GPIO.output(self.P2, 0)
            GPIO.output(self.P4, 1)
            sleep(self._T * 2)
            GPIO.output(self.P1, 0)
            GPIO.output(self.P3, 1)
            sleep(self._T * 2)
            GPIO.output(self.P4, 0)
            GPIO.output(self.P2, 1)
            sleep(self._T * 2)

    def _move_cw_2(self, big_steps):
        self.__clear()
        for i in range(big_steps):
            GPIO.output(self.P4, 0)
            GPIO.output(self.P2, 1)
            sleep(self._T * 2)
            GPIO.output(self.P1, 0)
            GPIO.output(self.P3, 1)
            sleep(self._T * 2)
            GPIO.output(self.P2, 0)
            GPIO.output(self.P4, 1)
            sleep(self._T * 2)
            GPIO.output(self.P3, 0)
            GPIO.output(self.P1, 1)
            sleep(self._T * 2)

    def _move_acw_3(self, big_steps):
        self.__clear()
        for i in range(big_steps):
            GPIO.output(self.P1, 0)
            sleep(self._T)
            GPIO.output(self.P3, 1)
            sleep(self._T)
            GPIO.output(self.P4, 0)
            sleep(self._T)
            GPIO.output(self.P2, 1)
            sleep(self._T)
            GPIO.output(self.P3, 0)
            sleep(self._T)
            GPIO.output(self.P1, 1)
            sleep(self._T)
            GPIO.output(self.P2, 0)
            sleep(self._T)
            GPIO.output(self.P4, 1)
            sleep(self._T)

    def _move_cw_3(self, big_steps):
        self.__clear()
        for i in range(big_steps):
            GPIO.output(self.P3, 0)
            sleep(self._T)
            GPIO.output(self.P1, 1)
            sleep(self._T)
            GPIO.output(self.P4, 0)
            sleep(self._T)
            GPIO.output(self.P2, 1)
            sleep(self._T)
            GPIO.output(self.P1, 0)
            sleep(self._T)
            GPIO.output(self.P3, 1)
            sleep(self._T)
            GPIO.output(self.P2, 0)
            sleep(self._T)
            GPIO.output(self.P4, 1)
            sleep(self._T)

if __name__ == "__main__":

  # http://wiki.openstreetmap.org/wiki/User:MHohmann/RPiCam
  def gpsexif(redirect):
    global camera, gpsd
    while True:
      report = gpsd.next()
      # print report
      # Wait for position information.
      if report['class'] == 'TPV':
        # Set orientation to normal landscape.
        camera.exif_tags['IFD0.Orientation'] = '1'

        # Set picture date and time to GPS values.
        now = parser.parse(report.get('time', datetime.now().isoformat()))
        print "date and time: " + `now.strftime('%Y-%m-%d %H:%M:%S')`
        camera.exif_tags['EXIF.DateTimeOriginal'] = now.strftime('%Y:%m:%d %H:%M:%S')

        # Set altitude to GPS value.
        alt = report.get('alt', 0.0)
        print "altitude: " + `alt`
        camera.exif_tags['GPS.GPSAltitudeRef'] = '0' if alt > 0 else '1'
        aalt = math.fabs(alt)
        camera.exif_tags['GPS.GPSAltitude'] = '%d/100' % int(100 * aalt)

        # Convert speed from m/s to km/h and set tag.
        speed = report.get('speed', 0.0)
        print "speed (m/s): " + `speed`
        camera.exif_tags['GPS.GPSSpeedRef'] = 'K'
        camera.exif_tags['GPS.GPSSpeed'] = '%d/1000' % int(3600 * speed)

        # Set direction of motion and direction along which the picture is taken (assuming frontal view).
        track = report.get('track', 0.0)
        print "gpsd track " + `track` + " degrees"
        print "redirect " + `redirect` + " degrees"
        track = track + redirect
        if track > 360:
          track = track - 360
        elif track < 0:
          track = track + 360
        print "real track " + `track` + " degrees"
        camera.exif_tags['GPS.GPSTrackRef'] = 'T'
        camera.exif_tags['GPS.GPSTrack'] = '%d/10' % int(10 * track)
        camera.exif_tags['GPS.GPSImgDirectionRef'] = 'T'
        camera.exif_tags['GPS.GPSImgDirection'] = '%d/10' % int(10 * track)

        # Set GPS latitude.
        lat = report.get('lat', 0.0)
        print "latitude: " + `lat`
        camera.exif_tags['GPS.GPSLatitudeRef'] = 'N' if lat > 0 else 'S'
        alat = math.fabs(lat)
        dlat = int(alat)
        mlat = int(60 * (alat - dlat))
        slat = int(6000 * (60 * (alat - dlat) - mlat))
        camera.exif_tags['GPS.GPSLatitude'] = '%d/1,%d/1,%d/100' % (dlat, mlat, slat)

        # Set GPS longitude.
        lon = report.get('lon', 0.0)
        print "longitude: " + `lon`
        camera.exif_tags['GPS.GPSLongitudeRef'] = 'E' if lon > 0 else 'W'
        alon = math.fabs(lon)
        dlon = int(alon)
        mlon = int(60 * (alon - dlon))
        slon = int(6000 * (60 * (alon - dlon) - mlon))
        camera.exif_tags['GPS.GPSLongitude'] = '%d/1,%d/1,%d/100' % (dlon, mlon, slon)

        # Provide image file name.
        filename = now.strftime('%s') + '.jpg'
        print "filename: " + `filename`
        return filename

  def move_and_shoot(directon):
    m.move_to(directon)
    camera.capture(gpsexif(directon))
    sleep(1)

  def Start():
      global started #use global variable
      if not started:
        started = True
        StartLoop()

  def Stop():
      global started #use global variable
      if started:
        started = False

  def StartLoop():
      global started #use global variable
      while started:
          move_and_shoot(-45)
          move_and_shoot(-90)
          move_and_shoot(-135)
          move_and_shoot(45)
          move_and_shoot(90)
          move_and_shoot(135)
          move_and_shoot(0)
          if GPIO.event_detected(12):
            print "Button pressed, stopping ..."
            Stop()

  # Connect to gpsd.
  gpsd = gps.gps(mode=gps.WATCH_ENABLE)

  # Initialize camera
  camera = picamera.PiCamera()
  camera.vflip = True

  GPIO.setmode(GPIO.BCM)
  # set GPIO pins
  m = Motor([24,25,8,7])
  m.rpm = 10
  print "Motor setup: Pause in seconds: " + `m._T`

  GPIO.setup(12, GPIO.IN, pull_up_down=GPIO.PUD_UP)
  started = False

  try:
    GPIO.add_event_detect(12, GPIO.BOTH, bouncetime=200)
    while True:
      print "waiting for button press ..."
      if (GPIO.event_detected(12) and not started):
        print "Button pressed, starting ...."
        Start()
      sleep(2)
  except KeyboardInterrupt:
    print "CTRL+C entered, exiting ..."
  finally:
    camera.close()
    GPIO.cleanup()