def __init__(self, port, addr=DEFAULT_ADDR, integrate=True): self.i2c = wpilib.I2C(port, addr) self.addr = addr self.resetAngle() self.centerX = 0 self.centerY = 0 self.centerZ = 0 self.xGyro = self.Gyro(self.Axis.X, self) self.yGyro = self.Gyro(self.Axis.Y, self) self.zGyro = self.Gyro(self.Axis.Z, self) self.intrTask = None if integrate: self.intrTask = TimerTask("ITG3200 Integrate", 0.05, self.update) self.intrTask.start() self.enabled = True
class ITG3200(PIDSource): def __init__(self, port, addr=DEFAULT_ADDR, integrate=True): self.i2c = wpilib.I2C(port, addr) self.addr = addr self.resetAngle() self.centerX = 0 self.centerY = 0 self.centerZ = 0 self.xGyro = self.Gyro(self.Axis.X, self) self.yGyro = self.Gyro(self.Axis.Y, self) self.zGyro = self.Gyro(self.Axis.Z, self) self.intrTask = None if integrate: self.intrTask = TimerTask("ITG3200 Integrate", 0.05, self.update) self.intrTask.start() self.enabled = True def free(self): self.i2c.free() if self.intrTask is not None: self.intrTask.cancel() def readI2C(self, register, count): if not self.enabled: return [0]*count try: return self.i2c.transaction([register], count) except IOError: wpilib.DriverStation.reportError("Error reading from ITG3200", False) return [0]*count def writeI2C(self, register, data): if not self.enabled: return None try: self.i2c.write(register, data) except IOError: wpilib.DriverStation.reportError("Error writing to ITG3200", False) def resetAngle(self): self.angleX = 0 self.angleY = 0 self.angleZ = 0 def calibrate(self, time=5.0, samples=100): """ Calibrate Watch out, this will actually pause the robot :param samples: The amount of samples to take :param time: The time to calibrate :return: """ gathered = 0 calX = 0 calY = 0 calZ = 0 print("Starting ITG3200 calibration routine for {} seconds".format(time)) while gathered < samples: calX += self.getRateX() calY += self.getRateY() calZ += self.getRateZ() gathered += 1 Timer.delay(time/samples) print("Done calibrating ITG3200") calX /= samples calY /= samples calZ /= samples print("Cal X: {}".format(calX)) print("Cal Y: {}".format(calY)) print("Cal Z: {}".format(calZ)) self.centerX = calX self.centerY = calY self.centerZ = calZ def update(self, dt=50): """ :param dt: Delta time in milliseconds :return: """ # TODO: Filter out data? Use accel? real_dt = dt/1000 # Turn ms into seconds self.angleX += self.getRateX() * real_dt self.angleY += self.getRateY() * real_dt self.angleZ += self.getRateZ() * real_dt # Power on and prepare for general usage. # This will activate the gyroscope, so be sure to adjust the power settings # after you call this method if you want it to enter standby mode, or another # less demanding mode of operation. This also sets the gyroscope to use the # X-axis gyro for a clock source. Note that it doesn't have any delays in the # routine, which means you might want to add ~50ms to be safe if you happen # to need to read gyro data immediately after initialization. The data will # flow in either case, but the first reports may have higher error offsets. def init(self): if self.i2c.addressOnly(): wpilib.DriverStation.reportError("Could not address ITG3200", False) self.enabled = False # return False if not self.testConnection(): wpilib.DriverStation.reportError("Could not connect to ITG3200", False) # self.enabled = False # return False wpilib.Timer.delay(50/1000) self.setFullScaleRange(FULLSCALE_2000) self.setSleepEnabled(False) self.setStandByXEnabled(False) self.setStandByYEnabled(False) self.setStandByZEnabled(False) self.setClockSource(CLOCK_PLL_XGYRO) self.setIntDeviceReadyEnabled(True) self.setIntDataReadyEnabled(True) self.enabled = True return True def getAngleX(self): return self.angleX def getAngleY(self): return self.angleY def getAngleZ(self): return self.angleZ def getAngle(self, axis): if axis == self.Axis.X: return self.getAngleX() if axis == self.Axis.Y: return self.getAngleY() if axis == self.Axis.Z: return self.getAngleZ() def getRate(self, axis): if axis == self.Axis.X: return self.getRateX() if axis == self.Axis.Y: return self.getRateY() if axis == self.Axis.Z: return self.getRateZ() # Gyro Interface def readShortFromRegister(self, register, count): buf = self.readI2C(register, count) return signed_short((buf[0] << 8) | buf[1]) def writeBits(self, register, bit, numBits, value): rawData = self.readI2C(register, 1) newValue = self.updateByte(rawData[0], bit, numBits, value) self.writeI2C(register, newValue) def readBit(self, register, bit): return (self.readI2C(register, bit) & bit) != 0 def writeBit(self, register, bit, value): """ :type value: bool """ buf = self.readI2C(register, 1) newValue = (buf[0] | (1 << bit)) if value else (buf[0] & ~(1 << bit)) self.writeI2C(register, newValue) # this routine should update the original byte with the new data properly shifted to the correct bit location def updateByte(self, original, bit, numBits, value): if numBits > 7 or bit > 7 or bit < numBits-1 or bit < 0 or numBits < 0: raise ValueError("Use 8-bit bytes: numBits: {} bit: {}".format(numBits, bit)) if value > 2**numBits: raise ValueError("Value too large") mask = self.getMask(bit, numBits) maskedOriginal = (original & mask) & 0xFF shiftedValue = (value << (1 + bit - numBits)) & 0xFF return (shiftedValue | maskedOriginal) & 0xFF def setFullScaleRange(self, range): self.writeBits(RA_DLPF_FS, DF_FS_SEL_BIT, DF_FS_SEL_LENGTH, range) def getFullScaleRange(self): return self.getRegisterBits(RA_DLPF_FS, DF_FS_SEL_BIT, DF_FS_SEL_LENGTH) def setClockSource(self, source): self.writeBits(RA_PWR_MGM, PWR_CLK_SEL_BIT, PWR_CLK_SEL_LENGTH, source) def setIntDeviceReadyEnabled(self, enabled): self.writeBit(RA_INT_CFG, INTCFG_ITG_RDY_EN_BIT, enabled) def setIntDataReadyEnabled(self, enabled): self.writeBit(RA_INT_CFG, INTCFG_RAW_RDY_EN_BIT, enabled) def getRateX(self): return ((self.readShortFromRegister(RA_GYRO_XOUT_H, 2)) / GYRO_SENSITIVITY) - self.centerX def getRateY(self): return ((self.readShortFromRegister(RA_GYRO_YOUT_H, 2))/GYRO_SENSITIVITY) - self.centerY def getRateZ(self): return ((self.readShortFromRegister(RA_GYRO_ZOUT_H, 2)) / GYRO_SENSITIVITY) - self.centerZ def getTemperature(self): return (self.readShortFromRegister(RA_TEMP_OUT_H, 2)/TEMP_SENSITIVITY) + TEMP_OFFSET def testConnection(self): return self.getDeviceID() == 0b110100 def getSampleRate(self): return self.getRegisterByte(RA_SMPLRT_DIV) def setSampleRate(self, rate): self.writeI2C(RA_SMPLRT_DIV, rate) # This register is used to verify the identity of the device def getDeviceID(self): return self.getRegisterBits(RA_WHO_AM_I, DEVID_BIT, DEVID_LENGTH) # Gets the bit mask for the given bit and number of bits def getMask(self, bit, numBits): newMask = 0 for i in range(7+1): if i > bit or i <= bit - numBits: newMask += 2**i return newMask & 0xFF # Get n bits from the byte to form a byte slice def getBits(self, bitField, bit, numBits): if numBits > 7 or bit > 7 or bit < numBits - 1 or bit < 0 or numBits < 0: raise ValueError("Use 8-bit bytes") mask = ~self.getMask(bit, numBits) & 0xFF maskedInput = (bitField & mask) & 0xFF return rshift(maskedInput, (1 + bit - numBits)) & 0xFF def getRegisterByte(self, register): return self.readI2C(register, 1)[0] def getRegisterBits(self, register, bit, numBits): containingByte = self.getRegisterByte(register) return self.getBits(containingByte, bit, numBits) def getSleepEnabled(self): return self.readBit(RA_PWR_MGM, PWR_SLEEP_BIT) def setSleepEnabled(self, enabled): self.writeBit(RA_PWR_MGM, PWR_SLEEP_BIT, enabled) def setStandByXEnabled(self, enabled): self.writeBit(RA_PWR_MGM, PWR_STBY_XG_BIT, enabled) def setStandByYEnabled(self, enabled): self.writeBit(RA_PWR_MGM, PWR_STBY_YG_BIT, enabled) def setStandByZEnabled(self, enabled): self.writeBit(RA_PWR_MGM, PWR_STBY_ZG_BIT, enabled) def getStandByXEnabled(self): return self.readBit(RA_PWR_MGM, PWR_STBY_XG_BIT) def getStandByYEnabled(self): return self.readBit(RA_PWR_MGM, PWR_STBY_YG_BIT) def getStandByZEnabled(self): return self.readBit(RA_PWR_MGM, PWR_STBY_ZG_BIT) class Axis: X = 0 Y = 1 Z = 2 class Gyro(PIDSource): def __init__(self, axis, gyroscope): """ :param axis: The axis to record :type axis: Axis :param gyroscope: The ITG3200 instance :type gyroscope: ITG3200 :return: The Gyro object for PID usage """ self.pidSourceType = PIDSource.PIDSourceType.kRate self.axis = axis self.gyroscope = gyroscope def getPIDSourceType(self): return self.pidSourceType def setPIDSourceType(self, pidSource): self.pidSourceType = pidSource def pidGet(self): if self.pidSourceType is self.PIDSourceType.kRate: return self.gyroscope.getRate(self.axis) else: return self.gyroscope.getAngle(self.axis)
class ITG3200(PIDSource): def __init__(self, port, addr=DEFAULT_ADDR, integrate=True): self.i2c = wpilib.I2C(port, addr) self.addr = addr self.resetAngle() self.centerX = 0 self.centerY = 0 self.centerZ = 0 self.xGyro = self.Gyro(self.Axis.X, self) self.yGyro = self.Gyro(self.Axis.Y, self) self.zGyro = self.Gyro(self.Axis.Z, self) self.intrTask = None if integrate: self.intrTask = TimerTask("ITG3200 Integrate", 0.05, self.update) self.intrTask.start() self.enabled = True def free(self): self.i2c.free() if self.intrTask is not None: self.intrTask.cancel() def readI2C(self, register, count): if not self.enabled: return [0] * count try: return self.i2c.transaction([register], count) except IOError: wpilib.DriverStation.reportError("Error reading from ITG3200", False) return [0] * count def writeI2C(self, register, data): if not self.enabled: return None try: self.i2c.write(register, data) except IOError: wpilib.DriverStation.reportError("Error writing to ITG3200", False) def resetAngle(self): self.angleX = 0 self.angleY = 0 self.angleZ = 0 def calibrate(self, time=5.0, samples=100): """ Calibrate Watch out, this will actually pause the robot :param samples: The amount of samples to take :param time: The time to calibrate :return: """ gathered = 0 calX = 0 calY = 0 calZ = 0 print( "Starting ITG3200 calibration routine for {} seconds".format(time)) while gathered < samples: calX += self.getRateX() calY += self.getRateY() calZ += self.getRateZ() gathered += 1 Timer.delay(time / samples) print("Done calibrating ITG3200") calX /= samples calY /= samples calZ /= samples print("Cal X: {}".format(calX)) print("Cal Y: {}".format(calY)) print("Cal Z: {}".format(calZ)) self.centerX = calX self.centerY = calY self.centerZ = calZ def update(self, dt=50): """ :param dt: Delta time in milliseconds :return: """ # TODO: Filter out data? Use accel? real_dt = dt / 1000 # Turn ms into seconds self.angleX += self.getRateX() * real_dt self.angleY += self.getRateY() * real_dt self.angleZ += self.getRateZ() * real_dt # Power on and prepare for general usage. # This will activate the gyroscope, so be sure to adjust the power settings # after you call this method if you want it to enter standby mode, or another # less demanding mode of operation. This also sets the gyroscope to use the # X-axis gyro for a clock source. Note that it doesn't have any delays in the # routine, which means you might want to add ~50ms to be safe if you happen # to need to read gyro data immediately after initialization. The data will # flow in either case, but the first reports may have higher error offsets. def init(self): if self.i2c.addressOnly(): wpilib.DriverStation.reportError("Could not address ITG3200", False) self.enabled = False # return False if not self.testConnection(): wpilib.DriverStation.reportError("Could not connect to ITG3200", False) # self.enabled = False # return False wpilib.Timer.delay(50 / 1000) self.setFullScaleRange(FULLSCALE_2000) self.setSleepEnabled(False) self.setStandByXEnabled(False) self.setStandByYEnabled(False) self.setStandByZEnabled(False) self.setClockSource(CLOCK_PLL_XGYRO) self.setIntDeviceReadyEnabled(True) self.setIntDataReadyEnabled(True) self.enabled = True return True def getAngleX(self): return self.angleX def getAngleY(self): return self.angleY def getAngleZ(self): return self.angleZ def getAngle(self, axis): if axis == self.Axis.X: return self.getAngleX() if axis == self.Axis.Y: return self.getAngleY() if axis == self.Axis.Z: return self.getAngleZ() def getRate(self, axis): if axis == self.Axis.X: return self.getRateX() if axis == self.Axis.Y: return self.getRateY() if axis == self.Axis.Z: return self.getRateZ() # Gyro Interface def readShortFromRegister(self, register, count): buf = self.readI2C(register, count) return signed_short((buf[0] << 8) | buf[1]) def writeBits(self, register, bit, numBits, value): rawData = self.readI2C(register, 1) newValue = self.updateByte(rawData[0], bit, numBits, value) self.writeI2C(register, newValue) def readBit(self, register, bit): return (self.readI2C(register, bit) & bit) != 0 def writeBit(self, register, bit, value): """ :type value: bool """ buf = self.readI2C(register, 1) newValue = (buf[0] | (1 << bit)) if value else (buf[0] & ~(1 << bit)) self.writeI2C(register, newValue) # this routine should update the original byte with the new data properly shifted to the correct bit location def updateByte(self, original, bit, numBits, value): if numBits > 7 or bit > 7 or bit < numBits - 1 or bit < 0 or numBits < 0: raise ValueError("Use 8-bit bytes: numBits: {} bit: {}".format( numBits, bit)) if value > 2**numBits: raise ValueError("Value too large") mask = self.getMask(bit, numBits) maskedOriginal = (original & mask) & 0xFF shiftedValue = (value << (1 + bit - numBits)) & 0xFF return (shiftedValue | maskedOriginal) & 0xFF def setFullScaleRange(self, range): self.writeBits(RA_DLPF_FS, DF_FS_SEL_BIT, DF_FS_SEL_LENGTH, range) def getFullScaleRange(self): return self.getRegisterBits(RA_DLPF_FS, DF_FS_SEL_BIT, DF_FS_SEL_LENGTH) def setClockSource(self, source): self.writeBits(RA_PWR_MGM, PWR_CLK_SEL_BIT, PWR_CLK_SEL_LENGTH, source) def setIntDeviceReadyEnabled(self, enabled): self.writeBit(RA_INT_CFG, INTCFG_ITG_RDY_EN_BIT, enabled) def setIntDataReadyEnabled(self, enabled): self.writeBit(RA_INT_CFG, INTCFG_RAW_RDY_EN_BIT, enabled) def getRateX(self): return ((self.readShortFromRegister(RA_GYRO_XOUT_H, 2)) / GYRO_SENSITIVITY) - self.centerX def getRateY(self): return ((self.readShortFromRegister(RA_GYRO_YOUT_H, 2)) / GYRO_SENSITIVITY) - self.centerY def getRateZ(self): return ((self.readShortFromRegister(RA_GYRO_ZOUT_H, 2)) / GYRO_SENSITIVITY) - self.centerZ def getTemperature(self): return (self.readShortFromRegister(RA_TEMP_OUT_H, 2) / TEMP_SENSITIVITY) + TEMP_OFFSET def testConnection(self): return self.getDeviceID() == 0b110100 def getSampleRate(self): return self.getRegisterByte(RA_SMPLRT_DIV) def setSampleRate(self, rate): self.writeI2C(RA_SMPLRT_DIV, rate) # This register is used to verify the identity of the device def getDeviceID(self): return self.getRegisterBits(RA_WHO_AM_I, DEVID_BIT, DEVID_LENGTH) # Gets the bit mask for the given bit and number of bits def getMask(self, bit, numBits): newMask = 0 for i in range(7 + 1): if i > bit or i <= bit - numBits: newMask += 2**i return newMask & 0xFF # Get n bits from the byte to form a byte slice def getBits(self, bitField, bit, numBits): if numBits > 7 or bit > 7 or bit < numBits - 1 or bit < 0 or numBits < 0: raise ValueError("Use 8-bit bytes") mask = ~self.getMask(bit, numBits) & 0xFF maskedInput = (bitField & mask) & 0xFF return rshift(maskedInput, (1 + bit - numBits)) & 0xFF def getRegisterByte(self, register): return self.readI2C(register, 1)[0] def getRegisterBits(self, register, bit, numBits): containingByte = self.getRegisterByte(register) return self.getBits(containingByte, bit, numBits) def getSleepEnabled(self): return self.readBit(RA_PWR_MGM, PWR_SLEEP_BIT) def setSleepEnabled(self, enabled): self.writeBit(RA_PWR_MGM, PWR_SLEEP_BIT, enabled) def setStandByXEnabled(self, enabled): self.writeBit(RA_PWR_MGM, PWR_STBY_XG_BIT, enabled) def setStandByYEnabled(self, enabled): self.writeBit(RA_PWR_MGM, PWR_STBY_YG_BIT, enabled) def setStandByZEnabled(self, enabled): self.writeBit(RA_PWR_MGM, PWR_STBY_ZG_BIT, enabled) def getStandByXEnabled(self): return self.readBit(RA_PWR_MGM, PWR_STBY_XG_BIT) def getStandByYEnabled(self): return self.readBit(RA_PWR_MGM, PWR_STBY_YG_BIT) def getStandByZEnabled(self): return self.readBit(RA_PWR_MGM, PWR_STBY_ZG_BIT) class Axis: X = 0 Y = 1 Z = 2 class Gyro(PIDSource): def __init__(self, axis, gyroscope): """ :param axis: The axis to record :type axis: Axis :param gyroscope: The ITG3200 instance :type gyroscope: ITG3200 :return: The Gyro object for PID usage """ self.pidSourceType = PIDSource.PIDSourceType.kRate self.axis = axis self.gyroscope = gyroscope def getPIDSourceType(self): return self.pidSourceType def setPIDSourceType(self, pidSource): self.pidSourceType = pidSource def pidGet(self): if self.pidSourceType is self.PIDSourceType.kRate: return self.gyroscope.getRate(self.axis) else: return self.gyroscope.getAngle(self.axis)