class BNO055: reg = dict( VECTOR_ACCELEROMETER=0x08, VECTOR_MAGNETOMETER=0x0e, VECTOR_GYROSCOPE=0x14, VECTOR_EULER=0x1a, VECTOR_LINEARACCEL=0x28, VECTOR_GRAVITY=0x2e, QUATERNION_DATA=0x20, TEMPERATURE=0x34, CHIP_ID=0x00, SYS_TRIGGER=0x3f, OPR_MODE=0x3d, PAGE_ID=0x07, PWR_MODE=0x3e, ACCEL_OFFSET_X_LSB_ADDR=0x55, CALIB_STAT_ADDR=0x35, ) modes = dict( CONFIG=0x00, NDOF=0x0c, ) power_modes = dict(NORMAL=0x00, LOW=0x01, SUSPEND=0x02) BNO055_ID = 0xa0 NUM_BNO055_OFFSET_REGISTERS = 22 def __init__(self, bus, reset_pin=None, default_address=True, declination=(0, 0)): self.i2c = pyb.I2C(bus, pyb.I2C.MASTER) if reset_pin is not None: self.reset_pin = pyb.Pin(reset_pin, pyb.Pin.OUT_PP) else: self.reset_pin = reset_pin if default_address: self.address = 0x28 else: self.address = 0x29 self.declination = \ (declination[0] + declination[1] / 60) * math.pi / 180 self.quat_scale = 1.0 / (1 << 14) self.sample_delay = 100 self.default_mode = self.modes['NDOF'] self.offsets = OrderedDict(accel_offset_x=0, accel_offset_y=0, accel_offset_z=0, mag_offset_x=0, mag_offset_y=0, mag_offset_z=0, gyro_offset_x=0, gyro_offset_y=0, gyro_offset_z=0, accel_radius=0, mag_radius=0) self.init_sensor() def init_sensor(self): pyb.delay(1000) addresses = self.i2c.scan() if self.address not in addresses: raise Exception("Address %s not found during scan: %s" % (self.address, addresses)) if not self.i2c.is_ready(self.address): raise Exception("Device not ready") pyb.delay(50) chip_id = self.read_8(self.reg['CHIP_ID']) if ord(chip_id) != self.BNO055_ID: pyb.delay(1000) # wait for boot chip_id = self.read_8(self.reg['CHIP_ID']) if ord(chip_id) != self.BNO055_ID: raise Exception("Chip ID invalid:", chip_id) self.set_mode(self.modes['CONFIG']) self.write_8(self.reg['SYS_TRIGGER'], 0x20) # reset pyb.delay(1000) # while ord(self.read_8(self.reg['CHIP_ID'])) != self.BNO055_ID: # pyb.delay(10) self.write_8(self.reg['PWR_MODE'], self.power_modes['NORMAL']) pyb.delay(10) self.write_8(self.reg['PAGE_ID'], 0) self.write_8(self.reg['SYS_TRIGGER'], 0x0) pyb.delay(10) self.set_mode(self.default_mode) pyb.delay(20) pyb.delay(100) self.set_ext_crystal_use() pyb.delay(100) def reset(self): if self.reset_pin is not None: self.reset_pin.low() pyb.delay(1) self.reset_pin.high() self.init_sensor() else: print("No reset pin defined. BNO055 not reset") def set_mode(self, mode): self.write_8(self.reg['OPR_MODE'], mode) pyb.delay(30) def set_ext_crystal_use(self): self.set_mode(self.modes['CONFIG']) pyb.delay(25) self.write_8(self.reg['PAGE_ID'], 0) self.write_8(self.reg['SYS_TRIGGER'], 0x80) pyb.delay(10) self.set_mode(self.default_mode) pyb.delay(20) def get_lin_accel(self): # acceleration in m/s^2 (excluding gravity) x, y, z = self.get_vector('VECTOR_LINEARACCEL') return x / 100.0, y / 100.0, z / 100.0 def get_gyro(self): # angular velocity in radians per second x, y, z = self.get_vector('VECTOR_GYROSCOPE') return x / 900.0, y / 900.0, z / 900.0 def get_quat(self): # quaternion vector (see: https://en.wikipedia.org/wiki/Quaternion) buf = self.read_len(self.reg['QUATERNION_DATA'], 8) w = (buf[1] << 8) | buf[0] x = (buf[3] << 8) | buf[2] y = (buf[5] << 8) | buf[4] z = (buf[7] << 8) | buf[6] return (w * self.quat_scale, x * self.quat_scale, y * self.quat_scale, z * self.quat_scale) def get_euler(self): # yaw, pitch, roll in degrees z, y, x = self.get_vector('VECTOR_EULER') return z / 16.0, y / 16.0, x / 16.0 def get_temp(self): # get temperature in degrees celsius return ord(self.read_8(self.reg['TEMPERATURE'])) def get_accel(self): # acceleration in m/s^2 (including gravity) x, y, z = self.get_vector('VECTOR_ACCELEROMETER') return x / 100.0, y / 100.0, z / 100.0 def get_grav(self): # gravity vector in m/s^2 x, y, z = self.get_vector('VECTOR_GRAVITY') return x / 100.0, y / 100.0, z / 100.0 def get_mag(self): # magnetic field strength in micro-Teslas x, y, z = self.get_vector('VECTOR_MAGNETOMETER') return x / 16.0, y / 16.0, z / 16.0 def get_vector(self, vector_type): # get an x, y, z data array from I2C buf = self.read_len(self.reg[vector_type], 6) data = [] for index in range(0, len(buf), 2): datum = (buf[index + 1] << 8) | buf[index] if datum > 0x7fff: datum -= 0x10000 data.append(datum) return data def get_calibration(self): calib_status = ord(self.read_8(self.reg['CALIB_STAT_ADDR'])) system = (calib_status >> 6) & 0x03 gyro = (calib_status >> 4) & 0x03 accel = (calib_status >> 2) & 0x03 mag = calib_status & 0x03 return system, gyro, accel, mag def is_fully_calibrated(self): for status in self.get_calibration()[1:]: if status < 3: return False return True def update_offsets(self): if self.is_fully_calibrated(): self.set_mode(self.modes['CONFIG']) calib_data = self.read_len(self.reg['ACCEL_OFFSET_X_LSB_ADDR'], self.NUM_BNO055_OFFSET_REGISTERS) self.set_mode(self.default_mode) index = 0 for offset in self.offsets.keys(): self.offsets[offset] = ( calib_data[index + 1] << 8) | calib_data[index] index += 2 # print(self.offsets) return True else: return False def get_heading(self): # compass heading in radians (be sure to get the correct declination) x, y, z = self.get_mag() heading = math.atan2(y, x) heading += self.declination # Correct for reversed heading if heading < 0: heading += 2 * math.pi # Check for wrap and compensate elif heading > 2 * math.pi: heading -= 2 * math.pi return heading def write_8(self, register, data): return self.i2c.mem_write(data, self.address, register) def read_8(self, register): return self.i2c.mem_read(1, self.address, register) def read_len(self, register, length): # try: return self.i2c.mem_read(length, self.address, register)
class BNO055: reg = dict( VECTOR_ACCELEROMETER=0x08, VECTOR_MAGNETOMETER=0x0e, VECTOR_GYROSCOPE=0x14, VECTOR_EULER=0x1a, VECTOR_LINEARACCEL=0x28, VECTOR_GRAVITY=0x2e, QUATERNION_DATA=0x20, TEMPERATURE=0x34, CHIP_ID=0x00, SYS_TRIGGER=0x3f, OPR_MODE=0x3d, PAGE_ID=0x07, PWR_MODE=0x3e, ACCEL_OFFSET_X_LSB_ADDR=0x55, CALIB_STAT_ADDR=0x35, ) modes = dict( CONFIG=0x00, NDOF=0x0c, ) power_modes = dict( NORMAL=0x00, LOW=0x01, SUSPEND=0x02 ) BNO055_ID = 0xa0 NUM_BNO055_OFFSET_REGISTERS = 22 def __init__(self, bus, reset_pin=None, default_address=True, declination=(0, 0)): print(bus) self.i2c = pyb.I2C(bus, pyb.I2C.MASTER) if reset_pin is not None: self.reset_pin = pyb.Pin(reset_pin, pyb.Pin.OUT_PP) else: self.reset_pin = reset_pin if default_address: self.address = 0x28 else: self.address = 0x29 self.declination = \ (declination[0] + declination[1] / 60) * math.pi / 180 self.quat_scale = 1.0 / (1 << 14) self.sample_delay = 100 self.default_mode = self.modes['NDOF'] self.offsets = OrderedDict( accel_offset_x=0, accel_offset_y=0, accel_offset_z=0, mag_offset_x=0, mag_offset_y=0, mag_offset_z=0, gyro_offset_x=0, gyro_offset_y=0, gyro_offset_z=0, accel_radius=0, mag_radius=0 ) self.init_sensor() def init_sensor(self): pyb.delay(1000) addresses = self.i2c.scan() if self.address not in addresses: raise Exception("Address %s not found during scan: %s" % ( self.address, addresses)) if not self.i2c.is_ready(self.address): raise Exception("Device not ready") pyb.delay(50) chip_id = self.read_8(self.reg['CHIP_ID']) if ord(chip_id) != self.BNO055_ID: pyb.delay(1000) # wait for boot chip_id = self.read_8(self.reg['CHIP_ID']) if ord(chip_id) != self.BNO055_ID: raise Exception("Chip ID invalid:", chip_id) self.set_mode(self.modes['CONFIG']) self.write_8(self.reg['SYS_TRIGGER'], 0x20) # reset pyb.delay(1000) # while ord(self.read_8(self.reg['CHIP_ID'])) != self.BNO055_ID: # pyb.delay(10) self.write_8(self.reg['PWR_MODE'], self.power_modes['NORMAL']) pyb.delay(10) self.write_8(self.reg['PAGE_ID'], 0) self.write_8(self.reg['SYS_TRIGGER'], 0x0) pyb.delay(10) self.set_mode(self.default_mode) pyb.delay(20) pyb.delay(100) self.set_ext_crystal_use() pyb.delay(100) def reset(self): if self.reset_pin is not None: self.reset_pin.low() pyb.delay(1) self.reset_pin.high() self.init_sensor() else: print("No reset pin defined. BNO055 not reset") def set_mode(self, mode): self.write_8(self.reg['OPR_MODE'], mode) pyb.delay(30) def set_ext_crystal_use(self): self.set_mode(self.modes['CONFIG']) pyb.delay(25) self.write_8(self.reg['PAGE_ID'], 0) self.write_8(self.reg['SYS_TRIGGER'], 0x80) pyb.delay(10) self.set_mode(self.default_mode) pyb.delay(20) def get_lin_accel(self): # acceleration in m/s^2 (excluding gravity) x, y, z = self.get_vector('VECTOR_LINEARACCEL') return x / 100.0, y / 100.0, z / 100.0 def get_gyro(self): # angular velocity in rotations per second x, y, z = self.get_vector('VECTOR_GYROSCOPE') return x / 900.0, y / 900.0, z / 900.0 def get_quat(self): # quaternion vector (see: https://en.wikipedia.org/wiki/Quaternion) buf = self.read_len(self.reg['QUATERNION_DATA'], 8) w = (buf[1] << 8) | buf[0] x = (buf[3] << 8) | buf[2] y = (buf[5] << 8) | buf[4] z = (buf[7] << 8) | buf[6] return (w * self.quat_scale, x * self.quat_scale, y * self.quat_scale, z * self.quat_scale) def get_euler(self): # yaw, pitch, roll in degrees z, y, x = self.get_vector('VECTOR_EULER') return z / 16.0, y / 16.0, x / 16.0 def get_temp(self): # get temperature in degrees celsius return ord(self.read_8(self.reg['TEMPERATURE'])) def get_accel(self): # acceleration in m/s^2 (including gravity) x, y, z = self.get_vector('VECTOR_ACCELEROMETER') return x / 100.0, y / 100.0, z / 100.0 def get_grav(self): # gravity vector in m/s^2 x, y, z = self.get_vector('VECTOR_GRAVITY') return x / 100.0, y / 100.0, z / 100.0 def get_mag(self): # magnetic field strength in micro-Teslas x, y, z = self.get_vector('VECTOR_MAGNETOMETER') return x / 16.0, y / 16.0, z / 16.0 def get_vector(self, vector_type): # get an x, y, z data array from I2C buf = self.read_len(self.reg[vector_type], 6) data = [] for index in range(0, len(buf), 2): datum = (buf[index + 1] << 8) | buf[index] if datum > 0x7fff: datum -= 0x10000 data.append(datum) return data def get_calibration(self): calib_status = ord(self.read_8(self.reg['CALIB_STAT_ADDR'])) system = (calib_status >> 6) & 0x03 gyro = (calib_status >> 4) & 0x03 accel = (calib_status >> 2) & 0x03 mag = calib_status & 0x03 return system, gyro, accel, mag def is_fully_calibrated(self): for status in self.get_calibration()[1:]: if status < 3: return False return True def update_offsets(self): if self.is_fully_calibrated(): self.set_mode(self.modes['CONFIG']) calib_data = self.read_len(self.reg['ACCEL_OFFSET_X_LSB_ADDR'], self.NUM_BNO055_OFFSET_REGISTERS) self.set_mode(self.default_mode) index = 0 for offset in self.offsets.keys(): self.offsets[offset] = (calib_data[index + 1] << 8) | calib_data[index] index += 2 # print(self.offsets) return True else: return False def get_heading(self): # compass heading in radians (be sure to get the correct declination) x, y, z = self.get_mag() heading = math.atan2(y, x) heading += self.declination # Correct for reversed heading if heading < 0: heading += 2 * math.pi # Check for wrap and compensate elif heading > 2 * math.pi: heading -= 2 * math.pi return heading def write_8(self, register, data): return self.i2c.mem_write(data, self.address, register) def read_8(self, register): return self.i2c.mem_read(1, self.address, register) def read_len(self, register, length): # try: return self.i2c.mem_read(length, self.address, register)
class Led: def __init__(self, scr, num, pin, rings=None): self.screen = scr self.screen.print("{:d} leds, pin {:d}".format(num, pin)) self.leds = neopixel.NeoPixel(machine.Pin(pin), num, timing=1, use_dma=True) self.leds.fill((0, 0, 0)) self.leds.write() self._patterns = OrderedDict([("rainbow", self.pat_rainbow), ("cylon", self.pat_bounce), ("rainbow cylon", self.pat_rainbowcyl), ("marquee", self.pat_marquee), ("solid", self.pat_solid), ("pulse", self.pat_pulse), ("rgb party", self.pat_rgb_party), ("flame", self.pat_flame), ("flame_g", self.pat_flame_g), ("flame_b", self.pat_flame_b)]) self._oneshots = OrderedDict([ ("bump", self.one_bump), ("whoosh", self.one_whoosh), ("rgb", self.one_rgb), ]) if (rings): ring_pats = OrderedDict([("r_radar", self.pat_radar), ("r_tunnel", self.pat_tunnel), ("r_bubbles", self.pat_bubbles)]) self.rings = rings self._patterns.update(ring_pats) else: self.rings = None self.hue = 0 self._colors = OrderedDict([("red", 0), ("orange", 30), ("yellow", 50), ("green", 120), ("blue", 240), ("indigo", 260), ("violet", 280)]) self.intens = 0.2 # 0-1, float self._active = self.pat_rainbow self.period = 0.2 # seconds self.stop_thread = False self.pat_chg = False _thread.start_new_thread(self.led_timer_thread, (None, )) def led_timer_thread(self, unused): num = self.leds.n while True: if (not self.stop_thread): self._active(num) sleep(self.period) self.pat_chg = False def led_timer_stop(self): self.stop_thread = True self.pat_chg = True def led_timer_start(self): if (self.stop_thread): self.stop_thread = False # RGB/HSV stuff from http://code.activestate.com/recipes/576919-python-rgb-and-hsv-conversion/ def hsv2rgb(self, h, s, v): h = float(h) s = float(s) v = float(v) h60 = h / 60.0 h60f = math.floor(h60) hi = int(h60f) % 6 f = h60 - h60f p = v * (1 - s) q = v * (1 - f * s) t = v * (1 - (1 - f) * s) r, g, b = 0, 0, 0 if hi == 0: r, g, b = v, t, p elif hi == 1: r, g, b = q, v, p elif hi == 2: r, g, b = p, v, t elif hi == 3: r, g, b = p, q, v elif hi == 4: r, g, b = t, p, v elif hi == 5: r, g, b = v, p, q r, g, b = int(r * 255), int(g * 255), int(b * 255) return r, g, b def rgb2hsv(self, r, g, b): r, g, b = r / 255.0, g / 255.0, b / 255.0 mx = max(r, g, b) mn = min(r, g, b) df = mx - mn if mx == mn: h = 0 elif mx == r: h = (60 * ((g - b) / df) + 360) % 360 elif mx == g: h = (60 * ((b - r) / df) + 120) % 360 elif mx == b: h = (60 * ((r - g) / df) + 240) % 360 if mx == 0: s = 0 else: s = df / mx v = mx return h, s, v @property def patterns(self): return list(self._patterns.keys()) @property def oneshots(self): return list(self._oneshots.keys()) @property def colors(self): return list(self._colors.keys()) @property def color_str(self): try: return next(key for key, self.hue in self._colors.items()) except StopIteration: return "red" @color_str.setter def color_str(self, name): self.hue = self._colors[name] @property def active_pat(self): try: return self.patterns[list(self._patterns.values()).index( self._active)] except StopIteration: raise ValueError @active_pat.setter def active_pat(self, name): self.screen.print("Selected " + name) self._active = self._patterns[name] self.pat_chg = True def do_oneshot(self, name): self.screen.print("OneShot " + name) self.pat_chg = True self.stop_thread = True self._oneshots[name]() self.stop_thread = False def pat_rainbow(self, num): step = 360 / num pos = 0 while (not self.pat_chg): for i in range(0, num): hue = ((i + pos) * step) % 360 rgb = self.hsv2rgb(hue, 1, self.intens) #print("hue {:f} pos {:d} rgb ".format(hue, self.pos) + str(rgb)) self.leds[i] = rgb self.leds.write() pos = (pos + 1) % num sleep(self.period) def pat_bounce(self, num): pos = 0 reverse = 0 while (not self.pat_chg): if (reverse): i = num - pos - 1 else: i = pos self.leds[i] = self.hsv2rgb(self.hue, 1, self.intens) self.leds.write() self.leds[i] = (0, 0, 0) if (pos == (num - 1)): reverse = not reverse pos = (pos + 1) % num sleep(self.period / 8) def pat_marquee(self, num): pos = 0 while (not self.pat_chg): for i in range(0, num): if ((i + pos) % 4 == 0): self.leds[i] = self.hsv2rgb(self.hue, 1, self.intens) else: self.leds[i] = (0, 0, 0) self.leds.write() pos = (pos + 1) % num sleep(self.period / 2) def pat_rainbowcyl(self, num): pos = 0 reverse = 0 step = 360 / num while (not self.pat_chg): if (reverse): i = num - pos - 1 else: i = pos hue = (pos * step) % 360 self.leds[i] = self.hsv2rgb(hue, 1, self.intens) self.leds.write() self.leds[i] = (0, 0, 0) if (pos == (num - 1)): reverse = not reverse pos = (pos + 1) % num sleep(self.period / 8) def pat_solid(self, num): self.leds.fill(self.hsv2rgb(self.hue, 1, self.intens)) self.leds.write() self.led_timer_stop() def pat_pulse(self, num): pos = 0 pulsing = 0 while (not self.pat_chg): if (pos == 0): pulsing = not pulsing self.leds.fill(self.hsv2rgb(self.hue, 1, self.intens)) if (pulsing): for i in range(pos - 8, pos): if (i >= 0): self.leds[i] = self.hsv2rgb( self.hue, 1, self.intens / (2**(9 - (pos - i)))) self.leds[pos] = (0, 0, 0) elif (pos < 8): for i in range(1, 9 - pos): self.leds[num - i] = self.hsv2rgb( self.hue, 1, self.intens / (2**(9 - (i + pos)))) self.leds.write() pos = (pos + 1) % num sleep(self.period / 4) def pat_radar(self, num): self.leds.fill((0, 0, 0)) pos = 0 while (not self.pat_chg): self.leds.fill((0, 0, 0)) for j, len in enumerate(self.rings): offset = sum(self.rings[:j]) index = (pos % len) + offset self.leds[index] = self.hsv2rgb(self.hue, 1, self.intens) self.leds.write() pos = (pos + 1) % num sleep(self.period / 4) def pat_tunnel(self, num): ring = 0 while (not self.pat_chg): self.leds.fill((0, 0, 0)) offset = sum(self.rings[:ring]) for i in range(offset, offset + self.rings[ring]): self.leds[i] = self.hsv2rgb(self.hue, 1, self.intens) self.leds.write() ring = (ring + 1) % len(self.rings) sleep(self.period) def pat_bubbles(self, num): ring = 0 self.leds.fill((0, 0, 0)) while (not self.pat_chg): self.leds.fill((0, 0, 0)) ring = random.randint(0, len(self.rings) - 1) offset = sum(self.rings[:ring]) color = self.hsv2rgb(random.randint(0, 359), 1, self.intens) for i in range(offset, offset + self.rings[ring]): self.leds[i] = color self.leds.write() sleep(self.period) def pat_rgb_party(self, num): pos = 0 cycle = 0 while (not self.pat_chg): self.leds[pos] = self.hsv2rgb(cycle * 90, 1, self.intens) self.leds.write() pos = (pos + 1) % num if (pos == 0): cycle = (cycle + 1) % 3 sleep(self.period / 16) # adapted from an arduino pattern at: # http://www.funkboxing.com/wordpress/wp-content/_postfiles/fluxbox_octo.ino def _pat_flame_internal(self, hmin, hmax, num): acolor = (0, 0, 0) hdif = hmax - hmin ahue = hmin self.leds.fill((0, 0, 0)) self.leds.write() while (not self.pat_chg): idelay = random.randint(1, 10) randtemp = random.randint(0, 3) hinc = (hdif / float(num)) + randtemp spread = random.randint(1, 5) start = random.randint(0, num - spread) for i in range(start, start + spread): if ((ahue + hinc) > hmax): ahue = hmin else: ahue = ahue + hinc acolor = self.hsv2rgb(ahue, 1, self.intens) self.leds[i] = acolor self.leds[num - i - 1] = acolor self.leds.write() sleep(idelay / 100.0) def pat_flame(self, num): self._pat_flame_internal(0.1, 40.0, num) def pat_flame_g(self, num): self._pat_flame_internal(80.0, 160.0, num) def pat_flame_b(self, num): self._pat_flame_internal(180.0, 280.0, num) def one_bump(self): time = self.period / 4 for i in range(0, 4): self.leds.fill(self.hsv2rgb(self.hue, 1, self.intens)) self.leds.write() sleep(time) self.leds.fill((0, 0, 0)) self.leds.write() sleep(time) def one_whoosh(self): color = self.hsv2rgb(self.hue, 1, self.intens) for i in range(0, self.leds.n): self.leds[i] = color if (i - 1 > 0): self.leds[i - 1] = color if (i - 2 > 0): self.leds[i - 2] = color if (i - 3 > 0): self.leds[i - 3] = color if (i - 4 > 0): self.leds[i - 4] = color self.leds.write() def one_rgb(self): time = self.period / 2 self.leds.fill((0, 0, 0)) self.leds.write() sleep(time) self.leds.fill((255, 0, 0)) self.leds.write() sleep(time) self.leds.fill((0, 255, 0)) self.leds.write() sleep(time) self.leds.fill((0, 0, 255)) self.leds.write() sleep(time)