def __init__(self, port):
spi = SPI(port)
spi.setClockRate(4000000) # 4 MHz (rRIO max, gyro can go high)
spi.setClockActiveHigh()
spi.setChipSelectActiveLow()
spi.setMSBFirst()
self._spi = spi
self._command = [0x00] * DATA_SIZE
self._data = [0x00] * DATA_SIZE
self._command[0] = READ_COMMAND
self._accumulated_angle = 0.0
self._current_rate = 0.0
self._accumulated_offset = 0.0
self._rate_offset = 0.0
self._last_time = 0
self._current_time = 0
self._calibration_timer = Timer()
self._calibration_timer.start()
self._update_timer = Timer()
self._update_timer.start()
#
# self._update_thread = Thread(self.update, daemon=True)
# self._update_thread.start()
self.update()
def __init__(self, port, bitrate=None):
if bitrate is None:
bitrate = DEFAULT_SPI_BITRATE_HZ
simPort = None
if hal.HALIsSimulation():
from ._impl.sim_io import NavXSPISim
simPort = NavXSPISim()
self.port = SPI(port, simPort=simPort)
self.bitrate = bitrate
def __init__(self):
self.spi = SPI(0)
self.spi.setClockRate(2000000)
self.leds = 22
self.lastIndex = self.leds - 1
self.gamma = bytearray(256)
self.buffer = [0 for __ in range(self.leds + 1)]
self.masterBrightness = 1.0
for led in range(self.leds):
self.buffer[led] = bytearray(3)
for i in range(256):
# Color calculations from
# http://learn.adafruit.com/light-painting-with-raspberry-pi
self.gamma[i] = 0x80 | int(
pow(float(i) / 255.0, 2.5) * 127.0 + 0.5)
self.fill_RGB(255, 0, 0)
def __init__(self, port, bitrate=None):
if bitrate is None:
bitrate = DEFAULT_SPI_BITRATE_HZ
simPort = None
if hal.HALIsSimulation():
from ._impl.sim_io import NavXSPISim
simPort = NavXSPISim()
self.port = SPI(port, simPort=simPort)
self.bitrate = bitrate
class LEDStrip(Component):
def __init__(self):
self.spi = SPI(0)
self.spi.setClockRate(2000000)
self.leds = 22
self.lastIndex = self.leds - 1
self.gamma = bytearray(256)
self.buffer = [0 for __ in range(self.leds + 1)]
self.masterBrightness = 1.0
for led in range(self.leds):
self.buffer[led] = bytearray(3)
for i in range(256):
# Color calculations from
# http://learn.adafruit.com/light-painting-with-raspberry-pi
self.gamma[i] = 0x80 | int(
pow(float(i) / 255.0, 2.5) * 127.0 + 0.5)
self.fill_RGB(255, 0, 0)
def set_brightness(self, bright):
if bright > 1.0 or bright < 0.0:
raise ValueError('Brightness must be between 0.0 and 1.0')
self.masterBrightness = bright
def fill(self, color, start=0, end=-1):
if start < 0:
start = 0
if end < 0 or end > self.lastIndex:
end = self.lastIndex
for led in range(start, end + 1):
self.__set_internal(led, color)
def fill_RGB(self, r, g, b, start=0, end=-1):
self.fill(Color(r, g, b), start, end)
def __set_internal(self, pixel, color):
if pixel < 0 or pixel > self.lastIndex:
return
self.buffer[pixel][1] = self.gamma[int(color.r *
self.masterBrightness)]
self.buffer[pixel][0] = self.gamma[int(color.g *
self.masterBrightness)]
self.buffer[pixel][2] = self.gamma[int(color.b *
self.masterBrightness)]
def update(self):
for x in range(self.leds):
self.spi.write(self.buffer[x])
self.spi.write(
bytearray(b'\x00\x00\x00')
) # zero fill the last to prevent stray colors at the end
def stop(self):
pass
class RegisterIO_SPI:
def __init__(self, port, bitrate=None):
if bitrate is None:
bitrate = DEFAULT_SPI_BITRATE_HZ
simPort = None
if hal.HALIsSimulation():
from ._impl.sim_io import NavXSPISim
simPort = NavXSPISim()
self.port = SPI(port, simPort=simPort)
self.bitrate = bitrate
def init(self):
logger.info("Initializing NavX SPI")
self.port.setClockRate(self.bitrate)
self.port.setMSBFirst()
self.port.setSampleDataOnFalling()
self.port.setClockActiveLow()
self.port.setChipSelectActiveLow()
logger.info("Initialized!")
return True
def write(self, address, value):
data = [address | 0x80, value]
data.append(crc7(data))
if self.port.write(data) != len(data):
return False
return True
def read(self, first_address, count):
data = [first_address, count]
data.append(crc7(data))
retcount = self.port.write(data)
if retcount != len(data):
raise IOError("Write error (%s != %s)" % (retcount, len(data)))
Timer.delay(0.001)
data = self.port.read(True, count + 1)
if len(data) != count + 1:
raise IOError("Read error (%s != %s)" % (len(data), count + 1))
crc = data[-1]
data = data[:-1]
if crc7(data) != crc:
raise IOError("CRC error")
return data
def shutdown(self):
# TODO: should free?
logger.info("NavX i2c shutdown")
self.port.free()
return True
class RegisterIO_SPI:
def __init__(self, port, bitrate=None):
if bitrate is None:
bitrate = DEFAULT_SPI_BITRATE_HZ
simPort = None
if hal.HALIsSimulation():
from ._impl.sim_io import NavXSPISim
simPort = NavXSPISim()
self.port = SPI(port, simPort=simPort)
self.bitrate = bitrate
def init(self):
logger.info("Initializing NavX SPI")
self.port.setClockRate(self.bitrate)
self.port.setMSBFirst()
self.port.setSampleDataOnFalling()
self.port.setClockActiveLow()
self.port.setChipSelectActiveLow()
logger.info("Initialized!")
return True
def write(self, address, value):
data = [address | 0x80, value]
data.append(crc7(data))
if self.port.write(data) != len(data):
return False
return True
def read(self, first_address, count):
data = [first_address, count]
data.append(crc7(data))
retcount = self.port.write(data)
if retcount != len(data):
raise IOError("Write error (%s != %s)" % (retcount, len(data)))
Timer.delay(0.001)
data = self.port.read(True, count + 1)
if len(data) != count + 1:
raise IOError("Read error (%s != %s)" % (len(data), count+1))
crc = data[-1]
data = data[:-1]
if crc7(data) != crc:
raise IOError("CRC error")
return data
def shutdown(self):
# TODO: should free?
logger.info("NavX i2c shutdown")
self.port.free()
return True
