Public Pastes
SHARE
TWEET

ITG3200 I2C Gyro code

a guest Feb 11th, 2016 71 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. # Original: https://github.com/bussell/SparkFun6Dof/blob/master/GyroITG3200.java
  2. # Translated by Nick Schatz for FRC 3184 Blaze Robotics
  3. # All credit is given to original author
  4.  
  5. # ============================================
  6. # GyroITG3200 device library code is placed under the MIT license
  7. # Copyright (c) 2011 by Jeff Rowberg
  8. # Copyright (c) 2015 Joe Bussell
  9. # Permission is hereby granted, free of charge, to any person obtaining a copy
  10. # of this software and associated documentation files (the "Software"), to deal
  11. # in the Software without restriction, including without limitation the rights
  12. # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  13. # copies of the Software, and to permit persons to whom the Software is
  14. # furnished to do so, subject to the following conditions:
  15. # The above copyright notice and this permission notice shall be included in
  16. # all copies or substantial portions of the Software.
  17. # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  18. # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  19. # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  20. # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  21. # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  22. # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  23. # THE SOFTWARE.
  24. # ===============================================
  25.  
  26. import wpilib
  27. import sys
  28. from wpilib.interfaces import PIDSource
  29.  
  30. # Constants
  31. ADDR_AD0_LOW = 0x68
  32. ADDR_AD0_HIGH = 0x69
  33. DEFAULT_ADDR = ADDR_AD0_LOW
  34.  
  35. RA_GYRO_XOUT_H = 0x1D
  36. RA_GYRO_XOUT_L = 0x1E
  37. RA_GYRO_YOUT_H = 0x1F
  38. RA_GYRO_YOUT_L = 0x20
  39. RA_GYRO_ZOUT_H = 0x21
  40. RA_GYRO_ZOUT_L = 0x22
  41. RA_TEMP_OUT_H = 0x1B
  42. RA_TEMP_OUT_L = 0x1C
  43. RA_WHO_AM_I = 0x00
  44. RA_DLPF_FS = 0x16
  45. RA_PWR_MGM = 0x3E
  46. RA_INT_CFG = 0x17
  47. RA_SMPLRT_DIV = 0x15
  48.  
  49. DF_FS_SEL_BIT = 4
  50. DF_FS_SEL_LENGTH = 2
  51.  
  52. INTCFG_ITG_RDY_EN_BIT = 2
  53. INTCFG_RAW_RDY_EN_BIT = 0
  54.  
  55. PWR_CLK_SEL_BIT = 2
  56. PWR_CLK_SEL_LENGTH = 3
  57. PWR_SLEEP_BIT = 6
  58. PWR_STBY_XG_BIT = 5
  59. PWR_STBY_YG_BIT = 4
  60. PWR_STBY_ZG_BIT = 3
  61.  
  62. DEVID_BIT = 6
  63. DEVID_LENGTH = 6
  64.  
  65. FULLSCALE_2000 = 0x03
  66.  
  67. CLOCK_PLL_XGYRO = 0x01
  68.  
  69.  
  70. def rshift(val, n):
  71.     return (val % 0x100000000) >> n
  72.  
  73.  
  74. class ITG3200(PIDSource):
  75.     def __init__(self, port, addr=DEFAULT_ADDR):
  76.         self.i2c = wpilib.I2C(port, addr)
  77.         self.addr = addr
  78.         self.pidAxis = self.Axis.X
  79.         self.setPIDSourceType(PIDSource.PIDSourceType.kRate)
  80.         self.reset()
  81.  
  82.     def readI2C(self, register, count):
  83.         try:
  84.             return self.i2c.read(register, count)
  85.         except:
  86.             e = sys.exc_info()[0]
  87.             wpilib.DriverStation.reportError("Unhandled Exception when reading gyro: {}".format(e), False)
  88.  
  89.     def writeI2C(self, register, data):
  90.         try:
  91.             self.i2c.write(register, data)
  92.         except:
  93.             e = sys.exc_info()[0]
  94.             wpilib.DriverStation.reportError("Unhandled Exception when writing to gyro: {}".format(e), False)
  95.  
  96.     def reset(self):
  97.         self.angleX = 0
  98.         self.angleY = 0
  99.         self.angleZ = 0
  100.  
  101.     def update(self, dt=20):
  102.         """
  103.  
  104.         :param dt: Delta time in milliseconds
  105.         :return:
  106.         """
  107.        
  108.         # TODO: Filter out data? Use accel?
  109.        
  110.         real_dt = dt/1000 # Turn ms into seconds
  111.  
  112.         self.angleX += self.getRateX() * real_dt
  113.         self.angleY += self.getRateY() * real_dt
  114.         self.angleZ += self.getRateZ() * real_dt
  115.  
  116.     # PID stuff    
  117.     class Axis:
  118.         X = 0
  119.         Y = 1
  120.         Z = 2
  121.  
  122.     def getPIDSourceType(self):
  123.         return self.pidSourceType
  124.  
  125.     def setPIDSourceType(self, pidSource):
  126.         self.pidSourceType = pidSource
  127.  
  128.     def setPIDAxis(self, axis):
  129.         self.pidAxis = axis
  130.  
  131.     def pidGet(self):
  132.         if self.getPIDSourceType() == PIDSource.PIDSourceType.kRate:
  133.             return self.getRate(self.pidAxis)
  134.         else:
  135.             return self.getAngle(self.pidAxis)
  136.  
  137.     # Power on and prepare for general usage.
  138.     # This will activate the gyroscope, so be sure to adjust the power settings
  139.     # after you call this method if you want it to enter standby mode, or another
  140.     # less demanding mode of operation. This also sets the gyroscope to use the
  141.     # X-axis gyro for a clock source. Note that it doesn't have any delays in the
  142.     # routine, which means you might want to add ~50ms to be safe if you happen
  143.     # to need to read gyro data immediately after initialization. The data will
  144.     # flow in either case, but the first reports may have higher error offsets.
  145.     def init(self):
  146.         if not self.testConnection():
  147.             wpilib.DriverStation.reportError("Could not connect to ITG3200", False)
  148.         self.setFullScaleRange(FULLSCALE_2000)
  149.         self.setClockSource(CLOCK_PLL_XGYRO)
  150.         self.setIntDeviceReadyEnabled(True)
  151.         self.setIntDataReadyEnabled(True)
  152.    
  153.     def getAngleX(self):
  154.         return self.angleX
  155.        
  156.     def getAngleY(self):
  157.         return self.angleY
  158.        
  159.     def getAngleZ(self):
  160.         return self.angleZ
  161.        
  162.     def getAngle(self, axis):
  163.         if axis == self.Axis.X:
  164.             return self.getAngleX()
  165.         if axis == self.Axis.Y:
  166.             return self.getAngleY()
  167.         if axis == self.Axis.Z:
  168.             return self.getAngleZ()
  169.    
  170.     def getRate(self, axis):
  171.         if axis == self.Axis.X:
  172.             return self.getRateX()
  173.         if axis == self.Axis.Y:
  174.             return self.getRateY()
  175.         if axis == self.Axis.Z:
  176.             return self.getRateZ()
  177.  
  178.     # Gyro Interface
  179.  
  180.     def readShortFromRegister(self, register, count):
  181.         buf = self.readI2C(register, count)
  182.         return (buf[0] << 8) | buf[1]
  183.  
  184.     def writeBits(self, register, bit, numBits, value):
  185.         rawData = self.readI2C(register, 1)
  186.         newValue = self.updateByte(rawData[0], bit, numBits, value)
  187.         self.writeI2C(register, newValue)
  188.  
  189.     def readBit(self, register, bit):
  190.         return (self.readI2C(register, bit) & bit) != 0
  191.    
  192.     def writeBit(self, register, bit, value):
  193.         """
  194.  
  195.         :type value: bool
  196.         """
  197.         buf = self.readI2C(register, 1)
  198.         newValue = (buf[0] | (1 << bit)) if value else (buf[0] & ~(1 << bit))
  199.         self.writeI2C(register, newValue)
  200.  
  201.     # this routine should update the original byte with the new data properly shifted to the correct bit location
  202.     def updateByte(self, original, bit, numBits, value):
  203.         if numBits > 7 or bit > 7 or bit < numBits-1 or bit < 0 or numBits < 0:
  204.             raise ValueError("Use 8-bit bytes: numBits: {} bit: {}".format(numBits, bit))
  205.         if value > 2**numBits:
  206.             raise ValueError("Value too large")
  207.         mask = self.getMask(bit, numBits)
  208.         maskedOriginal = (original & mask) & 0xFF
  209.         shiftedValue = (value << (1 + bit - numBits)) & 0xFF
  210.         return (shiftedValue | maskedOriginal) & 0xFF
  211.  
  212.     def setFullScaleRange(self, range):
  213.         self.writeBits(RA_DLPF_FS, DF_FS_SEL_BIT, DF_FS_SEL_LENGTH, range)
  214.        
  215.     def getFullScaleRange(self):
  216.         return self.getRegisterBits(RA_DLPF_FS, DF_FS_SEL_BIT, DF_FS_SEL_LENGTH)
  217.  
  218.     def setClockSource(self, source):
  219.         self.writeBits(RA_PWR_MGM, PWR_CLK_SEL_BIT, PWR_CLK_SEL_LENGTH, source)
  220.  
  221.     def setIntDeviceReadyEnabled(self, enabled):
  222.         self.writeBit(RA_INT_CFG, INTCFG_ITG_RDY_EN_BIT, enabled)
  223.  
  224.     def setIntDataReadyEnabled(self, enabled):
  225.         self.writeBit(RA_INT_CFG, INTCFG_RAW_RDY_EN_BIT, enabled)
  226.  
  227.     def getRateX(self):
  228.         return self.readShortFromRegister(RA_GYRO_XOUT_H, 2)
  229.  
  230.     def getRateY(self):
  231.         return self.readShortFromRegister(RA_GYRO_YOUT_H, 2)
  232.  
  233.     def getRateZ(self):
  234.         return self.readShortFromRegister(RA_GYRO_ZOUT_H, 2)
  235.        
  236.     def getTemperature(self):
  237.         return self.readShortFromRegister(RA_TEMP_OUT_H, 2)
  238.  
  239.     def testConnection(self):
  240.         return self.getDeviceID() == 0b110100
  241.    
  242.     def getSampleRate(self):
  243.         return self.getRegisterByte(RA_SMPLRT_DIV)
  244.        
  245.     def setSampleRate(self, rate):
  246.         self.writeI2C(RA_SMPLRT_DIV, rate)
  247.  
  248.     # This register is used to verify the identity of the device
  249.     def getDeviceID(self):
  250.         return self.getRegisterBits(RA_WHO_AM_I, DEVID_BIT, DEVID_LENGTH)
  251.  
  252.     # Gets the bit mask for the given bit and number of bits
  253.     def getMask(self, bit, numBits):
  254.         newMask = 0
  255.         for i in range(7+1):
  256.             if i > bit or i <= bit - numBits:
  257.                 newMask += 2**i
  258.         return newMask & 0xFF
  259.  
  260.     # Get n bits from the byte to form a byte slice
  261.     def getBits(self, bitField, bit, numBits):
  262.         if numBits > 7 or bit > 7 or bit < numBits - 1 or bit < 0 or numBits < 0:
  263.             raise ValueError("Use 8-bit bytes")
  264.         mask = ~self.getMask(bit, numBits) & 0xFF
  265.         maskedInput = (bitField & mask) & 0xFF
  266.         return rshift(maskedInput, (1 + bit - numBits)) & 0xFF
  267.  
  268.     def getRegisterByte(self, register):
  269.         return self.readI2C(register, 1)[0]
  270.  
  271.     def getRegisterBits(self, register, bit, numBits):
  272.         containingByte = self.getRegisterByte(register)
  273.         return self.getBits(containingByte, bit, numBits)
  274.    
  275.     def getSleepEnabled(self):
  276.         return self.readBit(RA_PWR_MGM, PWR_SLEEP_BIT)
  277.    
  278.     def setSleepEnabled(self, enabled):
  279.         self.writeBit(RA_PWR_MGM, PWR_SLEEP_BIT, enabled)
  280.        
  281.     def setStandByXEnabled(self, enabled):
  282.         self.writeBit(RA_PWR_MGM, PWR_STBY_XG_BIT, enabled)
  283.    
  284.     def setStandByYEnabled(self, enabled):
  285.         self.writeBit(RA_PWR_MGM, PWR_STBY_YG_BIT, enabled)
  286.    
  287.     def setStandByZEnabled(self, enabled):
  288.         self.writeBit(RA_PWR_MGM, PWR_STBY_ZG_BIT, enabled)
  289.        
  290.     def getStandByXEnabled(self):
  291.         return self.readBit(RA_PWR_MGM, PWR_STBY_XG_BIT)
  292.    
  293.     def getStandByYEnabled(self):
  294.         return self.readBit(RA_PWR_MGM, PWR_STBY_YG_BIT)
  295.    
  296.     def getStandByZEnabled(self):
  297.         return self.readBit(RA_PWR_MGM, PWR_STBY_ZG_BIT)
RAW Paste Data
Top