def wakeup(self):
mode = read_byte(self.bus, self.i2c_adr, self.regs['PWR_MGMT_1'])
sleep = mode & self.bits['SLEEP'] # And take the SLEEP bit
if not sleep:
return True
# if sleep:
mode = mode & ~self.bits['SLEEP'] # Clear SLEEP bit
write_byte(self.bus, self.i2c_adr, self.regs['PWR_MGMT_1'],
mode) # power_mgmt_1 reg = X0XX XXXX
def updatePowerInfo():
global globExtPowerAvailable, globIntPowerLevel
# Create i2c lock if it does not exist yet.
i2c.createI2cLock()
# Lock i2c communication for this thread.
i2c.globI2cLock.acquire()
# Going to read I2C data.
# Read external power level. If the level > 100, we assume external power is available.
i2c.write_byte(slaveAddressArduino, 0,
100) # Command to indicate a read is going to follow.
i2c.write_byte(slaveAddressArduino, 0,
128) # 128 means read external power level.
if i2c.read_byte(slaveAddressArduino, 0) > 100:
globExtPowerAvailable = True
else:
globExtPowerAvailable = False
# Delay before retrieving the next power info.
time.sleep(i2c.globI2cDelay)
# Read internal power level. This power level can be from external power or the batteries.
i2c.write_byte(slaveAddressArduino, 0,
100) # Command to indicate a read is going to follow.
i2c.write_byte(slaveAddressArduino, 0,
129) # 129 means read internal power level.
globIntPowerLevel = i2c.read_byte(slaveAddressArduino, 0)
# Delay for i2c communication.
time.sleep(i2c.globI2cDelay)
# Release i2c communication for this thread.
i2c.globI2cLock.release()
def moveCamRel(degrees, delay):
if degrees >= -90 and degrees <= 90:
if degrees > 0:
# Create i2c lock if it does not exist yet.
i2c.createI2cLock()
# Lock i2c communication for this thread.
i2c.globI2cLock.acquire()
# I2C command 10.
i2c.write_byte(slaveAddressArduino, 0, 10)
i2c.write_byte(slaveAddressArduino, 0, 128 + int(degrees))
# Delay for i2c communication.
time.sleep(i2c.globI2cDelay)
# Release i2c communication for this thread.
i2c.globI2cLock.release()
elif degrees < 0:
# Create i2c lock if it does not exist yet.
i2c.createI2cLock()
# Lock i2c communication for this thread.
i2c.globI2cLock.acquire()
# I2C command 11.
i2c.write_byte(slaveAddressArduino, 0, 11)
i2c.write_byte(slaveAddressArduino, 0, 128 - int(degrees))
# Delay for i2c communication.
time.sleep(i2c.globI2cDelay)
# Release i2c communication for this thread.
i2c.globI2cLock.release()
# Delay to let camera image stabilize.
time.sleep(delay)
def range_sensor(
self,
sensor,
fs_sel=0
): # Selects full scale range of gyroscopes ('GYRO') or accelerometer ('ACCEL') with fs_sel = 0,1,2,3
fs_sel = 'FS_SEL' + str(
fs_sel) # fs_sel choose the scale of the sensor
scale = sensor + '_SCALE_' + fs_sel
if sensor == 'GYRO': # sensor is used to determine which range sensor change and its config register
self.gyro_scale = self.scales[scale]
elif sensor == 'ACCEL':
self.accel_scale = self.scales[scale]
sensor += '_CONFIG'
write_byte(self.bus, self.i2c_adr, self.regs[sensor],
self.bits[fs_sel])
def sleep(
self,
noise=None
): # Put mpu6050 to sleep (default mode at power up) except when Noise=Something (then this is just a check, True = Sleep)
mode = read_byte(self.bus, self.i2c_adr, self.regs['PWR_MGMT_1'])
sleep = mode & self.bits['SLEEP'] # And take the SLEEP bit
if sleep: # If mpu is sleeping (PWR_MGMT_1 reg, SLEEP bit is 1)
return True # Exit, alredy sleeping, nothing to do here
else: # If mpu is not sleeping...
if noise is not None: # ...and there is noise, it wont sleep:
return False # Exit without sleeping
# Mpu is not sleeping but there is not noise so it will sleep:
mode = mode | self.bits['SLEEP'] # Copy MODE_1 byte but set SLEEP bit
write_byte(self.bus, self.i2c_adr, self.regs['PWR_MGMT_1'],
mode) # power_mgmt_1 reg = X1XX XXXX
return True # Mpu in bed
def sample_rate(
self, sr
): # Set a sample rate = sr (and return the SMPRT_DIV register value)
config = read_byte(self.data, self.i2c_adr, self.regs['CONFIG'])
dlpf = config & 0b111
if (dlpf == 0) or (dlpf == 7):
self.dlpf_enabled = False # Digital Low Pass Filter disabled
gyr = 8000 # Gyroscope Output Rate = 8000Hz
sr_div = (
gyr / sr
) - 1 # From datasheet; Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
else:
self.dlpf_enabled = True
gyr = 1000 # Gyroscope Output Rate = 1000Hz
sr_div = (gyr / sr) - 1
write_byte(self.bus, self.i2c_adr, self.regs['SMPRT_DIV'], sr_div)
return sr_div
def readCompass(debug = False):
# Create i2c lock if it does not exist yet.
i2c.createI2cLock()
# Lock i2c communication for this thread.
i2c.globI2cLock.acquire()
i2c.write_byte(slaveAddressCompass, 0, 0b01110000) # Set to 8 samples @ 15Hz.
i2c.write_byte(slaveAddressCompass, 1, 0b00100000) # 1.3 gain LSb / Gauss 1090 (default).
i2c.write_byte(slaveAddressCompass, 2, 0b00000000) # Continuous-Measurement Mode.
time.sleep(0.006) # Wait 6 ms as specified in data sheet.
x_out_raw = i2c.read_word_2c(slaveAddressCompass, 3)
y_out_raw = i2c.read_word_2c(slaveAddressCompass, 7)
z_out_raw = i2c.read_word_2c(slaveAddressCompass, 5)
# Calibration procedure:
# Run calibrateCompass() and below fill in the resulting values.
# Run testCompass() and position the robot such that raw degrees equals 0.
# Then physically rotate the robot 180 degrees and fill in raw degrees in rawDegreesAt180Degrees below.
# This calibration will result in x_out and y_out varying between -200 and +200.
# Because the HMC5883L is not exactly linear or mounted exactly horizontal,
# we apply an extra correction (offsetCorrectionAt180Degrees) at 180 degrees to straigten the curve.
x_factor = 0.90
x_offset = 148.42
y_factor = 0.90
y_offset = 131.38
rawDegreesAt180Degrees = 172.0
offsetCorrectionAt180Degrees = 180.0 - rawDegreesAt180Degrees
# When supplying a 'True' as parameter to this function the raw X Y Z data and the corrected data will be printed.
# From measurements of these raw values it shows that we have to compensate quite a bit with a gain and offset.
# This is because the HMC5883L is mounted on the robot and is very sensitive to surrounding metal and fields.
# We apply a scale and ofset to both x_out and y_out so the range will be [-200 .. 200]
x_out = x_out_raw * x_factor + x_offset
y_out = y_out_raw * y_factor + y_offset
bearing = math.atan2(y_out, x_out)
if (bearing < 0):
bearing += 2 * math.pi
degrees_raw = math.degrees(bearing)
# Correct for offset at 180 degrees.
if degrees_raw < 180:
degrees = degrees_raw + (degrees_raw / 180.0) * offsetCorrectionAt180Degrees
else:
degrees = degrees_raw + (360.0 - degrees_raw) / 180.0 * offsetCorrectionAt180Degrees
# Delay for i2c communication.
time.sleep(i2c.globI2cDelay)
# Release i2c communication for this thread.
i2c.globI2cLock.release()
if debug:
return x_out_raw, y_out_raw, z_out_raw, x_out, y_out, degrees_raw, degrees
else:
return degrees
def driveAndTurn(speedStraight, speedTurn, delayDrive, delayTurn,
delayAfterMove, doMove):
if doMove:
# Create i2c lock if it does not exist yet.
i2c.createI2cLock()
# Lock i2c communication for this thread.
i2c.globI2cLock.acquire()
# I2C command 1.
i2c.write_byte(slaveAddressArduino, 0, 1)
# Because the I2C parameters are in the range of [128..255], speed range [-63..63] is mapped to [129..255].
# Start with 129 to keep backward / forward or left / right symmetry around 192.
i2c.write_byte(slaveAddressArduino, 0, int(speedStraight) + 192)
i2c.write_byte(slaveAddressArduino, 0, int(speedTurn) + 192)
# Because the I2C parameters are in the range of [128..255], delay range [0..127] is mapped to [128..255].
i2c.write_byte(slaveAddressArduino, 0, int(delayDrive) + 128)
i2c.write_byte(slaveAddressArduino, 0, int(delayTurn) + 128)
# Delay for i2c communication.
time.sleep(i2c.globI2cDelay)
# Release i2c communication for this thread.
i2c.globI2cLock.release()
# Still delay when doMove == False to have similar timing.
time.sleep(delayAfterMove)
def switchLight(on):
if on == True:
# Create i2c lock if it does not exist yet.
i2c.createI2cLock()
# Lock i2c communication for this thread.
i2c.globI2cLock.acquire()
# I2C command 20.
i2c.write_byte(slaveAddressArduino, 0, 20)
# Delay for i2c communication.
time.sleep(i2c.globI2cDelay)
# Release i2c communication for this thread.
i2c.globI2cLock.release()
else:
# Create i2c lock if it does not exist yet.
i2c.createI2cLock()
# Lock i2c communication for this thread.
i2c.globI2cLock.acquire()
# I2C command 21.
i2c.write_byte(slaveAddressArduino, 0, 21)
# Delay for i2c communication.
time.sleep(i2c.globI2cDelay)
# Release i2c communication for this thread.
i2c.globI2cLock.release()
def en_data_interrupts(self):
interrupts = read_byte(self.data, self.i2c_adr,
self.regs['INT_ENABLE'])
interrupts = interrupts | self.bits['DATA_RDY_EN']
write_byte(self.bus, self.i2c_adr, self.regs['INT_ENABLE'], interrupts)
def dis_interrupts(self): # Disable all interrupts
write_byte(self.bus, self.i2c_adr, self.regs['INT_ENABLE'], 0x00)
def just_gyro_accel_fifo(
self
): # Set gx,gy,gz,accel bits of FIFO_EN; gyro and accel data from the sensor registers will be loaded into the fifo buffer
write_byte(self.bus, self.i2c_adr, self.regs['FIFO_EN'], 0xb01111000)
def dis_fifo(
self
): # Clear all bits of FIFO_EN; nothing will be loaded into the fifo buffer
write_byte(self.bus, self.i2c_adr, self.regs['FIFO_EN'], 0x00)