change version

# Conflicts:
#	640imutest.py

MANIFEST

@@ -0,0 +1,15 @@
+# file GENERATED by distutils, do NOT edit
+setup.py
+darkwater_640\GPIO.py
+darkwater_640\I2C.py
+darkwater_640\PCA9685.py
+darkwater_640\Platform.py
+darkwater_640\SPI.py
+darkwater_640\__init__.py
+darkwater_640\darkwater_640.py
+darkwater_640\mpu9250.py
+darkwater_640\smbus.py
+examples\640imutest.py
+examples\640motortest.py
+examples\640servotest.py
+examples\640steppertest.py

MANIFEST.in

@@ -0,0 +1 @@
+recursive-include examples *

README.md

@@ -1,4 +1,4 @@
-640 Mk1 Python driver
+640 Python driver
 =======================
 
 Based on the [Adafruit Python Library for DC + Stepper Motor HAT](https://github.com/adafruit/Adafruit-Motor-HAT-Python-Library)

darkwater_640/GPIO.py

@@ -0,0 +1,426 @@
+# Copyright (c) 2014 Adafruit Industries
+# Author: Tony DiCola
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+
+import Platform as Platform
+
+
+OUT     = 0
+IN      = 1
+HIGH    = True
+LOW     = False
+
+RISING      = 1
+FALLING     = 2
+BOTH        = 3
+
+PUD_OFF  = 0
+PUD_DOWN = 1
+PUD_UP   = 2
+
+class BaseGPIO(object):
+    """Base class for implementing simple digital IO for a platform.
+    Implementors are expected to subclass from this and provide an implementation
+    of the setup, output, and input functions."""
+
+    def setup(self, pin, mode, pull_up_down=PUD_OFF):
+        """Set the input or output mode for a specified pin.  Mode should be
+        either OUT or IN."""
+        raise NotImplementedError
+
+    def output(self, pin, value):
+        """Set the specified pin the provided high/low value.  Value should be
+        either HIGH/LOW or a boolean (true = high)."""
+        raise NotImplementedError
+
+    def input(self, pin):
+        """Read the specified pin and return HIGH/true if the pin is pulled high,
+        or LOW/false if pulled low."""
+        raise NotImplementedError
+
+    def set_high(self, pin):
+        """Set the specified pin HIGH."""
+        self.output(pin, HIGH)
+
+    def set_low(self, pin):
+        """Set the specified pin LOW."""
+        self.output(pin, LOW)
+
+    def is_high(self, pin):
+        """Return true if the specified pin is pulled high."""
+        return self.input(pin) == HIGH
+
+    def is_low(self, pin):
+        """Return true if the specified pin is pulled low."""
+        return self.input(pin) == LOW
+
+
+# Basic implementation of multiple pin methods just loops through pins and
+# processes each one individually. This is not optimal, but derived classes can
+# provide a more optimal implementation that deals with groups of pins
+# simultaneously.
+# See MCP230xx or PCF8574 classes for examples of optimized implementations.
+
+    def output_pins(self, pins):
+        """Set multiple pins high or low at once.  Pins should be a dict of pin
+        name to pin value (HIGH/True for 1, LOW/False for 0).  All provided pins
+        will be set to the given values.
+        """
+        # General implementation just loops through pins and writes them out
+        # manually.  This is not optimized, but subclasses can choose to implement
+        # a more optimal batch output implementation.  See the MCP230xx class for
+        # example of optimized implementation.
+        for pin, value in iter(pins.items()):
+            self.output(pin, value)
+
+    def setup_pins(self, pins):
+        """Setup multiple pins as inputs or outputs at once.  Pins should be a
+        dict of pin name to pin type (IN or OUT).
+        """
+        # General implementation that can be optimized by derived classes.
+        for pin, value in iter(pins.items()):
+            self.setup(pin, value)
+
+    def input_pins(self, pins):
+        """Read multiple pins specified in the given list and return list of pin values
+        GPIO.HIGH/True if the pin is pulled high, or GPIO.LOW/False if pulled low.
+        """
+        # General implementation that can be optimized by derived classes.
+        return [self.input(pin) for pin in pins]
+
+
+    def add_event_detect(self, pin, edge):
+        """Enable edge detection events for a particular GPIO channel.  Pin 
+        should be type IN.  Edge must be RISING, FALLING or BOTH.
+        """
+        raise NotImplementedError
+   
+    def remove_event_detect(self, pin):
+        """Remove edge detection for a particular GPIO channel.  Pin should be
+        type IN.
+        """
+        raise NotImplementedError
+  
+    def add_event_callback(self, pin, callback):
+        """Add a callback for an event already defined using add_event_detect().
+        Pin should be type IN.
+        """
+        raise NotImplementedError
+
+    def event_detected(self, pin):
+        """Returns True if an edge has occured on a given GPIO.  You need to 
+        enable edge detection using add_event_detect() first.   Pin should be 
+        type IN.
+        """
+        raise NotImplementedError
+
+    def wait_for_edge(self, pin, edge):
+        """Wait for an edge.   Pin should be type IN.  Edge must be RISING, 
+        FALLING or BOTH."""
+        raise NotImplementedError
+
+    def cleanup(self, pin=None):
+        """Clean up GPIO event detection for specific pin, or all pins if none 
+        is specified.
+        """
+        raise NotImplementedError
+
+
+# helper functions useful to derived classes
+
+    def _validate_pin(self, pin):
+        # Raise an exception if pin is outside the range of allowed values.
+        if pin < 0 or pin >= self.NUM_GPIO:
+            raise ValueError('Invalid GPIO value, must be between 0 and {0}.'.format(self.NUM_GPIO))
+
+    def _bit2(self, src, bit, val):
+        bit = 1 << bit
+        return (src | bit) if val else (src & ~bit)
+
+
+class RPiGPIOAdapter(BaseGPIO):
+    """GPIO implementation for the Raspberry Pi using the RPi.GPIO library."""
+
+    def __init__(self, rpi_gpio, mode=None):
+        self.rpi_gpio = rpi_gpio
+        # Suppress warnings about GPIO in use.
+        rpi_gpio.setwarnings(False)
+        # Setup board pin mode.
+        if mode == rpi_gpio.BOARD or mode == rpi_gpio.BCM:
+            rpi_gpio.setmode(mode)
+        elif mode is not None:
+            raise ValueError('Unexpected value for mode.  Must be BOARD or BCM.')
+        else:
+            # Default to BCM numbering if not told otherwise.
+            rpi_gpio.setmode(rpi_gpio.BCM)
+        # Define mapping of Adafruit GPIO library constants to RPi.GPIO constants.
+        self._dir_mapping = { OUT:      rpi_gpio.OUT,
+                              IN:       rpi_gpio.IN }
+        self._pud_mapping = { PUD_OFF:  rpi_gpio.PUD_OFF,
+                              PUD_DOWN: rpi_gpio.PUD_DOWN,
+                              PUD_UP:   rpi_gpio.PUD_UP }
+        self._edge_mapping = { RISING:  rpi_gpio.RISING,
+                               FALLING: rpi_gpio.FALLING,
+                               BOTH:    rpi_gpio.BOTH }
+
+    def setup(self, pin, mode, pull_up_down=PUD_OFF):
+        """Set the input or output mode for a specified pin.  Mode should be
+        either OUTPUT or INPUT.
+        """
+        self.rpi_gpio.setup(pin, self._dir_mapping[mode],
+                            pull_up_down=self._pud_mapping[pull_up_down])
+
+    def output(self, pin, value):
+        """Set the specified pin the provided high/low value.  Value should be
+        either HIGH/LOW or a boolean (true = high).
+        """
+        self.rpi_gpio.output(pin, value)
+
+    def input(self, pin):
+        """Read the specified pin and return HIGH/true if the pin is pulled high,
+        or LOW/false if pulled low.
+        """
+        return self.rpi_gpio.input(pin)
+
+    def input_pins(self, pins):
+        """Read multiple pins specified in the given list and return list of pin values
+        GPIO.HIGH/True if the pin is pulled high, or GPIO.LOW/False if pulled low.
+        """
+        # maybe rpi has a mass read...  it would be more efficient to use it if it exists
+        return [self.rpi_gpio.input(pin) for pin in pins]
+
+    def add_event_detect(self, pin, edge, callback=None, bouncetime=-1):
+        """Enable edge detection events for a particular GPIO channel.  Pin 
+        should be type IN.  Edge must be RISING, FALLING or BOTH.  Callback is a
+        function for the event.  Bouncetime is switch bounce timeout in ms for
+        callback
+        """
+        kwargs = {}
+        if callback:
+            kwargs['callback']=callback
+        if bouncetime > 0:
+            kwargs['bouncetime']=bouncetime
+        self.rpi_gpio.add_event_detect(pin, self._edge_mapping[edge], **kwargs)
+
+    def remove_event_detect(self, pin):
+        """Remove edge detection for a particular GPIO channel.  Pin should be
+        type IN.
+        """
+        self.rpi_gpio.remove_event_detect(pin)
+
+    def add_event_callback(self, pin, callback):
+        """Add a callback for an event already defined using add_event_detect().
+        Pin should be type IN.
+        """
+        self.rpi_gpio.add_event_callback(pin, callback)
+
+    def event_detected(self, pin):
+        """Returns True if an edge has occured on a given GPIO.  You need to 
+        enable edge detection using add_event_detect() first.   Pin should be
+        type IN.
+        """
+        return self.rpi_gpio.event_detected(pin)
+
+    def wait_for_edge(self, pin, edge):
+        """Wait for an edge.   Pin should be type IN.  Edge must be RISING,
+        FALLING or BOTH.
+        """
+        self.rpi_gpio.wait_for_edge(pin, self._edge_mapping[edge])
+
+    def cleanup(self, pin=None):
+        """Clean up GPIO event detection for specific pin, or all pins if none 
+        is specified.
+        """
+        if pin is None:
+            self.rpi_gpio.cleanup()
+        else:
+            self.rpi_gpio.cleanup(pin)
+
+class AdafruitBBIOAdapter(BaseGPIO):
+    """GPIO implementation for the Beaglebone Black using the Adafruit_BBIO
+    library.
+    """
+
+    def __init__(self, bbio_gpio):
+        self.bbio_gpio = bbio_gpio
+        # Define mapping of Adafruit GPIO library constants to RPi.GPIO constants.
+        self._dir_mapping = { OUT:      bbio_gpio.OUT,
+                              IN:       bbio_gpio.IN }
+        self._pud_mapping = { PUD_OFF:  bbio_gpio.PUD_OFF,
+                              PUD_DOWN: bbio_gpio.PUD_DOWN,
+                              PUD_UP:   bbio_gpio.PUD_UP }
+        self._edge_mapping = { RISING:  bbio_gpio.RISING,
+                               FALLING: bbio_gpio.FALLING,
+                               BOTH:    bbio_gpio.BOTH }
+
+    def setup(self, pin, mode, pull_up_down=PUD_OFF):
+        """Set the input or output mode for a specified pin.  Mode should be
+        either OUTPUT or INPUT.
+        """
+        self.bbio_gpio.setup(pin, self._dir_mapping[mode],
+                             pull_up_down=self._pud_mapping[pull_up_down])
+
+    def output(self, pin, value):
+        """Set the specified pin the provided high/low value.  Value should be
+        either HIGH/LOW or a boolean (true = high).
+        """
+        self.bbio_gpio.output(pin, value)
+
+    def input(self, pin):
+        """Read the specified pin and return HIGH/true if the pin is pulled high,
+        or LOW/false if pulled low.
+        """
+        return self.bbio_gpio.input(pin)
+
+    def input_pins(self, pins):
+        """Read multiple pins specified in the given list and return list of pin values
+        GPIO.HIGH/True if the pin is pulled high, or GPIO.LOW/False if pulled low.
+        """
+        # maybe bbb has a mass read...  it would be more efficient to use it if it exists
+        return [self.bbio_gpio.input(pin) for pin in pins]
+
+    def add_event_detect(self, pin, edge, callback=None, bouncetime=-1):
+        """Enable edge detection events for a particular GPIO channel.  Pin 
+        should be type IN.  Edge must be RISING, FALLING or BOTH.  Callback is a
+        function for the event.  Bouncetime is switch bounce timeout in ms for 
+        callback
+        """
+        kwargs = {}
+        if callback:
+            kwargs['callback']=callback
+        if bouncetime > 0:
+            kwargs['bouncetime']=bouncetime
+        self.bbio_gpio.add_event_detect(pin, self._edge_mapping[edge], **kwargs)
+
+    def remove_event_detect(self, pin):
+        """Remove edge detection for a particular GPIO channel.  Pin should be
+        type IN.
+        """
+        self.bbio_gpio.remove_event_detect(pin)
+
+    def add_event_callback(self, pin, callback, bouncetime=-1):
+        """Add a callback for an event already defined using add_event_detect().
+        Pin should be type IN.  Bouncetime is switch bounce timeout in ms for 
+        callback
+        """
+        kwargs = {}
+        if bouncetime > 0:
+            kwargs['bouncetime']=bouncetime
+        self.bbio_gpio.add_event_callback(pin, callback, **kwargs)
+
+    def event_detected(self, pin):
+        """Returns True if an edge has occured on a given GPIO.  You need to 
+        enable edge detection using add_event_detect() first.   Pin should be 
+        type IN.
+        """
+        return self.bbio_gpio.event_detected(pin)
+
+    def wait_for_edge(self, pin, edge):
+        """Wait for an edge.   Pin should be type IN.  Edge must be RISING, 
+        FALLING or BOTH.
+        """
+        self.bbio_gpio.wait_for_edge(pin, self._edge_mapping[edge])
+
+    def cleanup(self, pin=None):
+        """Clean up GPIO event detection for specific pin, or all pins if none 
+        is specified.
+        """
+        if pin is None:
+            self.bbio_gpio.cleanup()
+        else:
+            self.bbio_gpio.cleanup(pin)
+
+class AdafruitMinnowAdapter(BaseGPIO):
+    """GPIO implementation for the Minnowboard + MAX using the mraa library"""
+    
+    def __init__(self,mraa_gpio):
+        self.mraa_gpio = mraa_gpio
+        # Define mapping of Adafruit GPIO library constants to mraa constants
+        self._dir_mapping = { OUT:      self.mraa_gpio.DIR_OUT,
+                              IN:       self.mraa_gpio.DIR_IN }
+        self._pud_mapping = { PUD_OFF:  self.mraa_gpio.MODE_STRONG,
+                              PUD_UP:   self.mraa_gpio.MODE_HIZ,
+                              PUD_DOWN: self.mraa_gpio.MODE_PULLDOWN }
+        self._edge_mapping = { RISING:   self.mraa_gpio.EDGE_RISING,
+                              FALLING:  self.mraa_gpio.EDGE_FALLING,
+                              BOTH:     self.mraa_gpio.EDGE_BOTH }
+
+    def setup(self,pin,mode):
+        """Set the input or output mode for a specified pin.  Mode should be
+        either DIR_IN or DIR_OUT.
+        """
+        self.mraa_gpio.Gpio.dir(self.mraa_gpio.Gpio(pin),self._dir_mapping[mode])   
+
+    def output(self,pin,value):
+        """Set the specified pin the provided high/low value.  Value should be
+        either 1 (ON or HIGH), or 0 (OFF or LOW) or a boolean.
+        """
+        self.mraa_gpio.Gpio.write(self.mraa_gpio.Gpio(pin), value)
+    
+    def input(self,pin):
+        """Read the specified pin and return HIGH/true if the pin is pulled high,
+        or LOW/false if pulled low.
+        """
+        return self.mraa_gpio.Gpio.read(self.mraa_gpio.Gpio(pin))    
+    
+    def add_event_detect(self, pin, edge, callback=None, bouncetime=-1):
+        """Enable edge detection events for a particular GPIO channel.  Pin 
+        should be type IN.  Edge must be RISING, FALLING or BOTH.  Callback is a
+        function for the event.  Bouncetime is switch bounce timeout in ms for 
+        callback
+        """
+        kwargs = {}
+        if callback:
+            kwargs['callback']=callback
+        if bouncetime > 0:
+            kwargs['bouncetime']=bouncetime
+        self.mraa_gpio.Gpio.isr(self.mraa_gpio.Gpio(pin), self._edge_mapping[edge], **kwargs)
+
+    def remove_event_detect(self, pin):
+        """Remove edge detection for a particular GPIO channel.  Pin should be
+        type IN.
+        """
+        self.mraa_gpio.Gpio.isrExit(self.mraa_gpio.Gpio(pin))
+
+    def wait_for_edge(self, pin, edge):
+        """Wait for an edge.   Pin should be type IN.  Edge must be RISING, 
+        FALLING or BOTH.
+        """
+        self.bbio_gpio.wait_for_edge(self.mraa_gpio.Gpio(pin), self._edge_mapping[edge])
+
+def get_platform_gpio(**keywords):
+    """Attempt to return a GPIO instance for the platform which the code is being
+    executed on.  Currently supports only the Raspberry Pi using the RPi.GPIO
+    library and Beaglebone Black using the Adafruit_BBIO library.  Will throw an
+    exception if a GPIO instance can't be created for the current platform.  The
+    returned GPIO object is an instance of BaseGPIO.
+    """
+    plat = Platform.platform_detect()
+    if plat == Platform.RASPBERRY_PI:
+        import RPi.GPIO
+        return RPiGPIOAdapter(RPi.GPIO, **keywords)
+    elif plat == Platform.BEAGLEBONE_BLACK:
+        import Adafruit_BBIO.GPIO
+        return AdafruitBBIOAdapter(Adafruit_BBIO.GPIO, **keywords)
+    elif plat == Platform.MINNOWBOARD:
+        import mraa
+        return AdafruitMinnowAdapter(mraa, **keywords)
+    elif plat == Platform.UNKNOWN:
+        raise RuntimeError('Could not determine platform.')

darkwater_640/I2C.py

@@ -0,0 +1,200 @@
+# Copyright (c) 2014 Adafruit Industries
+# Author: Tony DiCola
+# Based on Adafruit_I2C.py created by Kevin Townsend.
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+import logging
+import subprocess
+
+import Platform as Platform
+
+
+def reverseByteOrder(data):
+    """Reverses the byte order of an int (16-bit) or long (32-bit) value."""
+    # Courtesy Vishal Sapre
+    byteCount = len(hex(data)[2:].replace('L','')[::2])
+    val       = 0
+    for i in range(byteCount):
+        val    = (val << 8) | (data & 0xff)
+        data >>= 8
+    return val
+
+def get_default_bus():
+    """Return the default bus number based on the device platform.  For a
+    Raspberry Pi either bus 0 or 1 (based on the Pi revision) will be returned.
+    For a Beaglebone Black the first user accessible bus, 1, will be returned.
+    """
+    plat = Platform.platform_detect()
+    if plat == Platform.RASPBERRY_PI:
+        if Platform.pi_revision() == 1:
+            # Revision 1 Pi uses I2C bus 0.
+            return 0
+        else:
+            # Revision 2 Pi uses I2C bus 1.
+            return 1
+    elif plat == Platform.BEAGLEBONE_BLACK:
+        # Beaglebone Black has multiple I2C buses, default to 1 (P9_19 and P9_20).
+        return 1
+    else:
+        raise RuntimeError('Could not determine default I2C bus for platform.')
+
+def get_i2c_device(address, busnum=None, i2c_interface=None, **kwargs):
+    """Return an I2C device for the specified address and on the specified bus.
+    If busnum isn't specified, the default I2C bus for the platform will attempt
+    to be detected.
+    """
+    if busnum is None:
+        busnum = get_default_bus()
+    return Device(address, busnum, i2c_interface, **kwargs)
+
+def require_repeated_start():
+    """Enable repeated start conditions for I2C register reads.  This is the
+    normal behavior for I2C, however on some platforms like the Raspberry Pi
+    there are bugs which disable repeated starts unless explicitly enabled with
+    this function.  See this thread for more details:
+      http://www.raspberrypi.org/forums/viewtopic.php?f=44&t=15840
+    """
+    plat = Platform.platform_detect()
+    if plat == Platform.RASPBERRY_PI:
+        # On the Raspberry Pi there is a bug where register reads don't send a
+        # repeated start condition like the kernel smbus I2C driver functions
+        # define.  As a workaround this bit in the BCM2708 driver sysfs tree can
+        # be changed to enable I2C repeated starts.
+        subprocess.check_call('chmod 666 /sys/module/i2c_bcm2708/parameters/combined', shell=True)
+        subprocess.check_call('echo -n 1 > /sys/module/i2c_bcm2708/parameters/combined', shell=True)
+    # Other platforms are a no-op because they (presumably) have the correct
+    # behavior and send repeated starts.
+
+
+class Device(object):
+    """Class for communicating with an I2C device using the adafruit-pureio pure
+    python smbus library, or other smbus compatible I2C interface. Allows reading
+    and writing 8-bit, 16-bit, and byte array values to registers
+    on the device."""
+    def __init__(self, address, busnum, i2c_interface=None):
+        """Create an instance of the I2C device at the specified address on the
+        specified I2C bus number."""
+        self._address = address
+        if i2c_interface is None:
+            # Use pure python I2C interface if none is specified.
+            import smbus
+            self._bus = smbus.SMBus(busnum)
+        else:
+            # Otherwise use the provided class to create an smbus interface.
+            self._bus = i2c_interface(busnum)
+        self._logger = logging.getLogger('Adafruit_I2C.Device.Bus.{0}.Address.{1:#0X}' \
+                                .format(busnum, address))
+
+    def writeRaw8(self, value):
+        """Write an 8-bit value on the bus (without register)."""
+        value = value & 0xFF
+        self._bus.write_byte(self._address, value)
+        self._logger.debug("Wrote 0x%02X",
+                     value)
+
+    def write8(self, register, value):
+        """Write an 8-bit value to the specified register."""
+        value = value & 0xFF
+        self._bus.write_byte_data(self._address, register, value)
+        self._logger.debug("Wrote 0x%02X to register 0x%02X",
+                     value, register)
+
+    def write16(self, register, value):
+        """Write a 16-bit value to the specified register."""
+        value = value & 0xFFFF
+        self._bus.write_word_data(self._address, register, value)
+        self._logger.debug("Wrote 0x%04X to register pair 0x%02X, 0x%02X",
+                     value, register, register+1)
+
+    def writeList(self, register, data):
+        """Write bytes to the specified register."""
+        self._bus.write_i2c_block_data(self._address, register, data)
+        self._logger.debug("Wrote to register 0x%02X: %s",
+                     register, data)
+
+    def readList(self, register, length):
+        """Read a length number of bytes from the specified register.  Results
+        will be returned as a bytearray."""
+        results = self._bus.read_i2c_block_data(self._address, register, length)
+        self._logger.debug("Read the following from register 0x%02X: %s",
+                     register, results)
+        return results
+
+    def readRaw8(self):
+        """Read an 8-bit value on the bus (without register)."""
+        result = self._bus.read_byte(self._address) & 0xFF
+        self._logger.debug("Read 0x%02X",
+                    result)
+        return result
+
+    def readU8(self, register):
+        """Read an unsigned byte from the specified register."""
+        result = self._bus.read_byte_data(self._address, register) & 0xFF
+        self._logger.debug("Read 0x%02X from register 0x%02X",
+                     result, register)
+        return result
+
+    def readS8(self, register):
+        """Read a signed byte from the specified register."""
+        result = self.readU8(register)
+        if result > 127:
+            result -= 256
+        return result
+
+    def readU16(self, register, little_endian=True):
+        """Read an unsigned 16-bit value from the specified register, with the
+        specified endianness (default little endian, or least significant byte
+        first)."""
+        result = self._bus.read_word_data(self._address,register) & 0xFFFF
+        self._logger.debug("Read 0x%04X from register pair 0x%02X, 0x%02X",
+                           result, register, register+1)
+        # Swap bytes if using big endian because read_word_data assumes little
+        # endian on ARM (little endian) systems.
+        if not little_endian:
+            result = ((result << 8) & 0xFF00) + (result >> 8)
+        return result
+
+    def readS16(self, register, little_endian=True):
+        """Read a signed 16-bit value from the specified register, with the
+        specified endianness (default little endian, or least significant byte
+        first)."""
+        result = self.readU16(register, little_endian)
+        if result > 32767:
+            result -= 65536
+        return result
+
+    def readU16LE(self, register):
+        """Read an unsigned 16-bit value from the specified register, in little
+        endian byte order."""
+        return self.readU16(register, little_endian=True)
+
+    def readU16BE(self, register):
+        """Read an unsigned 16-bit value from the specified register, in big
+        endian byte order."""
+        return self.readU16(register, little_endian=False)
+
+    def readS16LE(self, register):
+        """Read a signed 16-bit value from the specified register, in little
+        endian byte order."""
+        return self.readS16(register, little_endian=True)
+
+    def readS16BE(self, register):
+        """Read a signed 16-bit value from the specified register, in big
+        endian byte order."""
+        return self.readS16(register, little_endian=False)

darkwater_640/PCA9685.py

@@ -0,0 +1,167 @@
+# Copyright (c) 2016 Adafruit Industries
+# Author: Tony DiCola
+#
+# Updated by: Dark Water
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+from __future__ import division
+import logging
+import time
+import math
+
+
+# Registers/etc:
+PCA9685_ADDRESS    = 0x60
+MODE1              = 0x00
+MODE2              = 0x01
+SUBADR1            = 0x02
+SUBADR2            = 0x03
+SUBADR3            = 0x04
+PRESCALE           = 0xFE
+LED0_ON_L          = 0x06
+LED0_ON_H          = 0x07
+LED0_OFF_L         = 0x08
+LED0_OFF_H         = 0x09
+ALL_LED_ON_L       = 0xFA
+ALL_LED_ON_H       = 0xFB
+ALL_LED_OFF_L      = 0xFC
+ALL_LED_OFF_H      = 0xFD
+
+# Bits:
+RESTART            = 0x80
+SLEEP              = 0x10
+ALLCALL            = 0x01
+INVRT              = 0x10
+OUTDRV             = 0x04
+
+
+logger = logging.getLogger(__name__)
+
+
+def software_reset(i2c=None, **kwargs):
+    """Sends a software reset (SWRST) command to all servo drivers on the bus."""
+    # Setup I2C interface for device 0x00 to talk to all of them.
+    if i2c is None:
+        import I2C as I2C
+        i2c = I2C
+    self._device = i2c.get_i2c_device(0x00, **kwargs)
+    self._device.writeRaw8(0x06)  # SWRST
+
+
+class PCA9685(object):
+    """PCA9685 PWM LED/servo controller."""
+
+    def __init__(self, address=PCA9685_ADDRESS, i2c=None, **kwargs):
+        """Initialize the PCA9685."""
+        # Setup I2C interface for the device.
+        if i2c is None:
+            import I2C as I2C
+            i2c = I2C
+        self._device = i2c.get_i2c_device(address, **kwargs)
+        self.set_all_pwm(0, 0)
+        self._device.write8(MODE2, OUTDRV)
+        self._device.write8(MODE1, ALLCALL)
+        time.sleep(0.005)  # wait for oscillator
+        mode1 = self._device.readU8(MODE1)
+        mode1 = mode1 & ~SLEEP  # wake up (reset sleep)
+        self._device.write8(MODE1, mode1)
+        time.sleep(0.005)  # wait for oscillator
+
+    def set_pwm_freq(self, freq_hz, correctionFactor=1.0):
+        """Set the PWM frequency to the provided value in hertz."""
+        prescaleval = 25000000.0    # 25MHz
+        prescaleval /= 4096.0       # 12-bit
+        prescaleval /= float(freq_hz)
+        prescaleval -= 1.0
+        logger.debug('Setting PWM frequency to {0} Hz'.format(freq_hz))
+        logger.debug('Estimated pre-scale: {0}'.format(prescaleval))
+        prescale = round(prescaleval * correctionFactor + 0.5)
+        logger.debug('Final pre-scale: {0}'.format(prescale))
+        oldmode = self._device.readU8(MODE1);
+        newmode = (oldmode & 0x7F) | 0x10    # sleep
+        self._device.write8(MODE1, newmode)  # go to sleep
+        self._device.write8(PRESCALE, int(math.floor(prescale)))
+        self._device.write8(MODE1, oldmode)
+        time.sleep(0.005)
+        self._device.write8(MODE1, oldmode | 0x80)
+
+    def set_pwm_freq_min(self, freq_hz, correctionFactor=1.0):
+        "Sets the PWM frequency"
+        prescaleval = 25000000.0    # 25MHz
+        prescaleval /= 4096.0       # 12-bit
+        prescaleval /= float(freq_hz)
+        prescaleval -= 1.0
+        if (self.debug):
+          print("Setting PWM frequency to %d Hz" % freq_hz)
+          print("Estimated pre-scale: %d" % prescaleval)
+        prescale = math.floor(prescaleval * correctionFactor + 0.5)
+        if (self.debug):
+          print("Final pre-scale: %d" % prescale)
+
+        oldmode = self.i2c.readU8(self.__MODE1);
+        newmode = (oldmode & 0x7F) | 0x10             # sleep
+        self.i2c.write8(self.__MODE1, newmode)        # go to sleep
+        self.i2c.write8(self.__PRESCALE, int(math.floor(prescale)))
+        self.i2c.write8(self.__MODE1, oldmode)
+        time.sleep(0.005)
+        self.i2c.write8(self.__MODE1, oldmode | 0x80)
+
+    def set_pwm_freq_max(self, freq_hz, correctionFactor=1.0):
+        "Sets the PWM frequency"
+        prescaleval = 25000000.0    # 25MHz
+        prescaleval /= 4096.0       # 12-bit
+        prescaleval /= float(freq_hz)
+        prescaleval -= 1.0
+        if (self.debug):
+          print("Setting PWM frequency to %d Hz" % freq_hz)
+          print("Estimated pre-scale: %d" % prescaleval)
+        prescale = math.ceil(prescaleval * correctionFactor + 0.5)
+        if (self.debug):
+          print("Final pre-scale: %d" % prescale)
+
+        oldmode = self.i2c.readU8(self.__MODE1);
+        newmode = (oldmode & 0x7F) | 0x10             # sleep
+        self.i2c.write8(self.__MODE1, newmode)        # go to sleep
+        self.i2c.write8(self.__PRESCALE, int(math.floor(prescale)))
+        self.i2c.write8(self.__MODE1, oldmode)
+        time.sleep(0.005)
+        self.i2c.write8(self.__MODE1, oldmode | 0x80)
+
+    def get_pwm_freq(self):
+        prescale = self.i2c.readU8(self.__PRESCALE)
+        calcfreq = 25000000.0 / 4096.0 / ( float(prescale) + 1 )
+        if (self.debug):
+          print("Got pre-scale: %d" % prescale)
+          print("Calculated Frequency: %d" % calcfreq)
+
+        return calcfreq
+
+    def set_pwm(self, channel, on, off):
+        """Sets a single PWM channel."""
+        self._device.write8(LED0_ON_L+4*channel, on & 0xFF)
+        self._device.write8(LED0_ON_H+4*channel, on >> 8)
+        self._device.write8(LED0_OFF_L+4*channel, off & 0xFF)
+        self._device.write8(LED0_OFF_H+4*channel, off >> 8)
+
+    def set_all_pwm(self, on, off):
+        """Sets all PWM channels."""
+        self._device.write8(ALL_LED_ON_L, on & 0xFF)
+        self._device.write8(ALL_LED_ON_H, on >> 8)
+        self._device.write8(ALL_LED_OFF_L, off & 0xFF)
+        self._device.write8(ALL_LED_OFF_H, off >> 8)

darkwater_640/Platform.py

@@ -0,0 +1,106 @@
+# Copyright (c) 2014 Adafruit Industries
+# Author: Tony DiCola
+
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+# SOFTWARE.
+import platform
+import re
+
+# Platform identification constants.
+UNKNOWN          = 0
+RASPBERRY_PI     = 1
+BEAGLEBONE_BLACK = 2
+MINNOWBOARD      = 3
+
+def platform_detect():
+    """Detect if running on the Raspberry Pi or Beaglebone Black and return the
+    platform type.  Will return RASPBERRY_PI, BEAGLEBONE_BLACK, or UNKNOWN."""
+    # Handle Raspberry Pi
+    pi = pi_version()
+    if pi is not None:
+        return RASPBERRY_PI
+
+    # Handle Beaglebone Black
+    # TODO: Check the Beaglebone Black /proc/cpuinfo value instead of reading
+    # the platform.
+    plat = platform.platform()
+    if plat.lower().find('armv7l-with-debian') > -1:
+        return BEAGLEBONE_BLACK
+    elif plat.lower().find('armv7l-with-ubuntu') > -1:
+        return BEAGLEBONE_BLACK
+    elif plat.lower().find('armv7l-with-glibc2.4') > -1:
+        return BEAGLEBONE_BLACK
+        
+    # Handle Minnowboard
+    # Assumption is that mraa is installed
+    try: 
+        import mraa 
+        if mraa.getPlatformName()=='MinnowBoard MAX':
+            return MINNOWBOARD
+    except ImportError:
+        pass
+    
+    # Couldn't figure out the platform, just return unknown.
+    return UNKNOWN
+
+
+def pi_revision():
+    """Detect the revision number of a Raspberry Pi, useful for changing
+    functionality like default I2C bus based on revision."""
+    # Revision list available at: http://elinux.org/RPi_HardwareHistory#Board_Revision_History
+    with open('/proc/cpuinfo', 'r') as infile:
+        for line in infile:
+            # Match a line of the form "Revision : 0002" while ignoring extra
+            # info in front of the revsion (like 1000 when the Pi was over-volted).
+            match = re.match('Revision\s+:\s+.*(\w{4})$', line, flags=re.IGNORECASE)
+            if match and match.group(1) in ['0000', '0002', '0003']:
+                # Return revision 1 if revision ends with 0000, 0002 or 0003.
+                return 1
+            elif match:
+                # Assume revision 2 if revision ends with any other 4 chars.
+                return 2
+        # Couldn't find the revision, throw an exception.
+        raise RuntimeError('Could not determine Raspberry Pi revision.')
+
+
+def pi_version():
+    """Detect the version of the Raspberry Pi.  Returns either 1, 2 or
+    None depending on if it's a Raspberry Pi 1 (model A, B, A+, B+),
+    Raspberry Pi 2 (model B+), or not a Raspberry Pi.
+    """
+    # Check /proc/cpuinfo for the Hardware field value.
+    # 2708 is pi 1
+    # 2709 is pi 2
+    # Anything else is not a pi.
+    with open('/proc/cpuinfo', 'r') as infile:
+        cpuinfo = infile.read()
+    # Match a line like 'Hardware   : BCM2709'
+    match = re.search('^Hardware\s+:\s+(\w+)$', cpuinfo,
+                      flags=re.MULTILINE | re.IGNORECASE)
+    if not match:
+        # Couldn't find the hardware, assume it isn't a pi.
+        return None
+    if match.group(1) == 'BCM2708':
+        # Pi 1
+        return 1
+    elif match.group(1) == 'BCM2709':
+        # Pi 2
+        return 2
+    else:
+        # Something else, not a pi.
+        return None

darkwater_640/SPI.py

@@ -0,0 +1,336 @@
+# Copyright (c) 2014 Adafruit Industries
+# Author: Tony DiCola
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+
+import operator
+import time
+
+import Adafruit_GPIO as GPIO
+
+
+MSBFIRST = 0
+LSBFIRST = 1
+
+
+class SpiDev(object):
+    """Hardware-based SPI implementation using the spidev interface."""
+
+    def __init__(self, port, device, max_speed_hz=500000):
+        """Initialize an SPI device using the SPIdev interface.  Port and device
+        identify the device, for example the device /dev/spidev1.0 would be port
+        1 and device 0.
+        """
+        import spidev
+        self._device = spidev.SpiDev()
+        self._device.open(port, device)
+        self._device.max_speed_hz=max_speed_hz
+        # Default to mode 0.
+        self._device.mode = 0
+
+    def set_clock_hz(self, hz):
+        """Set the speed of the SPI clock in hertz.  Note that not all speeds
+        are supported and a lower speed might be chosen by the hardware.
+        """
+        self._device.max_speed_hz=hz
+
+    def set_mode(self, mode):
+        """Set SPI mode which controls clock polarity and phase.  Should be a
+        numeric value 0, 1, 2, or 3.  See wikipedia page for details on meaning:
+        http://en.wikipedia.org/wiki/Serial_Peripheral_Interface_Bus
+        """
+        if mode < 0 or mode > 3:
+            raise ValueError('Mode must be a value 0, 1, 2, or 3.')
+        self._device.mode = mode
+
+    def set_bit_order(self, order):
+        """Set order of bits to be read/written over serial lines.  Should be
+        either MSBFIRST for most-significant first, or LSBFIRST for
+        least-signifcant first.
+        """
+        if order == MSBFIRST:
+            self._device.lsbfirst = False
+        elif order == LSBFIRST:
+            self._device.lsbfirst = True
+        else:
+            raise ValueError('Order must be MSBFIRST or LSBFIRST.')
+
+    def close(self):
+        """Close communication with the SPI device."""
+        self._device.close()
+
+    def write(self, data):
+        """Half-duplex SPI write.  The specified array of bytes will be clocked
+        out the MOSI line.
+        """
+        self._device.writebytes(data)
+
+    def read(self, length):
+        """Half-duplex SPI read.  The specified length of bytes will be clocked
+        in the MISO line and returned as a bytearray object.
+        """
+        return bytearray(self._device.readbytes(length))
+
+    def transfer(self, data):
+        """Full-duplex SPI read and write.  The specified array of bytes will be
+        clocked out the MOSI line, while simultaneously bytes will be read from
+        the MISO line.  Read bytes will be returned as a bytearray object.
+        """
+        return bytearray(self._device.xfer2(data))
+
+class SpiDevMraa(object):
+    """Hardware SPI implementation with the mraa library on Minnowboard"""
+    def __init__(self, port, device, max_speed_hz=500000):
+        import mraa
+        self._device = mraa.Spi(0)
+        self._device.mode(0)
+        
+    def set_clock_hz(self, hz):
+        """Set the speed of the SPI clock in hertz.  Note that not all speeds
+        are supported and a lower speed might be chosen by the hardware.
+        """
+        self._device.frequency(hz)
+
+    def set_mode(self,mode):
+        """Set SPI mode which controls clock polarity and phase.  Should be a
+        numeric value 0, 1, 2, or 3.  See wikipedia page for details on meaning:
+        http://en.wikipedia.org/wiki/Serial_Peripheral_Interface_Bus
+        """
+        if mode < 0 or mode > 3:
+            raise ValueError('Mode must be a value 0, 1, 2, or 3.')
+        self._device.mode(mode)
+     
+    def set_mode(self,mode):
+        """Set SPI mode which controls clock polarity and phase.  Should be a
+        numeric value 0, 1, 2, or 3.  See wikipedia page for details on meaning:
+        http://en.wikipedia.org/wiki/Serial_Peripheral_Interface_Bus
+        """
+        if mode < 0 or mode > 3:
+            raise ValueError('Mode must be a value 0, 1, 2, or 3.')
+        self._device.mode(mode)
+
+    def set_bit_order(self, order):
+        """Set order of bits to be read/written over serial lines.  Should be
+        either MSBFIRST for most-significant first, or LSBFIRST for
+        least-signifcant first.
+        """
+        if order == MSBFIRST:
+            self._device.lsbmode(False)
+        elif order == LSBFIRST:
+            self._device.lsbmode(True)
+        else:
+            raise ValueError('Order must be MSBFIRST or LSBFIRST.')
+        
+    def close(self):
+        """Close communication with the SPI device."""
+        self._device.Spi()
+
+    def write(self, data):
+        """Half-duplex SPI write.  The specified array of bytes will be clocked
+        out the MOSI line.
+        """
+        self._device.write(bytearray(data))
+
+class BitBang(object):
+    """Software-based implementation of the SPI protocol over GPIO pins."""
+
+    def __init__(self, gpio, sclk, mosi=None, miso=None, ss=None):
+        """Initialize bit bang (or software) based SPI.  Must provide a BaseGPIO
+        class, the SPI clock, and optionally MOSI, MISO, and SS (slave select)
+        pin numbers. If MOSI is set to None then writes will be disabled and fail
+        with an error, likewise for MISO reads will be disabled.  If SS is set to
+        None then SS will not be asserted high/low by the library when
+        transfering data.
+        """
+        self._gpio = gpio
+        self._sclk = sclk
+        self._mosi = mosi
+        self._miso = miso
+        self._ss = ss
+        # Set pins as outputs/inputs.
+        gpio.setup(sclk, GPIO.OUT)
+        if mosi is not None:
+            gpio.setup(mosi, GPIO.OUT)
+        if miso is not None:
+            gpio.setup(miso, GPIO.IN)
+        if ss is not None:
+            gpio.setup(ss, GPIO.OUT)
+            # Assert SS high to start with device communication off.
+            gpio.set_high(ss)
+        # Assume mode 0.
+        self.set_mode(0)
+        # Assume most significant bit first order.
+        self.set_bit_order(MSBFIRST)
+
+    def set_clock_hz(self, hz):
+        """Set the speed of the SPI clock.  This is unsupported with the bit
+        bang SPI class and will be ignored.
+        """
+        pass
+
+    def set_mode(self, mode):
+        """Set SPI mode which controls clock polarity and phase.  Should be a
+        numeric value 0, 1, 2, or 3.  See wikipedia page for details on meaning:
+        http://en.wikipedia.org/wiki/Serial_Peripheral_Interface_Bus
+        """
+        if mode < 0 or mode > 3:
+            raise ValueError('Mode must be a value 0, 1, 2, or 3.')
+        if mode & 0x02:
+            # Clock is normally high in mode 2 and 3.
+            self._clock_base = GPIO.HIGH
+        else:
+            # Clock is normally low in mode 0 and 1.
+            self._clock_base = GPIO.LOW
+        if mode & 0x01:
+            # Read on trailing edge in mode 1 and 3.
+            self._read_leading = False
+        else:
+            # Read on leading edge in mode 0 and 2.
+            self._read_leading = True
+        # Put clock into its base state.
+        self._gpio.output(self._sclk, self._clock_base)
+
+    def set_bit_order(self, order):
+        """Set order of bits to be read/written over serial lines.  Should be
+        either MSBFIRST for most-significant first, or LSBFIRST for
+        least-signifcant first.
+        """
+        # Set self._mask to the bitmask which points at the appropriate bit to
+        # read or write, and appropriate left/right shift operator function for
+        # reading/writing.
+        if order == MSBFIRST:
+            self._mask = 0x80
+            self._write_shift = operator.lshift
+            self._read_shift = operator.rshift
+        elif order == LSBFIRST:
+            self._mask = 0x01
+            self._write_shift = operator.rshift
+            self._read_shift = operator.lshift
+        else:
+            raise ValueError('Order must be MSBFIRST or LSBFIRST.')
+
+    def close(self):
+        """Close the SPI connection.  Unused in the bit bang implementation."""
+        pass
+
+    def write(self, data, assert_ss=True, deassert_ss=True):
+        """Half-duplex SPI write.  If assert_ss is True, the SS line will be
+        asserted low, the specified bytes will be clocked out the MOSI line, and
+        if deassert_ss is True the SS line be put back high.
+        """
+        # Fail MOSI is not specified.
+        if self._mosi is None:
+            raise RuntimeError('Write attempted with no MOSI pin specified.')
+        if assert_ss and self._ss is not None:
+            self._gpio.set_low(self._ss)
+        for byte in data:
+            for i in range(8):
+                # Write bit to MOSI.
+                if self._write_shift(byte, i) & self._mask:
+                    self._gpio.set_high(self._mosi)
+                else:
+                    self._gpio.set_low(self._mosi)
+                # Flip clock off base.
+                self._gpio.output(self._sclk, not self._clock_base)
+                # Return clock to base.
+                self._gpio.output(self._sclk, self._clock_base)
+        if deassert_ss and self._ss is not None:
+            self._gpio.set_high(self._ss)
+
+    def read(self, length, assert_ss=True, deassert_ss=True):
+        """Half-duplex SPI read.  If assert_ss is true, the SS line will be
+        asserted low, the specified length of bytes will be clocked in the MISO
+        line, and if deassert_ss is true the SS line will be put back high.
+        Bytes which are read will be returned as a bytearray object.
+        """
+        if self._miso is None:
+            raise RuntimeError('Read attempted with no MISO pin specified.')
+        if assert_ss and self._ss is not None:
+            self._gpio.set_low(self._ss)
+        result = bytearray(length)
+        for i in range(length):
+            for j in range(8):
+                # Flip clock off base.
+                self._gpio.output(self._sclk, not self._clock_base)
+                # Handle read on leading edge of clock.
+                if self._read_leading:
+                    if self._gpio.is_high(self._miso):
+                        # Set bit to 1 at appropriate location.
+                        result[i] |= self._read_shift(self._mask, j)
+                    else:
+                        # Set bit to 0 at appropriate location.
+                        result[i] &= ~self._read_shift(self._mask, j)
+                # Return clock to base.
+                self._gpio.output(self._sclk, self._clock_base)
+                # Handle read on trailing edge of clock.
+                if not self._read_leading:
+                    if self._gpio.is_high(self._miso):
+                        # Set bit to 1 at appropriate location.
+                        result[i] |= self._read_shift(self._mask, j)
+                    else:
+                        # Set bit to 0 at appropriate location.
+                        result[i] &= ~self._read_shift(self._mask, j)
+        if deassert_ss and self._ss is not None:
+            self._gpio.set_high(self._ss)
+        return result
+
+    def transfer(self, data, assert_ss=True, deassert_ss=True):
+        """Full-duplex SPI read and write.  If assert_ss is true, the SS line
+        will be asserted low, the specified bytes will be clocked out the MOSI
+        line while bytes will also be read from the MISO line, and if
+        deassert_ss is true the SS line will be put back high.  Bytes which are
+        read will be returned as a bytearray object.
+        """
+        if self._mosi is None:
+            raise RuntimeError('Write attempted with no MOSI pin specified.')
+        if self._mosi is None:
+            raise RuntimeError('Read attempted with no MISO pin specified.')
+        if assert_ss and self._ss is not None:
+            self._gpio.set_low(self._ss)
+        result = bytearray(len(data))
+        for i in range(len(data)):
+            for j in range(8):
+                # Write bit to MOSI.
+                if self._write_shift(data[i], j) & self._mask:
+                    self._gpio.set_high(self._mosi)
+                else:
+                    self._gpio.set_low(self._mosi)
+                # Flip clock off base.
+                self._gpio.output(self._sclk, not self._clock_base)
+                # Handle read on leading edge of clock.
+                if self._read_leading:
+                    if self._gpio.is_high(self._miso):
+                        # Set bit to 1 at appropriate location.
+                        result[i] |= self._read_shift(self._mask, j)
+                    else:
+                        # Set bit to 0 at appropriate location.
+                        result[i] &= ~self._read_shift(self._mask, j)
+                # Return clock to base.
+                self._gpio.output(self._sclk, self._clock_base)
+                # Handle read on trailing edge of clock.
+                if not self._read_leading:
+                    if self._gpio.is_high(self._miso):
+                        # Set bit to 1 at appropriate location.
+                        result[i] |= self._read_shift(self._mask, j)
+                    else:
+                        # Set bit to 0 at appropriate location.
+                        result[i] &= ~self._read_shift(self._mask, j)
+        if deassert_ss and self._ss is not None:
+            self._gpio.set_high(self._ss)
+        return result

darkwater_640/__init__.py

@@ -0,0 +1,2 @@
+from .darkwater_640 import dw_Motor, dw_Servo, dw_Stepper, dw_Controller
+from .mpu9250 import MPU9250

darkwater_640/darkwater_640.py

@@ -0,0 +1,431 @@
+#!/usr/bin/python
+
+import RPi.GPIO as GPIO
+from .PCA9685 import PCA9685
+import time
+import math
+
+# Stepper motor code based on Adafruit Python Library for DC + Stepper Motor HAT
+# Written by Limor Fried for Adafruit Industries. MIT license.
+
+class dw_Stepper:
+        MICROSTEPS = 8
+        MICROSTEP_CURVE = [0, 50, 98, 142, 180, 212, 236, 250, 255]
+
+        #MICROSTEPS = 16
+        # a sinusoidal curve NOT LINEAR!
+        #MICROSTEP_CURVE = [0, 25, 50, 74, 98, 120, 141, 162, 180, 197, 212, 225, 236, 244, 250, 253, 255]
+
+        def __init__(self, controller, num, steps=200, forcemode=False):
+                self.speed = 0
+                self.MC = controller
+                self.motornum = num
+                modepin = in1 = in2 = 0
+
+                self.revsteps = steps
+                self.sec_per_step = 0.1
+                self.steppingcounter = 0
+                self.currentstep = 0
+
+                if(self.MC.getMode() != dw_Controller.ININ and forcemode != True ):
+                        raise NameError('Mode needs to be set to ININ for stepper mode operation')
+                else:
+                        self.MC.setMode(dw_Controller.ININ)
+
+                if (num == 0):
+                        ain1 = 2 
+                        ain2 = 3  
+                        bin1 = 4  
+                        bin2 = 5  
+                elif (num == 1):
+                        ain1 = 6  
+                        ain2 = 7  
+                        bin1 = 8  
+                        bin2 = 9  
+                elif (num == 2):
+                        ain1 = 10  
+                        ain2 = 11  
+                        bin1 = 12  
+                        bin2 = 13  
+                else:
+                        raise NameError('Stepper must be between 1 and 3 inclusive')
+
+                self.ain1 = ain1
+                self.ain2 = ain2
+                self.bin1 = bin1
+                self.bin2 = bin2
+
+                # switch off both drivers
+                self.run(dw_Controller.STOP, 0)
+
+        def run(self, command, speed = 0):
+                if not self.MC:
+                        return
+                
+                if (command == dw_Controller.STOP):
+                        self.MC.setPin(self.ain1, 0)
+                        self.MC.setPin(self.ain2, 0)
+                        self.MC.setPin(self.bin1, 0)
+                        self.MC.setPin(self.bin2, 0)
+
+        def off(self):
+                self.run(dw_Controller.STOP, 0)
+
+        def setMotorSpeed(self, rpm):
+                self.sec_per_step = 60.0 / (self.revsteps * rpm)
+                self.steppingcounter = 0
+
+        def oneStep(self, dir, style):
+                pwm_a = pwm_b = 255
+
+                # first determine what sort of stepping procedure we're up to
+                if (style == dw_Controller.SINGLE):
+                        if ((self.currentstep/(self.MICROSTEPS/2)) % 2):
+                                # we're at an odd step, weird
+                                if (dir == dw_Controller.FORWARD):
+                                        self.currentstep += self.MICROSTEPS/2
+                                else:
+                                        self.currentstep -= self.MICROSTEPS/2
+                        else:
+                                # go to next even step
+                                if (dir == dw_Controller.FORWARD):
+                                        self.currentstep += self.MICROSTEPS
+                                else:
+                                        self.currentstep -= self.MICROSTEPS
+                if (style == dw_Controller.DOUBLE):
+                        if not (self.currentstep/(self.MICROSTEPS/2) % 2):
+                                # we're at an even step, weird
+                                if (dir == dw_Controller.FORWARD):
+                                        self.currentstep += self.MICROSTEPS/2
+                                else:
+                                        self.currentstep -= self.MICROSTEPS/2
+                        else:
+                                # go to next odd step
+                                if (dir == dw_Controller.FORWARD):
+                                        self.currentstep += self.MICROSTEPS
+                                else:
+                                        self.currentstep -= self.MICROSTEPS
+
+                if (style == dw_Controller.MICROSTEP):
+                        if (dir == dw_Controller.FORWARD):
+                                self.currentstep += 1
+                        else:
+                                self.currentstep -= 1
+
+                        # go to next 'step' and wrap around
+                        self.currentstep += self.MICROSTEPS * 4
+                        self.currentstep %= self.MICROSTEPS * 4
+
+                        pwm_a = pwm_b = 0
+                        if (self.currentstep >= 0) and (self.currentstep < self.MICROSTEPS):
+                                pwm_a = self.MICROSTEP_CURVE[self.MICROSTEPS - self.currentstep]
+                                pwm_b = self.MICROSTEP_CURVE[self.currentstep]
+                        elif (self.currentstep >= self.MICROSTEPS) and (self.currentstep < self.MICROSTEPS*2):
+                                pwm_a = self.MICROSTEP_CURVE[self.currentstep - self.MICROSTEPS]
+                                pwm_b = self.MICROSTEP_CURVE[self.MICROSTEPS*2 - self.currentstep]
+                        elif (self.currentstep >= self.MICROSTEPS*2) and (self.currentstep < self.MICROSTEPS*3):
+                                pwm_a = self.MICROSTEP_CURVE[self.MICROSTEPS*3 - self.currentstep]
+                                pwm_b = self.MICROSTEP_CURVE[self.currentstep - self.MICROSTEPS*2]
+                        elif (self.currentstep >= self.MICROSTEPS*3) and (self.currentstep < self.MICROSTEPS*4):
+                                pwm_a = self.MICROSTEP_CURVE[self.currentstep - self.MICROSTEPS*3]
+                                pwm_b = self.MICROSTEP_CURVE[self.MICROSTEPS*4 - self.currentstep]
+
+
+                # go to next 'step' and wrap around
+                self.currentstep += self.MICROSTEPS * 4
+                self.currentstep %= self.MICROSTEPS * 4
+
+                # set up coil energizing!
+                coils = [0, 0, 0, 0]
+
+                if (style == dw_Controller.MICROSTEP):
+                        if (self.currentstep >= 0) and (self.currentstep < self.MICROSTEPS):
+                                coils = [1, 1, 0, 0]
+                        elif (self.currentstep >= self.MICROSTEPS) and (self.currentstep < self.MICROSTEPS*2):
+                                coils = [0, 1, 1, 0]
+                        elif (self.currentstep >= self.MICROSTEPS*2) and (self.currentstep < self.MICROSTEPS*3):
+                                coils = [0, 0, 1, 1]
+                        elif (self.currentstep >= self.MICROSTEPS*3) and (self.currentstep < self.MICROSTEPS*4):
+                                coils = [1, 0, 0, 1]
+                else:
+                        step2coils = [  [1, 0, 0, 0],
+                                        [1, 1, 0, 0],
+                                        [0, 1, 0, 0],
+                                        [0, 1, 1, 0],
+                                        [0, 0, 1, 0],
+                                        [0, 0, 1, 1],
+                                        [0, 0, 0, 1],
+                                        [1, 0, 0, 1] ]
+                        coils = step2coils[self.currentstep/(self.MICROSTEPS/2)]
+
+                self.MC.setPin(self.ain1, coils[0]) #ain2
+                self.MC.setPin(self.bin1, coils[1]) #bin1
+                self.MC.setPin(self.ain2, coils[2]) #ain1
+                self.MC.setPin(self.bin2, coils[3]) #bin2
+
+                return self.currentstep
+
+        def step(self, steps, direction, stepstyle):
+              s_per_s = self.sec_per_step
+              lateststep = 0
+
+              if (stepstyle == dw_Controller.MICROSTEP):
+                      s_per_s /= self.MICROSTEPS
+                      steps *= self.MICROSTEPS
+
+              for s in range(steps):
+                      lateststep = self.oneStep(direction, stepstyle)
+                      time.sleep(s_per_s)
+
+              if (stepstyle == dw_Controller.MICROSTEP):
+                      # this is an edge case, if we are in between full steps, lets just keep going
+                      # so we end on a full step
+                      while (lateststep != 0) and (lateststep != self.MICROSTEPS):
+                              lateststep = self.oneStep(dir, stepstyle)
+                              time.sleep(s_per_s)
+
+class dw_Motor:
+        def __init__(self, controller, num):
+                self.speed = 0
+                self.MC = controller
+                self.motornum = num
+                modepin = in1 = in2 = 0
+
+                if (num == 0):
+                         in2 = 2   #phase
+                         in1 = 3   #enable
+                elif (num == 1):
+                         in2 = 4   #phase
+                         in1 = 5   #enable
+                elif (num == 2):
+                         in2 = 6  #phase
+                         in1 = 7  #enable
+                elif (num == 3):
+                         in2 = 8  #phase
+                         in1 = 9  #enable
+                elif (num == 4):
+                         in2 = 10   #phase
+                         in1 = 11   #enable
+                elif (num == 5):
+                         in2 = 12  #phase
+                         in1 = 13  #enable
+                else:
+                        raise NameError('Motors must be between 1 and 6 inclusive')
+
+                # for phase enable
+                self.PHpin = in2
+                self.ENpin = in1
+                # for in/in
+                self.IN2pin = in2
+                self.IN1pin = in1
+                # switch off
+                self.run(dw_Controller.STOP, 0)
+
+        def setMotorSpeed(self, value):
+                # Check for PWM values
+                if(value >= 1000) and (value < 1500):
+                        self.run(dw_Controller.REVERSE, round(translate(value,1500,1000, 0, 255)))
+                if(value > 1500) and (value <= 2000):
+                        self.run(dw_Controller.FORWARD, round(translate(value, 1500, 2000, 0, 255)))
+                if(value == 1500):
+                        self.run(dw_Controller.STOP, 0)
+                if(value > 0) and (value <= 255):
+                        self.run(dw_Controller.FORWARD, value)
+                if(value == 0):
+                        self.run(dw_Controller.STOP, value)
+                if(value < 0) and (value >= -255):
+                        self.run(dw_Controller.REVERSE, abs(value))
+
+        def run(self, command, speed = 0):
+                if not self.MC:
+                        return
+                if (self.MC.getMode() == dw_Controller.PHASE):
+                        if (command == dw_Controller.FORWARD):
+                                self.MC.setPin(self.PHpin, 0)
+                                self.MC._pwm.set_pwm(self.ENpin, 0, speed*16)
+                        if (command == dw_Controller.REVERSE):
+                                self.MC.setPin(self.PHpin, 1)
+                                self.MC._pwm.set_pwm(self.ENpin, 0, speed*16)
+                        if (command == dw_Controller.STOP):
+                                self.MC.setPin(self.PHpin, 0)
+                                self.MC.setPin(self.ENpin, 0)
+                else:   ## IN/IN mode
+                        if (command == dw_Controller.FORWARD):
+                                self.MC.setPin(self.IN2pin, 0)
+                                self.MC._pwm.set_pwm(self.IN1pin, 0, speed*16)
+                        if (command == dw_Controller.REVERSE):
+                                self.MC.setPin(self.IN1pin, 0)
+                                self.MC._pwm.set_pwm(self.IN2pin, 0, speed*16)
+                        if (command == dw_Controller.STOP):
+                                self.MC.setPin(self.IN1pin, 1)
+                                self.MC.setPin(self.IN2pin, 1)
+                        if (command == dw_Controller.COAST):
+                                self.MC.setPin(self.IN1pin, 0)
+                                self.MC.setPin(self.IN2pin, 0)   
+
+        def off(self):
+                self.run(dw_Controller.STOP, 0)
+
+
+class dw_Servo:
+        def __init__(self, controller, num, freq):
+
+                _SERVO_MIN_MS = 1.250 #ms
+                _SERVO_MAX_MS = 1.750 #ms
+
+                self.speed = 0
+                self.MC = controller
+                self.cnum = num
+                self.pin = 0
+
+                self.freq = freq
+
+                self.servo_min = math.trunc( ( _SERVO_MIN_MS * 4096 ) / (1000.0 / self.freq ) - 1 )
+                self.servo_max = math.trunc( ( _SERVO_MAX_MS * 4096 ) / (1000.0 / self.freq ) - 1 )
+
+                self.servo_zero = math.trunc( ( self.servo_min + self.servo_max ) / 2 ) # halfway = 0 degrees
+
+
+                if (num == 0):
+                    self.pin = 0
+                elif (num == 1):
+                    self.pin = 1
+                else:
+                    raise NameError('Port must be between 0 and 1 inclusive')
+
+                # switch off
+                self.off()
+
+        def off(self):
+                self.MC.setPin(self.pin, 0)
+
+        def setAngle(self, angle):
+                pulse = self.servo_zero + ( (self.servo_zero - self.servo_min ) * angle / 80 )
+                #print "angle=%s pulse=%s" % (angle, pulse)
+                #self.setPWMmS( pulse )
+
+        def setPWM(self, value):
+                if(value > 0):
+                    self.MC._pwm.set_pwm(self.pin, 0, int(value) )
+                if(value == 0):
+                    self.off()
+
+        def setPWMmS(self, length_ms):
+                self.setPWM( math.trunc( ( length_ms * 4096 ) / ( 1000.0 / self.freq ) ) - 1 )
+
+        def setPWMuS(self, length_us):
+                self.setPWM( math.trunc( ( length_us * 4096 ) / ( 1000000.0 / self.freq ) ) -1 )
+
+
+
+class dw_Controller:
+        FORWARD = 1
+        REVERSE = 2
+        BRAKEFORWARD = 3
+        BRAKEREVERSE = 4
+        STOP = 5 #brake
+        COAST = 6 # in/in only
+
+        SINGLE = 1
+        DOUBLE = 2
+        INTERLEAVE = 3
+        MICROSTEP = 4
+
+        ININ = 0
+        PHASE = 1
+
+        def __init__(self, addr = 0x60, freq = 100, correctionFactor = 1.0):
+                self._i2caddr = addr            # default addr on HAT
+                self._frequency = freq          # default @1600Hz PWM freq
+                # self.steppers = [ Adafruit_StepperMotor(self, 1), Adafruit_StepperMotor(self, 2) ]
+                self._pwm =  PCA9685(addr)
+                self._pwm.set_pwm_freq(self._frequency, correctionFactor)
+                # Just gonna default to high for now
+                GPIO.setmode(GPIO.BCM)
+                GPIO.setwarnings(False)
+                GPIO.setup(27, GPIO.OUT)
+
+                self.setMode(self.ININ) # set high for en/phase mode - low = in/in mode
+
+                self.motors = [ dw_Motor(self, m) for m in range(6) ]
+                self.servos = [ dw_Servo(self, m, freq) for m in range(2) ]
+
+                self.steppers =  [ dw_Stepper(self, m) for m in range(3) ]
+
+        def setMode(self, mode = 1):
+                if (mode == self.ININ):
+                        self._mode = self.ININ
+                        GPIO.output(27, GPIO.LOW)
+                else:
+                        self._mode = self.PHASE
+                        GPIO.output(27, GPIO.HIGH)
+
+        def getMode(self):
+                return self._mode
+
+        def setPin(self, pin, value):
+                if (pin < 0) or (pin > 15):
+                        raise NameError('PWM pin must be between 0 and 15 inclusive')
+                if (value != 0) and (value != 1):
+                        raise NameError('Pin value must be 0 or 1!')
+                if (value == 0):
+                        self._pwm.set_pwm(pin, 0, 4096)
+                if (value == 1):
+                        self._pwm.set_pwm(pin, 4096, 0)
+
+        def setAllPin(self, value):
+                if (pin < 0) or (pin > 15):
+                        raise NameError('PWM pin must be between 0 and 15 inclusive')
+                if (value != 0) and (value != 1):
+                        raise NameError('Pin value must be 0 or 1!')
+                if (value == 0):
+                        self._pwm.set_all_pwm(0, 4096)
+                if (value == 1):
+                        self._pwm.set_all_pwm(4096, 0)
+
+        def getMotor(self, num):
+                if (num < 1) or (num > 6):
+                        raise NameError('Motors must be between 1 and 6 inclusive')
+                return self.motors[num-1]
+
+        def getServo(self, num):
+                if (num < 1) or (num > 2):
+                        raise NameError('Servos must be between 1 and 2 inclusive')
+                return self.servos[num-1]
+
+        def getStepper(self, num):
+                if (num < 1) or (num > 3):
+                        raise NameError('Stepper motors must be between 1 and 3 inclusive')
+                return self.steppers[num-1]
+
+        def setAllPWM(self, value):
+                if(value > 0):
+                        self._pwm.set_all_pwm(0, value)
+                if(value == 0):
+                        self.allOff()
+
+        def setAllPWMmS(self, value):
+                if(value > 0):
+                        self._pwm.set_all_pwm(0, value)
+                if(value == 0):
+                        self.allOff()
+
+        def setAllPWMuS(self, value):
+                if(value > 0):
+                        self._pwm.set_all_pwm(0, value)
+                if(value == 0):
+                        self.allOff()
+
+        def allOff(self):
+                this.setAllPin( 0 );
+
+def translate(value, leftMin, leftMax, rightMin, rightMax):
+    # Figure out how 'wide' each range is
+    leftSpan = leftMax - leftMin
+    rightSpan = rightMax - rightMin
+
+    # Convert the left range into a 0-1 range (float)
+    valueScaled = float(value - leftMin) / float(leftSpan)
+
+    # Convert the 0-1 range into a value in the right range.
+    return rightMin + (valueScaled * rightSpan)

darkwater_640/mpu9250.py

@@ -0,0 +1,614 @@
+"""
+MPU9250 driver code is placed under the BSD license.
+Copyright (c) 2014, Emlid Limited, www.emlid.com
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+    notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+    notice, this list of conditions and the following disclaimer in the
+    documentation and/or other materials provided with the distribution.
+    * Neither the name of the Emlid Limited nor the names of its contributors
+    may be used to endorse or promote products derived from this software
+    without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL EMLID LIMITED BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+"""
+
+import spidev
+import time
+import struct
+import array
+
+class MPU9250:
+
+    G_SI = 9.80665
+    PI = 3.14159
+
+    # MPU9250 registers
+    __MPUREG_XG_OFFS_TC = 0x00
+    __MPUREG_YG_OFFS_TC = 0x01
+    __MPUREG_ZG_OFFS_TC = 0x02
+    __MPUREG_X_FINE_GAIN = 0x03
+    __MPUREG_Y_FINE_GAIN = 0x04
+    __MPUREG_Z_FINE_GAIN = 0x05
+    __MPUREG_XA_OFFS_H = 0x06
+    __MPUREG_XA_OFFS_L = 0x07
+    __MPUREG_YA_OFFS_H = 0x08
+    __MPUREG_YA_OFFS_L = 0x09
+    __MPUREG_ZA_OFFS_H = 0x0A
+    __MPUREG_ZA_OFFS_L = 0x0B
+    __MPUREG_PRODUCT_ID = 0x0C
+    __MPUREG_SELF_TEST_X = 0x0D
+    __MPUREG_SELF_TEST_Y = 0x0E
+    __MPUREG_SELF_TEST_Z = 0x0F
+    __MPUREG_SELF_TEST_A = 0x10
+    __MPUREG_XG_OFFS_USRH = 0x13
+    __MPUREG_XG_OFFS_USRL = 0x14
+    __MPUREG_YG_OFFS_USRH = 0x15
+    __MPUREG_YG_OFFS_USRL = 0x16
+    __MPUREG_ZG_OFFS_USRH = 0x17
+    __MPUREG_ZG_OFFS_USRL = 0x18
+    __MPUREG_SMPLRT_DIV = 0x19
+    __MPUREG_CONFIG = 0x1A
+    __MPUREG_GYRO_CONFIG = 0x1B
+    __MPUREG_ACCEL_CONFIG = 0x1C
+    __MPUREG_ACCEL_CONFIG_2      = 0x1D
+    __MPUREG_LP_ACCEL_ODR        = 0x1E
+    __MPUREG_MOT_THR             = 0x1F
+    __MPUREG_FIFO_EN             = 0x23
+    __MPUREG_I2C_MST_CTRL        = 0x24
+    __MPUREG_I2C_SLV0_ADDR       = 0x25
+    __MPUREG_I2C_SLV0_REG        = 0x26
+    __MPUREG_I2C_SLV0_CTRL       = 0x27
+    __MPUREG_I2C_SLV1_ADDR       = 0x28
+    __MPUREG_I2C_SLV1_REG        = 0x29
+    __MPUREG_I2C_SLV1_CTRL       = 0x2A
+    __MPUREG_I2C_SLV2_ADDR       = 0x2B
+    __MPUREG_I2C_SLV2_REG        = 0x2C
+    __MPUREG_I2C_SLV2_CTRL       = 0x2D
+    __MPUREG_I2C_SLV3_ADDR       = 0x2E
+    __MPUREG_I2C_SLV3_REG        = 0x2F
+    __MPUREG_I2C_SLV3_CTRL       = 0x30
+    __MPUREG_I2C_SLV4_ADDR       = 0x31
+    __MPUREG_I2C_SLV4_REG        = 0x32
+    __MPUREG_I2C_SLV4_DO         = 0x33
+    __MPUREG_I2C_SLV4_CTRL       = 0x34
+    __MPUREG_I2C_SLV4_DI         = 0x35
+    __MPUREG_I2C_MST_STATUS      = 0x36
+    __MPUREG_INT_PIN_CFG = 0x37
+    __MPUREG_INT_ENABLE = 0x38
+    __MPUREG_ACCEL_XOUT_H = 0x3B
+    __MPUREG_ACCEL_XOUT_L = 0x3C
+    __MPUREG_ACCEL_YOUT_H = 0x3D
+    __MPUREG_ACCEL_YOUT_L = 0x3E
+    __MPUREG_ACCEL_ZOUT_H = 0x3F
+    __MPUREG_ACCEL_ZOUT_L = 0x40
+    __MPUREG_TEMP_OUT_H = 0x41
+    __MPUREG_TEMP_OUT_L = 0x42
+    __MPUREG_GYRO_XOUT_H = 0x43
+    __MPUREG_GYRO_XOUT_L = 0x44
+    __MPUREG_GYRO_YOUT_H = 0x45
+    __MPUREG_GYRO_YOUT_L = 0x46
+    __MPUREG_GYRO_ZOUT_H = 0x47
+    __MPUREG_GYRO_ZOUT_L = 0x48
+    __MPUREG_EXT_SENS_DATA_00    = 0x49
+    __MPUREG_EXT_SENS_DATA_01    = 0x4A
+    __MPUREG_EXT_SENS_DATA_02    = 0x4B
+    __MPUREG_EXT_SENS_DATA_03    = 0x4C
+    __MPUREG_EXT_SENS_DATA_04    = 0x4D
+    __MPUREG_EXT_SENS_DATA_05    = 0x4E
+    __MPUREG_EXT_SENS_DATA_06    = 0x4F
+    __MPUREG_EXT_SENS_DATA_07    = 0x50
+    __MPUREG_EXT_SENS_DATA_08    = 0x51
+    __MPUREG_EXT_SENS_DATA_09    = 0x52
+    __MPUREG_EXT_SENS_DATA_10    = 0x53
+    __MPUREG_EXT_SENS_DATA_11    = 0x54
+    __MPUREG_EXT_SENS_DATA_12    = 0x55
+    __MPUREG_EXT_SENS_DATA_13    = 0x56
+    __MPUREG_EXT_SENS_DATA_14    = 0x57
+    __MPUREG_EXT_SENS_DATA_15    = 0x58
+    __MPUREG_EXT_SENS_DATA_16    = 0x59
+    __MPUREG_EXT_SENS_DATA_17    = 0x5A
+    __MPUREG_EXT_SENS_DATA_18    = 0x5B
+    __MPUREG_EXT_SENS_DATA_19    = 0x5C
+    __MPUREG_EXT_SENS_DATA_20    = 0x5D
+    __MPUREG_EXT_SENS_DATA_21    = 0x5E
+    __MPUREG_EXT_SENS_DATA_22    = 0x5F
+    __MPUREG_EXT_SENS_DATA_23    = 0x60
+    __MPUREG_I2C_SLV0_DO         = 0x63
+    __MPUREG_I2C_SLV1_DO         = 0x64
+    __MPUREG_I2C_SLV2_DO         = 0x65
+    __MPUREG_I2C_SLV3_DO         = 0x66
+    __MPUREG_I2C_MST_DELAY_CTRL  = 0x67
+    __MPUREG_SIGNAL_PATH_RESET   = 0x68
+    __MPUREG_MOT_DETECT_CTRL     = 0x69
+    __MPUREG_USER_CTRL = 0x6A
+    __MPUREG_PWR_MGMT_1 = 0x6B
+    __MPUREG_PWR_MGMT_2 = 0x6C
+    __MPUREG_BANK_SEL = 0x6D
+    __MPUREG_MEM_START_ADDR = 0x6E
+    __MPUREG_MEM_R_W = 0x6F
+    __MPUREG_DMP_CFG_1 = 0x70
+    __MPUREG_DMP_CFG_2 = 0x71
+    __MPUREG_FIFO_COUNTH = 0x72
+    __MPUREG_FIFO_COUNTL = 0x73
+    __MPUREG_FIFO_R_W = 0x74
+    __MPUREG_WHOAMI = 0x75
+    __MPUREG_XA_OFFSET_H         = 0x77
+    __MPUREG_XA_OFFSET_L         = 0x78
+    __MPUREG_YA_OFFSET_H         = 0x7A
+    __MPUREG_YA_OFFSET_L         = 0x7B
+    __MPUREG_ZA_OFFSET_H         = 0x7D
+    __MPUREG_ZA_OFFSET_L         = 0x7E
+
+    # ---- AK8963 Reg In MPU9250 -----------------------------------------------
+
+    __AK8963_I2C_ADDR             = 0x0c #0x18
+    __AK8963_Device_ID            = 0x48
+
+    # Read-only Reg
+    __AK8963_WIA                  = 0x00
+    __AK8963_INFO                 = 0x01
+    __AK8963_ST1                  = 0x02
+    __AK8963_HXL                  = 0x03
+    __AK8963_HXH                  = 0x04
+    __AK8963_HYL                  = 0x05
+    __AK8963_HYH                  = 0x06
+    __AK8963_HZL                  = 0x07
+    __AK8963_HZH                  = 0x08
+    __AK8963_ST2                  = 0x09
+
+    # Write/Read Reg
+    __AK8963_CNTL1                = 0x0A
+    __AK8963_CNTL2                = 0x0B
+    __AK8963_ASTC                 = 0x0C
+    __AK8963_TS1                  = 0x0D
+    __AK8963_TS2                  = 0x0E
+    __AK8963_I2CDIS               = 0x0F
+
+    # Read-only Reg ( ROM )
+    __AK8963_ASAX                 = 0x10
+    __AK8963_ASAY                 = 0x11
+    __AK8963_ASAZ                 = 0x12
+
+    # Configuration bits MPU9250
+    __BIT_SLEEP = 0x40
+    __BIT_H_RESET = 0x80
+    __BITS_CLKSEL = 0x07
+    __MPU_CLK_SEL_PLLGYROX = 0x01
+    __MPU_CLK_SEL_PLLGYROZ = 0x03
+    __MPU_EXT_SYNC_GYROX = 0x02
+    __BITS_FS_250DPS              = 0x00
+    __BITS_FS_500DPS              = 0x08
+    __BITS_FS_1000DPS             = 0x10
+    __BITS_FS_2000DPS             = 0x18
+    __BITS_FS_2G                  = 0x00
+    __BITS_FS_4G                  = 0x08
+    __BITS_FS_8G                  = 0x10
+    __BITS_FS_16G                 = 0x18
+    __BITS_FS_MASK                = 0x18
+    __BITS_DLPF_CFG_256HZ_NOLPF2  = 0x00
+    __BITS_DLPF_CFG_188HZ         = 0x01
+    __BITS_DLPF_CFG_98HZ          = 0x02
+    __BITS_DLPF_CFG_42HZ          = 0x03
+    __BITS_DLPF_CFG_20HZ          = 0x04
+    __BITS_DLPF_CFG_10HZ          = 0x05
+    __BITS_DLPF_CFG_5HZ           = 0x06
+    __BITS_DLPF_CFG_2100HZ_NOLPF  = 0x07
+    __BITS_DLPF_CFG_MASK          = 0x07
+    __BIT_INT_ANYRD_2CLEAR        = 0x10
+    __BIT_RAW_RDY_EN              = 0x01
+    __BIT_I2C_IF_DIS              = 0x10
+
+    __READ_FLAG   = 0x80
+
+    # ---- Sensitivity ---------------------------------------------------------
+
+    __MPU9250A_2g       = (0.000061035156) # 0.000061035156 g/LSB
+    __MPU9250A_4g       = (0.000122070312) # 0.000122070312 g/LSB
+    __MPU9250A_8g       = (0.000244140625) # 0.000244140625 g/LSB
+    __MPU9250A_16g      = (0.000488281250) # 0.000488281250 g/LSB
+
+    __MPU9250G_250dps   = (0.007633587786) # 0.007633587786 dps/LSB
+    __MPU9250G_500dps   = (0.015267175572) # 0.015267175572 dps/LSB
+    __MPU9250G_1000dps  = (0.030487804878) # 0.030487804878 dps/LSB
+    __MPU9250G_2000dps  = (0.060975609756) # 0.060975609756 dps/LSB
+
+    __MPU9250M_4800uT   = (0.6)            # 0.6 uT/LSB
+
+    __MPU9250T_85degC   = (0.002995177763) # 0.002995177763 degC/LSB
+
+    __Magnetometer_Sensitivity_Scale_Factor = (0.15)
+
+    def __init__(self, spi_bus_number = 0, spi_dev_number = 1):
+        self.bus = spidev.SpiDev()
+        self.spi_bus_number = spi_bus_number
+        self.spi_dev_number = spi_dev_number
+        self.gyro_divider = 0.0
+        self.acc_divider = 0.0
+        self.calib_data = [0.0, 0.0, 0.0]
+        self.magnetometer_ASA = [0.0, 0.0, 0.0]
+        self.temperature = 0.0
+        self.gyroscope_data = [0.0, 0.0, 0.0]
+        self.accelerometer_data = [0.0, 0.0, 0.0]
+        self.magnetometer_data = [0.0, 0.0, 0.0]
+
+# -----------------------------------------------------------------------------------------------
+#                                     REGISTER READ & WRITE
+# usage: use these methods to read and write MPU9250 registers over SPI
+# -----------------------------------------------------------------------------------------------
+
+    def WriteReg(self, reg_address, data):
+        self.bus.open(self.spi_bus_number, self.spi_dev_number)
+        tx = [reg_address, data]
+        rx = self.bus.xfer2(tx)
+        self.bus.close()
+        return rx
+
+# -----------------------------------------------------------------------------------------------
+
+    def ReadReg(self, reg_address):
+        self.bus.open(self.spi_bus_number, self.spi_dev_number)
+        tx = [reg_address | self.__READ_FLAG, 0x00]
+        rx = self.bus.xfer2(tx)
+        self.bus.close()
+        return rx[1]
+
+# -----------------------------------------------------------------------------------------------
+
+    def ReadRegs(self, reg_address, length):
+        self.bus.open(self.spi_bus_number, self.spi_dev_number)
+        tx = [0] * (length + 1)
+        tx[0] = reg_address | self.__READ_FLAG
+
+        rx = self.bus.xfer2(tx)
+
+        self.bus.close()
+        return rx[1:len(rx)]
+
+# -----------------------------------------------------------------------------------------------
+#                                 TEST CONNECTION
+# usage: call this function to know if SPI and MPU9250 are working correctly.
+# returns true if mpu9250 answers
+# -----------------------------------------------------------------------------------------------
+
+    def testConnection(self):
+        response = self.ReadReg(self.__MPUREG_WHOAMI)
+        if (response == 0x71):
+            return True
+        else:
+            return False
+
+# -----------------------------------------------------------------------------------------------
+#                                     INITIALIZATION
+# usage: call this function at startup, giving the sample rate divider (raging from 0 to 255) and
+# low pass filter value; suitable values are:
+# BITS_DLPF_CFG_256HZ_NOLPF2
+# BITS_DLPF_CFG_188HZ
+# BITS_DLPF_CFG_98HZ
+# BITS_DLPF_CFG_42HZ
+# BITS_DLPF_CFG_20HZ
+# BITS_DLPF_CFG_10HZ
+# BITS_DLPF_CFG_5HZ
+# BITS_DLPF_CFG_2100HZ_NOLPF
+# returns 1 if an error occurred
+# -----------------------------------------------------------------------------------------------
+
+    def initialize(self, sample_rate_div = 1, low_pass_filter = 0x01):
+        MPU_InitRegNum = 17
+        MPU_Init_Data = [[0, 0]] * MPU_InitRegNum
+
+        MPU_Init_Data = [
+        [0x80, self.__MPUREG_PWR_MGMT_1],   # Reset Device
+        [0x01, self.__MPUREG_PWR_MGMT_1],          # Clock Source
+        [0x00, self.__MPUREG_PWR_MGMT_2],          # Enable Acc & Gyro
+        [low_pass_filter, self.__MPUREG_CONFIG],   # Use DLPF set Gyroscope bandwidth 184Hz, temperature bandwidth 188Hz
+        [0x18, self.__MPUREG_GYRO_CONFIG],         # +-2000dps
+        [0x08, self.__MPUREG_ACCEL_CONFIG],        # +-4G
+        [0x09, self.__MPUREG_ACCEL_CONFIG_2],      # Set Acc Data Rates, Enable Acc LPF , Bandwidth 184Hz
+        [0x30, self.__MPUREG_INT_PIN_CFG],
+        #[0x40, self.__MPUREG_I2C_MST_CTRL],       # I2C Speed 348 kHz
+        #[0x20, self.__MPUREG_USER_CTRL],          # Enable AUX
+        [0x20, self.__MPUREG_USER_CTRL],           # I2C Master mode
+        [0x0D, self.__MPUREG_I2C_MST_CTRL],        #  I2C configuration multi-master  IIC 400KHz
+
+        [self.__AK8963_I2C_ADDR, self.__MPUREG_I2C_SLV0_ADDR],  #Set the I2C slave addres of AK8963 and set for write.
+        #[0x09, self.__MPUREG_I2C_SLV4_CTRL],
+        #[0x81, self.__MPUREG_I2C_MST_DELAY_CTRL], #Enable I2C delay
+
+        [self.__AK8963_CNTL2, self.__MPUREG_I2C_SLV0_REG], #I2C slave 0 register address from where to begin data transfer
+        [0x01, self.__MPUREG_I2C_SLV0_DO], # Reset AK8963
+        [0x81, self.__MPUREG_I2C_SLV0_CTRL],  #Enable I2C and set 1 byte
+
+        [self.__AK8963_CNTL1, self.__MPUREG_I2C_SLV0_REG], #I2C slave 0 register address from where to begin data transfer
+        [0x12, self.__MPUREG_I2C_SLV0_DO], # Register value to continuous measurement in 16bit
+        [0x81, self.__MPUREG_I2C_SLV0_CTRL]  #Enable I2C and set 1 byte
+        ]
+
+        for i in range(0, MPU_InitRegNum):
+            self.WriteReg(MPU_Init_Data[i][1], MPU_Init_Data[i][0])
+            time.sleep(0.01) # I2C must slow down the write speed, otherwise it won't work
+
+        self.set_acc_scale(self.__BITS_FS_16G)
+        self.set_gyro_scale(self.__BITS_FS_2000DPS)
+
+        self.calib_mag()
+
+# -----------------------------------------------------------------------------------------------
+#                                 ACCELEROMETER SCALE
+# usage: call this function at startup, after initialization, to set the right range for the
+# accelerometers. Suitable ranges are:
+# BITS_FS_2G
+# BITS_FS_4G
+# BITS_FS_8G
+# BITS_FS_16G
+# returns the range set (2,4,8 or 16)
+# -----------------------------------------------------------------------------------------------
+
+    def set_acc_scale(self, scale):
+        self.WriteReg(self.__MPUREG_ACCEL_CONFIG, scale)
+        if (scale == self.__BITS_FS_2G):
+            self.acc_divider = 16384.0
+        elif (scale == self.__BITS_FS_4G):
+            self.acc_divider = 8192.0
+        elif (scale == self.__BITS_FS_8G):
+            self.acc_divider = 4096.0
+        elif (scale == self.__BITS_FS_16G):
+            self.acc_divider = 2048.0
+
+        temp_scale = self.ReadReg(self.__MPUREG_ACCEL_CONFIG)
+        if (temp_scale == self.__BITS_FS_2G):
+            temp_scale = 2
+        elif (temp_scale == self.__BITS_FS_4G):
+            temp_scale = 4
+        elif (temp_scale == self.__BITS_FS_8G):
+            temp_scale = 8
+        elif (temp_scale == self.__BITS_FS_16G):
+            temp_scale = 16
+
+        return temp_scale
+
+# -----------------------------------------------------------------------------------------------
+#                                 GYROSCOPE SCALE
+# usage: call this function at startup, after initialization, to set the right range for the
+# gyroscopes. Suitable ranges are:
+# BITS_FS_250DPS
+# BITS_FS_500DPS
+# BITS_FS_1000DPS
+# BITS_FS_2000DPS
+# returns the range set (250,500,1000 or 2000)
+# -----------------------------------------------------------------------------------------------
+
+    def set_gyro_scale(self, scale):
+        self.WriteReg(self.__MPUREG_GYRO_CONFIG, scale)
+        if (scale == self.__BITS_FS_250DPS):
+            self.gyro_divider = 131.0
+        elif (scale == self.__BITS_FS_500DPS):
+            self.gyro_divider = 65.6
+        elif (scale == self.__BITS_FS_1000DPS):
+            self.gyro_divider = 32.8
+        elif (scale == self.__BITS_FS_2000DPS):
+            self.gyro_divider = 16.4
+
+        temp_scale = self.ReadReg(self.__MPUREG_GYRO_CONFIG)
+        if (temp_scale == self.__BITS_FS_250DPS):
+            temp_scale = 250
+        elif (temp_scale == self.__BITS_FS_500DPS):
+            temp_scale = 500
+        elif (temp_scale == self.__BITS_FS_1000DPS):
+            temp_scale = 1000
+        elif (temp_scale == self.__BITS_FS_2000DPS):
+            temp_scale = 2000
+
+        return temp_scale
+
+# -----------------------------------------------------------------------------------------------
+#                                 WHO AM I?
+# usage: call this function to know if SPI is working correctly. It checks the I2C address of the
+# mpu9250 which should be 104 when in SPI mode.
+# returns the I2C address (104)
+# -----------------------------------------------------------------------------------------------
+
+    def whoami(self):
+        return self.ReadReg(self.__MPUREG_WHOAMI)
+
+# -----------------------------------------------------------------------------------------------
+#                                 READ ACCELEROMETER
+# usage: call this function to read accelerometer data. Axis represents selected axis:
+# 0 -> X axis
+# 1 -> Y axis
+# 2 -> Z axis
+# -----------------------------------------------------------------------------------------------
+
+    def read_acc(self):
+        response = self.ReadRegs(self.__MPUREG_ACCEL_XOUT_H, 6)
+
+        for i in range(0, 3):
+            data = self.byte_to_float(response[i*2:i*2+2])
+            self.accelerometer_data[i] = self.G_SI * data / self.acc_divider
+
+# -----------------------------------------------------------------------------------------------
+#                                 READ GYROSCOPE
+# usage: call this function to read gyroscope data. Axis represents selected axis:
+# 0 -> X axis
+# 1 -> Y axis
+# 2 -> Z axis
+# -----------------------------------------------------------------------------------------------
+
+    def read_gyro(self):
+        response = self.ReadRegs(self.__MPUREG_GYRO_XOUT_H, 6)
+
+        for i in range(0, 3):
+            data = self.byte_to_float(response[i*2:i*2+2])
+            self.gyroscope_data[i] = (self.PI/180) * data / self.gyro_divider
+
+# -----------------------------------------------------------------------------------------------
+#                                 READ TEMPERATURE
+# usage: call this function to read temperature data.
+# returns the value in Celsius
+# -----------------------------------------------------------------------------------------------
+
+    def read_temp(self):
+        response = self.ReadRegs(self.__MPUREG_TEMP_OUT_H, 2)
+
+        #temp = response[0]*256.0 + response[1]
+        temp = self.byte_to_float(response)
+        self.temperature = (temp/340.0)+36.53
+
+# -----------------------------------------------------------------------------------------------
+#                                 READ ACCELEROMETER CALIBRATION
+# usage: call this function to read accelerometer data. Axis represents selected axis:
+# 0 -> X axis
+# 1 -> Y axis
+# 2 -> Z axis
+# returns Factory Trim value
+# -----------------------------------------------------------------------------------------------
+
+    def calib_acc(self):
+        temp_scale = self.ReadReg(self.__MPUREG_ACCEL_CONFIG)
+        self.set_acc_scale(self.__BITS_FS_8G)
+
+        response = self.ReadRegs(self.__MPUREG_SELF_TEST_X, 4)
+
+        self.calib_data[0] = ((response[0] & 11100000) >> 3) | ((response[3] & 00110000) >> 4)
+        self.calib_data[1] = ((response[1] & 11100000) >> 3) | ((response[3] & 00001100) >> 2)
+        self.calib_data[2] = ((response[2] & 11100000) >> 3) | ((response[3] & 00000011))
+
+        self.set_acc_scale(temp_scale)
+
+# -----------------------------------------------------------------------------------------------
+
+    def AK8963_whoami(self):
+        self.WriteReg(self.__MPUREG_I2C_SLV0_ADDR, self.__AK8963_I2C_ADDR | self.__READ_FLAG) #Set the I2C slave addres of AK8963 and set for read.
+        self.WriteReg(self.__MPUREG_I2C_SLV0_REG, self.__AK8963_WIA) #I2C slave 0 register address from where to begin data transfer
+        self.WriteReg(self.__MPUREG_I2C_SLV0_CTRL, 0x81) #Read 1 byte from the magnetometer
+
+        #self.WriteReg(self.__MPUREG_I2C_SLV0_CTRL, 0x81) # Enable I2C and set bytes
+        time.sleep(0.01)
+
+        return self.ReadReg(self.__MPUREG_EXT_SENS_DATA_00) # Read I2C
+
+# -----------------------------------------------------------------------------------------------
+
+    def calib_mag(self):
+        # Set the I2C slave addres of AK8963 and set for read.
+        self.WriteReg(self.__MPUREG_I2C_SLV0_ADDR, self.__AK8963_I2C_ADDR | self.__READ_FLAG)
+        # I2C slave 0 register address from where to begin data transfer
+        self.WriteReg(self.__MPUREG_I2C_SLV0_REG, self.__AK8963_ASAX)
+        # Read 3 bytes from the magnetometer
+        self.WriteReg(self.__MPUREG_I2C_SLV0_CTRL, 0x83)
+
+        time.sleep(0.01)
+
+        response = self.ReadRegs(self.__MPUREG_EXT_SENS_DATA_00, 3)
+
+        for i in range(0, 3):
+            self.magnetometer_ASA[i] = ((float(response[i]) - 128)/256 + 1) * self.__Magnetometer_Sensitivity_Scale_Factor
+
+# -----------------------------------------------------------------------------------------------
+
+    def read_mag(self):
+        # Set the I2C slave addres of AK8963 and set for read.
+        self.WriteReg(self.__MPUREG_I2C_SLV0_ADDR, self.__AK8963_I2C_ADDR | self.__READ_FLAG)
+        # I2C slave 0 register address from where to begin data transfer
+        self.WriteReg(self.__MPUREG_I2C_SLV0_REG, self.__AK8963_HXL)
+        # Read 6 bytes from the magnetometer
+        self.WriteReg(self.__MPUREG_I2C_SLV0_CTRL, 0x87)
+
+        time.sleep(0.01)
+
+        response = self.ReadRegs(self.__MPUREG_EXT_SENS_DATA_00, 7)
+        #must start your read from AK8963A register 0x03 and read seven bytes so that upon read of ST2 register 0x09 the AK8963A will unlatch the data registers for the next measurement.
+        for i in range(0, 3):
+            data = self.byte_to_float_le(response[i*2:i*2+2])
+            self.magnetometer_data[i] = data * self.magnetometer_ASA[i]
+
+# -----------------------------------------------------------------------------------------------
+
+    def read_all(self):
+        # Send I2C command at first
+        # Set the I2C slave addres of AK8963 and set for read.
+        self.WriteReg(self.__MPUREG_I2C_SLV0_ADDR, self.__AK8963_I2C_ADDR | self.__READ_FLAG)
+        # I2C slave 0 register address from where ; //Read 7 bytes from the magnetometerto begin data transfer
+        self.WriteReg(self.__MPUREG_I2C_SLV0_REG, self.__AK8963_HXL)
+        # Read 7 bytes from the magnetometer
+        self.WriteReg(self.__MPUREG_I2C_SLV0_CTRL, 0x87)
+        # must start your read from AK8963A register 0x03 and read seven bytes so that upon read of ST2 register 0x09 the AK8963A will unlatch the data registers for the next measurement.
+
+        # time.sleep(0.001)
+        response = self.ReadRegs(self.__MPUREG_ACCEL_XOUT_H, 21);
+
+        # Get Accelerometer values
+        for i in range(0, 3):
+            data = self.byte_to_float(response[i*2:i*2+2])
+            self.accelerometer_data[i] = self.G_SI * data / self.acc_divider
+
+        # Get temperature
+        i = 3
+        temp = self.byte_to_float(response[i*2:i*2+2])
+        self.temperature = (temp/340.0)+36.53
+
+        # Get gyroscope values
+        for i in range(4, 7):
+            data = self.byte_to_float(response[i*2:i*2+2])
+            self.gyroscope_data[i-4] = (self.PI/180) * data / self.gyro_divider
+
+        # Get magnetometer values
+        for i in range(7, 10):
+            data = self.byte_to_float_le(response[i*2:i*2+2])
+            self.magnetometer_data[i-7] = data * self.magnetometer_ASA[i-7]
+
+# -----------------------------------------------------------------------------------------------
+#                                          GET VALUES
+# usage: call this functions to read and get values
+# returns accel, gyro and mag values
+# -----------------------------------------------------------------------------------------------
+
+    def getMotion9(self):
+        self.read_all()
+        m9a = self.accelerometer_data
+        m9g = self.gyroscope_data
+        m9m = self.magnetometer_data
+
+        return m9a, m9g, m9m
+
+# -----------------------------------------------------------------------------------------------
+
+    def getMotion6(self):
+        self.read_acc()
+        self.read_gyro()
+
+        m6a = self.accelerometer_data
+        m6g = self.gyroscope_data
+
+        return m6a, m6g
+
+# -----------------------------------------------------------------------------------------------
+
+    def byte_to_float(self, input_buffer):
+        byte_array = array.array("B", input_buffer)
+        signed_16_bit_int, = struct.unpack(">h", byte_array)
+        return float(signed_16_bit_int)
+
+# -----------------------------------------------------------------------------------------------
+
+    def byte_to_float_le(self, input_buffer):
+        byte_array = array.array("B", input_buffer)
+        signed_16_bit_int, = struct.unpack("<h", byte_array)
+        return float(signed_16_bit_int)
+
+
+
+

darkwater_640/smbus.py

@@ -0,0 +1,294 @@
+# Copyright (c) 2016 Adafruit Industries
+# Author: Tony DiCola
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+from ctypes import *
+from fcntl import ioctl
+import struct
+
+# I2C C API constants (from linux kernel headers)
+I2C_M_TEN             = 0x0010  # this is a ten bit chip address
+I2C_M_RD              = 0x0001  # read data, from slave to master
+I2C_M_STOP            = 0x8000  # if I2C_FUNC_PROTOCOL_MANGLING
+I2C_M_NOSTART         = 0x4000  # if I2C_FUNC_NOSTART
+I2C_M_REV_DIR_ADDR    = 0x2000  # if I2C_FUNC_PROTOCOL_MANGLING
+I2C_M_IGNORE_NAK      = 0x1000  # if I2C_FUNC_PROTOCOL_MANGLING
+I2C_M_NO_RD_ACK       = 0x0800  # if I2C_FUNC_PROTOCOL_MANGLING
+I2C_M_RECV_LEN        = 0x0400  # length will be first received byte
+
+I2C_SLAVE             = 0x0703  # Use this slave address
+I2C_SLAVE_FORCE       = 0x0706  # Use this slave address, even if
+                                # is already in use by a driver!
+I2C_TENBIT            = 0x0704  # 0 for 7 bit addrs, != 0 for 10 bit
+I2C_FUNCS             = 0x0705  # Get the adapter functionality mask
+I2C_RDWR              = 0x0707  # Combined R/W transfer (one STOP only)
+I2C_PEC               = 0x0708  # != 0 to use PEC with SMBus
+I2C_SMBUS             = 0x0720  # SMBus transfer
+
+
+# ctypes versions of I2C structs defined by kernel.
+class i2c_msg(Structure):
+    _fields_ = [
+        ('addr',  c_uint16),
+        ('flags', c_uint16),
+        ('len',   c_uint16),
+        ('buf',   POINTER(c_uint8))
+    ]
+
+class i2c_rdwr_ioctl_data(Structure):
+    _fields_ = [
+        ('msgs',  POINTER(i2c_msg)),
+        ('nmsgs', c_uint32)
+    ]
+
+
+def make_i2c_rdwr_data(messages):
+    """Utility function to create and return an i2c_rdwr_ioctl_data structure
+    populated with a list of specified I2C messages.  The messages parameter
+    should be a list of tuples which represent the individual I2C messages to
+    send in this transaction.  Tuples should contain 4 elements: address value,
+    flags value, buffer length, ctypes c_uint8 pointer to buffer.
+    """
+    # Create message array and populate with provided data.
+    msg_data_type = i2c_msg*len(messages)
+    msg_data = msg_data_type()
+    for i, m in enumerate(messages):
+        msg_data[i].addr  = m[0] & 0x7F
+        msg_data[i].flags = m[1]
+        msg_data[i].len   = m[2]
+        msg_data[i].buf   = m[3]
+    # Now build the data structure.
+    data = i2c_rdwr_ioctl_data()
+    data.msgs  = msg_data
+    data.nmsgs = len(messages)
+    return data
+
+
+# Create an interface that mimics the Python SMBus API.
+class SMBus(object):
+    """I2C interface that mimics the Python SMBus API but is implemented with
+    pure Python calls to ioctl and direct /dev/i2c device access.
+    """
+
+    def __init__(self, bus=None):
+        """Create a new smbus instance.  Bus is an optional parameter that
+        specifies the I2C bus number to use, for example 1 would use device
+        /dev/i2c-1.  If bus is not specified then the open function should be
+        called to open the bus.
+        """
+        self._device = None
+        if bus is not None:
+            self.open(bus)
+
+    def __del__(self):
+        """Clean up any resources used by the SMBus instance."""
+        self.close()
+
+    def __enter__(self):
+        """Context manager enter function."""
+        # Just return this object so it can be used in a with statement, like
+        # with SMBus(1) as bus:
+        #     # do stuff!
+        return self
+
+    def __exit__(self, exc_type, exc_val, exc_tb):
+        """Context manager exit function, ensures resources are cleaned up."""
+        self.close()
+        return False  # Don't suppress exceptions.
+
+    def open(self, bus):
+        """Open the smbus interface on the specified bus."""
+        # Close the device if it's already open.
+        if self._device is not None:
+            self.close()
+        # Try to open the file for the specified bus.  Must turn off buffering
+        # or else Python 3 fails (see: https://bugs.python.org/issue20074)
+        self._device = open('/dev/i2c-{0}'.format(bus), 'r+b', buffering=0)
+        # TODO: Catch IOError and throw a better error message that describes
+        # what's wrong (i.e. I2C may not be enabled or the bus doesn't exist).
+
+    def close(self):
+        """Close the smbus connection.  You cannot make any other function
+        calls on the bus unless open is called!"""
+        if self._device is not None:
+            self._device.close()
+            self._device = None
+
+    def _select_device(self, addr):
+        """Set the address of the device to communicate with on the I2C bus."""
+        ioctl(self._device.fileno(), I2C_SLAVE, addr & 0x7F)
+
+    def read_byte(self, addr):
+        """Read a single byte from the specified device."""
+        assert self._device is not None, 'Bus must be opened before operations are made against it!'
+        self._select_device(addr)
+        return ord(self._device.read(1))
+
+    def read_byte_data(self, addr, cmd):
+        """Read a single byte from the specified cmd register of the device."""
+        assert self._device is not None, 'Bus must be opened before operations are made against it!'
+        # Build ctypes values to marshall between ioctl and Python.
+        reg = c_uint8(cmd)
+        result = c_uint8()
+        # Build ioctl request.
+        request = make_i2c_rdwr_data([
+            (addr, 0, 1, pointer(reg)),             # Write cmd register.
+            (addr, I2C_M_RD, 1, pointer(result))    # Read 1 byte as result.
+        ])
+        # Make ioctl call and return result data.
+        ioctl(self._device.fileno(), I2C_RDWR, request)
+        return result.value
+
+    def read_word_data(self, addr, cmd):
+        """Read a word (2 bytes) from the specified cmd register of the device.
+        Note that this will interpret data using the endianness of the processor
+        running Python (typically little endian)!
+        """
+        assert self._device is not None, 'Bus must be opened before operations are made against it!'
+        # Build ctypes values to marshall between ioctl and Python.
+        reg = c_uint8(cmd)
+        result = c_uint16()
+        # Build ioctl request.
+        request = make_i2c_rdwr_data([
+            (addr, 0, 1, pointer(reg)),             # Write cmd register.
+            (addr, I2C_M_RD, 2, cast(pointer(result), POINTER(c_uint8)))   # Read word (2 bytes).
+        ])
+        # Make ioctl call and return result data.
+        ioctl(self._device.fileno(), I2C_RDWR, request)
+        return result.value
+
+    def read_block_data(self, addr, cmd):
+        """Perform a block read from the specified cmd register of the device.
+        The amount of data read is determined by the first byte send back by
+        the device.  Data is returned as a bytearray.
+        """
+        # TODO: Unfortunately this will require calling the low level I2C
+        # access ioctl to trigger a proper read_block_data.  The amount of data
+        # returned isn't known until the device starts responding so an I2C_RDWR
+        # ioctl won't work.
+        raise NotImplementedError()
+
+    def read_i2c_block_data(self, addr, cmd, length=32):
+        """Perform a read from the specified cmd register of device.  Length number
+        of bytes (default of 32) will be read and returned as a bytearray.
+        """
+        assert self._device is not None, 'Bus must be opened before operations are made against it!'
+        # Build ctypes values to marshall between ioctl and Python.
+        reg = c_uint8(cmd)
+        result = create_string_buffer(length)
+        # Build ioctl request.
+        request = make_i2c_rdwr_data([
+            (addr, 0, 1, pointer(reg)),             # Write cmd register.
+            (addr, I2C_M_RD, length, cast(result, POINTER(c_uint8)))   # Read data.
+        ])
+        # Make ioctl call and return result data.
+        ioctl(self._device.fileno(), I2C_RDWR, request)
+        return bytearray(result.raw)  # Use .raw instead of .value which will stop at a null byte!
+
+    def write_quick(self, addr):
+        """Write a single byte to the specified device."""
+        # What a strange function, from the python-smbus source this appears to
+        # just write a single byte that initiates a write to the specified device
+        # address (but writes no data!).  The functionality is duplicated below
+        # but the actual use case for this is unknown.
+        assert self._device is not None, 'Bus must be opened before operations are made against it!'
+        # Build ioctl request.
+        request = make_i2c_rdwr_data([
+            (addr, 0, 0, None),  # Write with no data.
+        ])
+        # Make ioctl call and return result data.
+        ioctl(self._device.fileno(), I2C_RDWR, request)
+
+    def write_byte(self, addr, val):
+        """Write a single byte to the specified device."""
+        assert self._device is not None, 'Bus must be opened before operations are made against it!'
+        self._select_device(addr)
+        data = bytearray(1)
+        data[0] = val & 0xFF
+        self._device.write(data)
+
+    def write_byte_data(self, addr, cmd, val):
+        """Write a byte of data to the specified cmd register of the device.
+        """
+        assert self._device is not None, 'Bus must be opened before operations are made against it!'
+        # Construct a string of data to send with the command register and byte value.
+        data = bytearray(2)
+        data[0] = cmd & 0xFF
+        data[1] = val & 0xFF
+        # Send the data to the device.
+        self._select_device(addr)
+        self._device.write(data)
+
+    def write_word_data(self, addr, cmd, val):
+        """Write a word (2 bytes) of data to the specified cmd register of the
+        device.  Note that this will write the data in the endianness of the
+        processor running Python (typically little endian)!
+        """
+        assert self._device is not None, 'Bus must be opened before operations are made against it!'
+        # Construct a string of data to send with the command register and word value.
+        data = struct.pack('=BH', cmd & 0xFF, val & 0xFFFF)
+        # Send the data to the device.
+        self._select_device(addr)
+        self._device.write(data)
+
+    def write_block_data(self, addr, cmd, vals):
+        """Write a block of data to the specified cmd register of the device.
+        The amount of data to write should be the first byte inside the vals
+        string/bytearray and that count of bytes of data to write should follow
+        it.
+        """
+        # Just use the I2C block data write to write the provided values and
+        # their length as the first byte.
+        data = bytearray(len(vals)+1)
+        data[0] = len(vals) & 0xFF
+        data[1:] = vals[0:]
+        self.write_i2c_block_data(addr, cmd, data)
+
+    def write_i2c_block_data(self, addr, cmd, vals):
+        """Write a buffer of data to the specified cmd register of the device.
+        """
+        assert self._device is not None, 'Bus must be opened before operations are made against it!'
+        # Construct a string of data to send, including room for the command register.
+        data = bytearray(len(vals)+1)
+        data[0] = cmd & 0xFF  # Command register at the start.
+        data[1:] = vals[0:]   # Copy in the block data (ugly but necessary to ensure
+                              # the entire write happens in one transaction).
+        # Send the data to the device.
+        self._select_device(addr)
+        self._device.write(data)
+
+    def process_call(self, addr, cmd, val):
+        """Perform a smbus process call by writing a word (2 byte) value to
+        the specified register of the device, and then reading a word of response
+        data (which is returned).
+        """
+        assert self._device is not None, 'Bus must be opened before operations are made against it!'
+        # Build ctypes values to marshall between ioctl and Python.
+        data = create_string_buffer(struct.pack('=BH', cmd, val))
+        result = c_uint16()
+        # Build ioctl request.
+        request = make_i2c_rdwr_data([
+            (addr, 0, 3, cast(pointer(data), POINTER(c_uint8))),          # Write data.
+            (addr, I2C_M_RD, 2, cast(pointer(result), POINTER(c_uint8)))  # Read word (2 bytes).
+        ])
+        # Make ioctl call and return result data.
+        ioctl(self._device.fileno(), I2C_RDWR, request)
+        # Note the python-smbus code appears to have a rather serious bug and
+        # does not return the result value!  This is fixed below by returning it.
+        return result.value

dw640HAT/Adafruit_I2C.py

@@ -1,161 +0,0 @@
-#!/usr/bin/python
-import re
-import smbus
-
-# ===========================================================================
-# Adafruit_I2C Class
-# ===========================================================================
-
-class Adafruit_I2C(object):
-
-  @staticmethod
-  def getPiRevision():
-    "Gets the version number of the Raspberry Pi board"
-    # Revision list available at: http://elinux.org/RPi_HardwareHistory#Board_Revision_History
-    try:
-      with open('/proc/cpuinfo', 'r') as infile:
-        for line in infile:
-          # Match a line of the form "Revision : 0002" while ignoring extra
-          # info in front of the revsion (like 1000 when the Pi was over-volted).
-          match = re.match('Revision\s+:\s+.*(\w{4})$', line)
-          if match and match.group(1) in ['0000', '0002', '0003']:
-            # Return revision 1 if revision ends with 0000, 0002 or 0003.
-            return 1
-          elif match:
-            # Assume revision 2 if revision ends with any other 4 chars.
-            return 2
-        # Couldn't find the revision, assume revision 0 like older code for compatibility.
-        return 0
-    except:
-      return 0
-
-  @staticmethod
-  def getPiI2CBusNumber():
-    # Gets the I2C bus number /dev/i2c#
-    return 1 if Adafruit_I2C.getPiRevision() > 1 else 0
-
-  def __init__(self, address, busnum=-1, debug=False):
-    self.address = address
-    # By default, the correct I2C bus is auto-detected using /proc/cpuinfo
-    # Alternatively, you can hard-code the bus version below:
-    # self.bus = smbus.SMBus(0); # Force I2C0 (early 256MB Pi's)
-    # self.bus = smbus.SMBus(1); # Force I2C1 (512MB Pi's)
-    self.bus = smbus.SMBus(busnum if busnum >= 0 else Adafruit_I2C.getPiI2CBusNumber())
-    self.debug = debug
-
-  def reverseByteOrder(self, data):
-    "Reverses the byte order of an int (16-bit) or long (32-bit) value"
-    # Courtesy Vishal Sapre
-    byteCount = len(hex(data)[2:].replace('L','')[::2])
-    val       = 0
-    for i in range(byteCount):
-      val    = (val << 8) | (data & 0xff)
-      data >>= 8
-    return val
-
-  def errMsg(self):
-    print "Error accessing 0x%02X: Check your I2C address" % self.address
-    return -1
-
-  def write8(self, reg, value):
-    "Writes an 8-bit value to the specified register/address"
-    try:
-      self.bus.write_byte_data(self.address, reg, value)
-      if self.debug:
-        print "I2C: Wrote 0x%02X to register 0x%02X" % (value, reg)
-    except IOError, err:
-      return self.errMsg()
-
-  def write16(self, reg, value):
-    "Writes a 16-bit value to the specified register/address pair"
-    try:
-      self.bus.write_word_data(self.address, reg, value)
-      if self.debug:
-        print ("I2C: Wrote 0x%02X to register pair 0x%02X,0x%02X" %
-         (value, reg, reg+1))
-    except IOError, err:
-      return self.errMsg()
-
-  def writeRaw8(self, value):
-    "Writes an 8-bit value on the bus"
-    try:
-      self.bus.write_byte(self.address, value)
-      if self.debug:
-        print "I2C: Wrote 0x%02X" % value
-    except IOError, err:
-      return self.errMsg()
-
-  def writeList(self, reg, list):
-    "Writes an array of bytes using I2C format"
-    try:
-      if self.debug:
-        print "I2C: Writing list to register 0x%02X:" % reg
-        print list
-      self.bus.write_i2c_block_data(self.address, reg, list)
-    except IOError, err:
-      return self.errMsg()
-
-  def readList(self, reg, length):
-    "Read a list of bytes from the I2C device"
-    try:
-      results = self.bus.read_i2c_block_data(self.address, reg, length)
-      if self.debug:
-        print ("I2C: Device 0x%02X returned the following from reg 0x%02X" %
-         (self.address, reg))
-        print results
-      return results
-    except IOError, err:
-      return self.errMsg()
-
-  def readU8(self, reg):
-    "Read an unsigned byte from the I2C device"
-    try:
-      result = self.bus.read_byte_data(self.address, reg)
-      if self.debug:
-        print ("I2C: Device 0x%02X returned 0x%02X from reg 0x%02X" %
-         (self.address, result & 0xFF, reg))
-      return result
-    except IOError, err:
-      return self.errMsg()
-
-  def readS8(self, reg):
-    "Reads a signed byte from the I2C device"
-    try:
-      result = self.bus.read_byte_data(self.address, reg)
-      if result > 127: result -= 256
-      if self.debug:
-        print ("I2C: Device 0x%02X returned 0x%02X from reg 0x%02X" %
-         (self.address, result & 0xFF, reg))
-      return result
-    except IOError, err:
-      return self.errMsg()
-
-  def readU16(self, reg, little_endian=True):
-    "Reads an unsigned 16-bit value from the I2C device"
-    try:
-      result = self.bus.read_word_data(self.address,reg)
-      # Swap bytes if using big endian because read_word_data assumes little 
-      # endian on ARM (little endian) systems.
-      if not little_endian:
-        result = ((result << 8) & 0xFF00) + (result >> 8)
-      if (self.debug):
-        print "I2C: Device 0x%02X returned 0x%04X from reg 0x%02X" % (self.address, result & 0xFFFF, reg)
-      return result
-    except IOError, err:
-      return self.errMsg()
-
-  def readS16(self, reg, little_endian=True):
-    "Reads a signed 16-bit value from the I2C device"
-    try:
-      result = self.readU16(reg,little_endian)
-      if result > 32767: result -= 65536
-      return result
-    except IOError, err:
-      return self.errMsg()
-
-if __name__ == '__main__':
-  try:
-    bus = Adafruit_I2C(address=0)
-    print "Default I2C bus is accessible"
-  except:
-    print "Error accessing default I2C bus"

dw640HAT/Adafruit_PWM_Servo_Driver.py

@@ -1,92 +0,0 @@
-#!/usr/bin/python
-
-import time
-import math
-from Adafruit_I2C import Adafruit_I2C
-
-# ============================================================================
-# Adafruit PCA9685 16-Channel PWM Servo Driver
-# ============================================================================
-
-class PWM :
-  # Registers/etc.
-  __MODE1              = 0x00
-  __MODE2              = 0x01
-  __SUBADR1            = 0x02
-  __SUBADR2            = 0x03
-  __SUBADR3            = 0x04
-  __PRESCALE           = 0xFE
-  __LED0_ON_L          = 0x06
-  __LED0_ON_H          = 0x07
-  __LED0_OFF_L         = 0x08
-  __LED0_OFF_H         = 0x09
-  __ALL_LED_ON_L       = 0xFA
-  __ALL_LED_ON_H       = 0xFB
-  __ALL_LED_OFF_L      = 0xFC
-  __ALL_LED_OFF_H      = 0xFD
-
-  # Bits
-  __RESTART            = 0x80
-  __SLEEP              = 0x10
-  __ALLCALL            = 0x01
-  __INVRT              = 0x10
-  __OUTDRV             = 0x04
-
-  general_call_i2c = Adafruit_I2C(0x00)
-
-  @classmethod
-  def softwareReset(cls):
-    "Sends a software reset (SWRST) command to all the servo drivers on the bus"
-    cls.general_call_i2c.writeRaw8(0x06)        # SWRST
-
-  def __init__(self, address=0x40, debug=False):
-    self.i2c = Adafruit_I2C(address)
-    self.i2c.debug = debug
-    self.address = address
-    self.debug = debug
-    if (self.debug):
-      print "Reseting PCA9685 MODE1 (without SLEEP) and MODE2"
-    self.setAllPWM(0, 0)
-    self.i2c.write8(self.__MODE2, self.__OUTDRV)
-    self.i2c.write8(self.__MODE1, self.__ALLCALL)
-    time.sleep(0.005)                                       # wait for oscillator
-    
-    mode1 = self.i2c.readU8(self.__MODE1)
-    mode1 = mode1 & ~self.__SLEEP                 # wake up (reset sleep)
-    self.i2c.write8(self.__MODE1, mode1)
-    time.sleep(0.005)                             # wait for oscillator
-
-  def setPWMFreq(self, freq):
-    "Sets the PWM frequency"
-    prescaleval = 25000000.0    # 25MHz
-    prescaleval /= 4096.0       # 12-bit
-    prescaleval /= float(freq)
-    prescaleval -= 1.0
-    if (self.debug):
-      print "Setting PWM frequency to %d Hz" % freq
-      print "Estimated pre-scale: %d" % prescaleval
-    prescale = math.floor(prescaleval + 0.5)
-    if (self.debug):
-      print "Final pre-scale: %d" % prescale
-
-    oldmode = self.i2c.readU8(self.__MODE1);
-    newmode = (oldmode & 0x7F) | 0x10             # sleep
-    self.i2c.write8(self.__MODE1, newmode)        # go to sleep
-    self.i2c.write8(self.__PRESCALE, int(math.floor(prescale)))
-    self.i2c.write8(self.__MODE1, oldmode)
-    time.sleep(0.005)
-    self.i2c.write8(self.__MODE1, oldmode | 0x80)
-
-  def setPWM(self, channel, on, off):
-    "Sets a single PWM channel"
-    self.i2c.write8(self.__LED0_ON_L+4*channel, on & 0xFF)
-    self.i2c.write8(self.__LED0_ON_H+4*channel, on >> 8)
-    self.i2c.write8(self.__LED0_OFF_L+4*channel, off & 0xFF)
-    self.i2c.write8(self.__LED0_OFF_H+4*channel, off >> 8)
-
-  def setAllPWM(self, on, off):
-    "Sets a all PWM channels"
-    self.i2c.write8(self.__ALL_LED_ON_L, on & 0xFF)
-    self.i2c.write8(self.__ALL_LED_ON_H, on >> 8)
-    self.i2c.write8(self.__ALL_LED_OFF_L, off & 0xFF)
-    self.i2c.write8(self.__ALL_LED_OFF_H, off >> 8)

dw640HAT/__init__.py

@@ -1 +0,0 @@
-from .dw640HAT import dw_DCMotor, dw_MotorCONTROL

dw640HAT/dw640HAT.py

@@ -1,282 +0,0 @@
-#!/usr/bin/python
-
-import RPi.GPIO as GPIO
-from Adafruit_PWM_Servo_Driver import PWM
-import time
-
-# class Adafruit_StepperMotor:
-#       MICROSTEPS = 8
-#       MICROSTEP_CURVE = [0, 50, 98, 142, 180, 212, 236, 250, 255]
-#
-#       #MICROSTEPS = 16
-#       # a sinusoidal curve NOT LINEAR!
-#       #MICROSTEP_CURVE = [0, 25, 50, 74, 98, 120, 141, 162, 180, 197, 212, 225, 236, 244, 250, 253, 255]
-#
-#       def __init__(self, controller, num, steps=200):
-#               self.MC = controller
-#               self.revsteps = steps
-#               self.motornum = num
-#               self.sec_per_step = 0.1
-#               self.steppingcounter = 0
-#               self.currentstep = 0
-#
-#               num -= 1
-#
-#               if (num == 0):
-#                       self.PWMA = 8
-#                       self.AIN2 = 9
-#                       self.AIN1 = 10
-#                       self.PWMB = 13
-#                       self.BIN2 = 12
-#                       self.BIN1 = 11
-#               elif (num == 1):
-#                       self.PWMA = 2
-#                       self.AIN2 = 3
-#                       self.AIN1 = 4
-#                       self.PWMB = 7
-#                       self.BIN2 = 6
-#                       self.BIN1 = 5
-#               else:
-#                       raise NameError('MotorHAT Stepper must be between 1 and 2 inclusive')
-#
-#       def setSpeed(self, rpm):
-#               self.sec_per_step = 60.0 / (self.revsteps * rpm)
-#               self.steppingcounter = 0
-#
-#       def oneStep(self, dir, style):
-#               pwm_a = pwm_b = 255
-#
-#               # first determine what sort of stepping procedure we're up to
-#               if (style == Adafruit_MotorHAT.SINGLE):
-#                       if ((self.currentstep/(self.MICROSTEPS/2)) % 2):
-#                               # we're at an odd step, weird
-#                               if (dir == Adafruit_MotorHAT.FORWARD):
-#                                       self.currentstep += self.MICROSTEPS/2
-#                               else:
-#                                       self.currentstep -= self.MICROSTEPS/2
-#                       else:
-#                               # go to next even step
-#                               if (dir == Adafruit_MotorHAT.FORWARD):
-#                                       self.currentstep += self.MICROSTEPS
-#                               else:
-#                                       self.currentstep -= self.MICROSTEPS
-#               if (style == Adafruit_MotorHAT.DOUBLE):
-#                       if not (self.currentstep/(self.MICROSTEPS/2) % 2):
-#                               # we're at an even step, weird
-#                               if (dir == Adafruit_MotorHAT.FORWARD):
-#                                       self.currentstep += self.MICROSTEPS/2
-#                               else:
-#                                       self.currentstep -= self.MICROSTEPS/2
-#                       else:
-#                               # go to next odd step
-#                               if (dir == Adafruit_MotorHAT.FORWARD):
-#                                       self.currentstep += self.MICROSTEPS
-#                               else:
-#                                       self.currentstep -= self.MICROSTEPS
-#               if (style == Adafruit_MotorHAT.INTERLEAVE):
-#                       if (dir == Adafruit_MotorHAT.FORWARD):
-#                               self.currentstep += self.MICROSTEPS/2
-#                       else:
-#                               self.currentstep -= self.MICROSTEPS/2
-#
-#               if (style == Adafruit_MotorHAT.MICROSTEP):
-#                       if (dir == Adafruit_MotorHAT.FORWARD):
-#                               self.currentstep += 1
-#                       else:
-#                               self.currentstep -= 1
-#
-#                       # go to next 'step' and wrap around
-#                       self.currentstep += self.MICROSTEPS * 4
-#                       self.currentstep %= self.MICROSTEPS * 4
-#
-#                       pwm_a = pwm_b = 0
-#                       if (self.currentstep >= 0) and (self.currentstep < self.MICROSTEPS):
-#                               pwm_a = self.MICROSTEP_CURVE[self.MICROSTEPS - self.currentstep]
-#                               pwm_b = self.MICROSTEP_CURVE[self.currentstep]
-#                       elif (self.currentstep >= self.MICROSTEPS) and (self.currentstep < self.MICROSTEPS*2):
-#                               pwm_a = self.MICROSTEP_CURVE[self.currentstep - self.MICROSTEPS]
-#                               pwm_b = self.MICROSTEP_CURVE[self.MICROSTEPS*2 - self.currentstep]
-#                       elif (self.currentstep >= self.MICROSTEPS*2) and (self.currentstep < self.MICROSTEPS*3):
-#                               pwm_a = self.MICROSTEP_CURVE[self.MICROSTEPS*3 - self.currentstep]
-#                               pwm_b = self.MICROSTEP_CURVE[self.currentstep - self.MICROSTEPS*2]
-#                       elif (self.currentstep >= self.MICROSTEPS*3) and (self.currentstep < self.MICROSTEPS*4):
-#                                 pwm_a = self.MICROSTEP_CURVE[self.currentstep - self.MICROSTEPS*3]
-#                                 pwm_b = self.MICROSTEP_CURVE[self.MICROSTEPS*4 - self.currentstep]
-#
-#
-#               # go to next 'step' and wrap around
-#               self.currentstep += self.MICROSTEPS * 4
-#               self.currentstep %= self.MICROSTEPS * 4
-#
-#               # only really used for microstepping, otherwise always on!
-#               self.MC._pwm.setPWM(self.PWMA, 0, pwm_a*16)
-#               self.MC._pwm.setPWM(self.PWMB, 0, pwm_b*16)
-#
-#               # set up coil energizing!
-#               coils = [0, 0, 0, 0]
-#
-#               if (style == Adafruit_MotorHAT.MICROSTEP):
-#                       if (self.currentstep >= 0) and (self.currentstep < self.MICROSTEPS):
-#                               coils = [1, 1, 0, 0]
-#                         elif (self.currentstep >= self.MICROSTEPS) and (self.currentstep < self.MICROSTEPS*2):
-#                               coils = [0, 1, 1, 0]
-#                         elif (self.currentstep >= self.MICROSTEPS*2) and (self.currentstep < self.MICROSTEPS*3):
-#                               coils = [0, 0, 1, 1]
-#                         elif (self.currentstep >= self.MICROSTEPS*3) and (self.currentstep < self.MICROSTEPS*4):
-#                               coils = [1, 0, 0, 1]
-#               else:
-#                       step2coils = [  [1, 0, 0, 0],
-#                               [1, 1, 0, 0],
-#                               [0, 1, 0, 0],
-#                               [0, 1, 1, 0],
-#                               [0, 0, 1, 0],
-#                               [0, 0, 1, 1],
-#                               [0, 0, 0, 1],
-#                               [1, 0, 0, 1] ]
-#                       coils = step2coils[self.currentstep/(self.MICROSTEPS/2)]
-#
-#               #print "coils state = " + str(coils)
-#               self.MC.setPin(self.AIN2, coils[0])
-#               self.MC.setPin(self.BIN1, coils[1])
-#               self.MC.setPin(self.AIN1, coils[2])
-#               self.MC.setPin(self.BIN2, coils[3])
-#
-#               return self.currentstep
-#
-#       def step(self, steps, direction, stepstyle):
-#               s_per_s = self.sec_per_step
-#               lateststep = 0
-#
-#               if (stepstyle == Adafruit_MotorHAT.INTERLEAVE):
-#                       s_per_s = s_per_s / 2.0
-#               if (stepstyle == Adafruit_MotorHAT.MICROSTEP):
-#                       s_per_s /= self.MICROSTEPS
-#                       steps *= self.MICROSTEPS
-#
-#               print s_per_s, " sec per step"
-#
-#               for s in range(steps):
-#                       lateststep = self.oneStep(direction, stepstyle)
-#                       time.sleep(s_per_s)
-#
-#               if (stepstyle == Adafruit_MotorHAT.MICROSTEP):
-#                       # this is an edge case, if we are in between full steps, lets just keep going
-#                       # so we end on a full step
-#                       while (lateststep != 0) and (lateststep != self.MICROSTEPS):
-#                               lateststep = self.oneStep(dir, stepstyle)
-#                               time.sleep(s_per_s)
-
-class dw_DCMotor:
-        def __init__(self, controller, num):
-                self.speed = 0
-                self.MC = controller
-                self.motornum = num
-                modepin = in1 = in2 = 0
-
-                if (num == 0):
-                         in2 = 8  #phase
-                         in1 = 9  #enable
-                elif (num == 1):
-                         in2 = 10 #phase
-                         in1 = 11 #enable
-                elif (num == 2):
-                         in2 = 7  #phase
-                         in1 = 6  #enable
-                elif (num == 3):
-                         in2 = 5  #phase
-                         in1 = 4  #enable
-                elif (num == 4):
-                         in2 = 0  #phase
-                         in1 = 1  #enable
-                elif (num == 5):
-                         in2 = 2  #phase
-                         in1 = 3  #enable
-                else:
-                        raise NameError('MotorHAT Motor must be between 1 and 6 inclusive')
-
-                self.PHpin = in2
-                self.ENpin = in1
-                # switch off
-                self.run(dw_MotorCONTROL.RELEASE, 0)
-
-        def setMotorSpeed(self, value):
-                if(value > 0) and (value <= 255):
-                        self.run(dw_MotorCONTROL.FORWARD, value)
-                if(value == 0):
-                        self.run(dw_MotorCONTROL.RELEASE, value)
-                if(value < 0) and (value >= -255):
-                        self.run(dw_MotorCONTROL.BACKWARD, abs(value))
-
-        def run(self, command, speed = 0):
-                if not self.MC:
-                        return
-                if (command == dw_MotorCONTROL.FORWARD):
-                        self.MC.setPin(self.PHpin, 0)
-                        self.MC._pwm.setPWM(self.ENpin, 0, speed*16)
-                if (command == dw_MotorCONTROL.BACKWARD):
-                        self.MC.setPin(self.PHpin, 1)
-                        self.MC._pwm.setPWM(self.ENpin, 0, speed*16)
-                if (command == dw_MotorCONTROL.RELEASE):
-                        self.MC.setPin(self.PHpin, 0)
-                        self.MC.setPin(self.ENpin, 0)
-
-        def setSpeed(self, speed):
-                if (speed < 0):
-                        speed = 0
-                if (speed > 255):
-                        speed = 255
-                self.MC._pwm.setPWM(self.ENpin, 0, speed*16)
-
-class dw_MotorCONTROL:
-        FORWARD = 1
-        BACKWARD = 2
-        BRAKE = 3
-        RELEASE = 4
-
-        SINGLE = 1
-        DOUBLE = 2
-        INTERLEAVE = 3
-        MICROSTEP = 4
-
-        ININ = 0
-        PHASE = 1
-
-        def __init__(self, addr = 0x60, freq = 1600):
-                self._i2caddr = addr            # default addr on HAT
-                self._frequency = freq          # default @1600Hz PWM freq
-                # self.steppers = [ Adafruit_StepperMotor(self, 1), Adafruit_StepperMotor(self, 2) ]
-                self._pwm =  PWM(addr, debug=False)
-                self._pwm.setPWMFreq(self._frequency)
-                # Just gonna default to high for now
-                GPIO.setmode(GPIO.BCM)
-
-                GPIO.setwarnings(False)
-                
-                GPIO.setup(17, GPIO.OUT)
-                GPIO.setup(27, GPIO.OUT)
-                GPIO.setup(22, GPIO.OUT)
-
-                GPIO.output(17, GPIO.HIGH)
-                GPIO.output(27, GPIO.HIGH)
-                GPIO.output(22, GPIO.HIGH)
-
-                self.motors = [ dw_DCMotor(self, m) for m in range(6) ]
-
-        def setPin(self, pin, value):
-                if (pin < 0) or (pin > 15):
-                        raise NameError('PWM pin must be between 0 and 15 inclusive')
-                if (value != 0) and (value != 1):
-                        raise NameError('Pin value must be 0 or 1!')
-                if (value == 0):
-                        self._pwm.setPWM(pin, 0, 4096)
-                if (value == 1):
-                        self._pwm.setPWM(pin, 4096, 0)
-
-        def getMotor(self, num):
-                if (num < 1) or (num > 6):
-                        raise NameError('MotorHAT Motor must be between 1 and 6 inclusive')
-                return self.motors[num-1]
-
-        def allOff(self):
-                for y in range(5):
-                        self.motors[y].run(dw_MotorCONTROL.RELEASE, 0)

examples/640imutest.py

@@ -0,0 +1,65 @@
+"""
+MS5611 driver code is placed under the BSD license.
+Copyright (c) 2014, Emlid Limited, www.emlid.com
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+	* Redistributions of source code must retain the above copyright
+	notice, this list of conditions and the following disclaimer.
+	* Redistributions in binary form must reproduce the above copyright
+	notice, this list of conditions and the following disclaimer in the
+	documentation and/or other materials provided with the distribution.
+	* Neither the name of the Emlid Limited nor the names of its contributors
+	may be used to endorse or promote products derived from this software
+	without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL EMLID LIMITED BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+"""
+
+import spidev
+import time
+import sys
+from darkwater_640 import MPU9250
+
+imu = MPU9250()
+
+if imu.testConnection():
+    print("Connection established: True")
+else: 
+    sys.exit("Connection established: False")
+
+imu.initialize()
+
+time.sleep(1)
+
+while True:
+	# imu.read_all()
+	# imu.read_gyro()
+	# imu.read_acc()
+	# imu.read_temp()
+	# imu.read_mag()
+
+	# print "Accelerometer: ", imu.accelerometer_data
+	# print "Gyroscope:     ", imu.gyroscope_data
+	# print "Temperature:   ", imu.temperature
+	# print "Magnetometer:  ", imu.magnetometer_data
+
+	# time.sleep(0.1)
+
+	m9a, m9g, m9m = imu.getMotion9()
+
+	print("Acc:", "{:+7.3f}".format(m9a[0]), "{:+7.3f}".format(m9a[1]), "{:+7.3f}".format(m9a[2]) )
+	print("Gyr:", "{:+8.3f}".format(m9g[0]), "{:+8.3f}".format(m9g[1]), "{:+8.3f}".format(m9g[2]) )
+	print("Mag:", "{:+7.3f}".format(m9m[0]), "{:+7.3f}".format(m9m[1]), "{:+7.3f}".format(m9m[2]) )
+
+	time.sleep(0.5)

examples/640motortest.py

@@ -0,0 +1,84 @@
+import time
+from darkwater_640 import dw_Controller, dw_Motor
+
+dw = dw_Controller( addr=0x60 )
+m1 = dw.getMotor(1)
+m2 = dw.getMotor(2)
+m3 = dw.getMotor(3)
+m4 = dw.getMotor(4)
+m5 = dw.getMotor(5)
+m6 = dw.getMotor(6)
+
+m1.off()
+m2.off()
+m3.off()
+m4.off()
+m5.off()
+m6.off()
+time.sleep(1)
+
+##time.sleep(10)
+print("Set forward - ")
+print("Motor 1")
+m1.setMotorSpeed(255)
+time.sleep(1)
+print("Motor 2")
+m2.setMotorSpeed(255)
+time.sleep(1)
+print("Motor 3")
+m3.setMotorSpeed(255)
+time.sleep(1)
+print("Motor 4")
+m4.setMotorSpeed(255)
+time.sleep(1)
+print("Motor 5")
+m5.setMotorSpeed(255)
+time.sleep(1)
+print("Motor 6")
+m6.setMotorSpeed(255)
+time.sleep(1)
+print("Stopping - ")
+print("Motor 1")
+m1.setMotorSpeed(0)
+time.sleep(1)
+print("Motor 2")
+m2.setMotorSpeed(0)
+time.sleep(1)
+print("Motor 3")
+m3.setMotorSpeed(0)
+time.sleep(1)
+print("Motor 4")
+m4.setMotorSpeed(0)
+time.sleep(1)
+print("Motor 5")
+m5.setMotorSpeed(0)
+time.sleep(1)
+print("Motor 6")
+m6.setMotorSpeed(0)
+time.sleep(1)
+print("Set reverse - ")
+print("Motor 1")
+m1.setMotorSpeed(-255)
+time.sleep(1)
+print("Motor 2")
+m2.setMotorSpeed(-255)
+time.sleep(1)
+print("Motor 3")
+m3.setMotorSpeed(-255)
+time.sleep(1)
+print("Motor 4")
+m4.setMotorSpeed(-255)
+time.sleep(1)
+print("Motor 5")
+m5.setMotorSpeed(-255)
+time.sleep(1)
+print("Motor 6")
+m6.setMotorSpeed(-255)
+time.sleep(1)
+print("All off")
+m1.off()
+m2.off()
+m3.off()
+m4.off()
+m5.off()
+m6.off()

examples/640servotest.py

@@ -0,0 +1,35 @@
+import time
+from darkwater_640 import dw_Controller, dw_Servo
+
+dw = dw_Controller( addr=0x60 )
+s1 = dw.getServo(1)
+s2 = dw.getServo(2)
+
+s1.off()
+s2.off()
+time.sleep(1)
+
+print("Set 2000uS - ")
+print("Servo 1")
+s1.setPWMuS(2000)
+time.sleep(1)
+print("Servo 2")
+s2.setPWMuS(2000)
+time.sleep(1)
+print("Set 1500uS - ")
+print("Servo 1")
+s1.setPWMuS(1500)
+time.sleep(1)
+print("Servo 2")
+s2.setPWMuS(1500)
+time.sleep(1)
+print("Set 1000uS - ")
+print("Servo 1")
+s1.setPWMuS(1000)
+time.sleep(1)
+print("Servo 2")
+s2.setPWMuS(1000)
+time.sleep(1)
+print("All off")
+s1.off()
+s2.off()

examples/640steppertest.py

@@ -0,0 +1,33 @@
+import time
+from darkwater_640 import dw_Controller, dw_Stepper
+
+dw = dw_Controller( addr=0x60 )
+s1 = dw.getStepper(1)
+s2 = dw.getStepper(2)
+s3 = dw.getStepper(3)
+
+s1.off()
+s2.off()
+s3.off()
+
+s1.setMotorSpeed(200)
+s2.setMotorSpeed(200)
+s3.setMotorSpeed(200)
+
+print("1 Forward")
+s1.step( 400, dw_Controller.FORWARD, dw_Controller.DOUBLE )
+print("2 Forward")
+s2.step( 400, dw_Controller.FORWARD, dw_Controller.DOUBLE )
+print("3 Forward")
+s3.step( 400, dw_Controller.FORWARD, dw_Controller.DOUBLE )
+
+print("1 Reverse")
+s1.step( 400, dw_Controller.REVERSE, dw_Controller.DOUBLE )
+print("2 Reverse")
+s2.step( 400, dw_Controller.REVERSE, dw_Controller.DOUBLE )
+print("3 Reverse")
+s3.step( 400, dw_Controller.REVERSE, dw_Controller.DOUBLE )
+
+s1.off()
+s2.off()
+s3.off()

examples/640test.py

@@ -1,21 +0,0 @@
-import time
-from dw640HAT import dw_MotorCONTROL, dw_DCMotor
-
-dw = dw_MotorCONTROL( addr=0x60 )
-m = dw.getMotor(1)
-
-m.run(dw_MotorCONTROL.RELEASE)
-time.sleep(5)
-
-##time.sleep(10)
-print "Set forward"
-m.setMotorSpeed(255)
-time.sleep(5)
-print "stop"
-m.setMotorSpeed(0)
-time.sleep(5)
-print "Set reverse"
-m.setMotorSpeed(-255)
-time.sleep(5)
-print "stop"
-m.run(dw_MotorCONTROL.RELEASE)

setup.py

@@ -0,0 +1,11 @@
+from distutils.core import setup
+
+setup(name              = 'darkwater_640',
+      version           = '1.0.2',
+      author            = 'Team Dark Water',
+      author_email      = 'team@darkwater.io',
+      description       = 'Library for Dark Water 640 board',
+      license           = 'MIT',
+      url               = 'https://github.com/darkwaterfoundation/darkwater_python_640',
+      packages          = ['darkwater_640']
+      )