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()
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