API tools faq
paste
Advertisement
Guest User

ITG3200 I2C Gyro code

a guest
Feb 11th, 2016
220
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 10.05 KB | None | 0 0
raw download clone embed print report
  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)
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!