def __init__(self): i2c = busio.I2C(board.SCL, board.SDA) self.sensor = adafruit_lsm303.LSM303(i2c) self.FORWARD = self.heading() self.theta = atan2( self.sensor.acceleration[0], sqrt(self.sensor.acceleration[1]**2 + self.sensor.acceleration[2]**2)) self.psi = atan2( self.sensor.acceleration[1], sqrt(self.sensor.acceleration[0]**2 + self.sensor.acceleration[2]**2))
""" Display both accelerometer and magnetometer data once per second """ import time import board import busio import adafruit_lsm303 i2c = busio.I2C(board.SCL, board.SDA) sensor = adafruit_lsm303.LSM303(i2c) while True: raw_accel_x, raw_accel_y, raw_accel_z = sensor.raw_acceleration accel_x, accel_y, accel_z = sensor.acceleration raw_mag_x, raw_mag_y, raw_mag_z = sensor.raw_magnetic mag_x, mag_y, mag_z = sensor.magnetic print( 'Acceleration raw: ({0:6d}, {1:6d}, {2:6d}), (m/s^2): ({3:10.3f}, {4:10.3f}, {5:10.3f})' .format(raw_accel_x, raw_accel_y, raw_accel_z, accel_x, accel_y, accel_z)) print( 'Magnetometer raw: ({0:6d}, {1:6d}, {2:6d}), (gauss): ({3:10.3f}, {4:10.3f}, {5:10.3f})' .format(raw_mag_x, raw_mag_y, raw_mag_z, mag_x, mag_y, mag_z)) print('') time.sleep(1.0)
import numpy import math import time import board import busio import adafruit_l3gd20 import adafruit_lsm303 import matplotlib.pyplot as plt outf = open('output.txt', 'a') outf.truncate(0) # Hardware I2C setup: I2C = busio.I2C(board.SCL, board.SDA) sensorGyro = adafruit_l3gd20.L3GD20_I2C(I2C) sensorCompass = adafruit_lsm303.LSM303(I2C) sensorCompass.mag_rate = adafruit_lsm303.MAGRATE_220 accsX = [] accsY = [] accsZ = [] counter = 0 angles = numpy.arange(-90, 100, 10) xReals = [] yReals = [] def readAngle(): acc_x, acc_y, acc_z = sensorCompass.acceleration acc_z -= 0.74
import time import math import sys import board import busio import adafruit_lsm303 import neopixel import digitalio BLACK = 0x000000 RED = 0xFF0000 GREEN = 0x00FF00 BLUE = 0x0000FF i2c = busio.I2C(board.SCL, board.SDA) compass = adafruit_lsm303.LSM303(i2c) compass.mag_rate = adafruit_lsm303.MAGRATE_30 #compass.mag_gain = adafruit_lsm303.MAGGAIN_8_1 calibrate_button = digitalio.DigitalInOut(board.BUTTON_A) calibrate_button.direction = digitalio.Direction.INPUT calibrate_button.pull = digitalio.Pull.UP #------------------------------------------------------------------------------- # Replace these two lines with the results of calibration #------------------------------------------------------------------------------- raw_mins = [1000.0, 1000.0] raw_maxes = [-1000.0, -1000.0] #-------------------------------------------------------------------------------
if beschleunigung_x >= 3: print("Bergab!") FAHRGESCHWINDIGKEIT = 30 elif beschleunigung_x <= -3: print("Bergauf!") FAHRGESCHWINDIGKEIT = 75 else: FAHRGESCHWINDIGKEIT = 50 def scale(wert, a, b, c, d): wert = (wert-a)*((d-c)/(b-a))+c return wert try: i2c_bus = busio.I2C(board.SCL, board.SDA) beschl_kompass_sensor = adafruit_lsm303.LSM303(i2c_bus) gyro_sensor = adafruit_l3gd20.L3GD20_I2C(i2c_bus) kamera = PiCamera(resolution=(PIXEL_BREITE, PIXEL_HOEHE), framerate=32) speichert_einen_frame = numpy.empty((PIXEL_HOEHE, PIXEL_BREITE, 3), dtype=numpy.uint8) motorsteuerung = AFMotorShield( pin_clk=23, pin_data=6, pin_enable=5, pin_latch=26 ) motor_links = DCMotor(motorsteuerung, 2, pwm_pin=22, reversed=True
def loop(self): """ Magnet sensor loop. Watches the LSM303 magnetometer values and, if they jump by a great enough threshold, initiates the berry-selected message. """ logging.info('Entering magnet selection loop') try: import board import busio import time import adafruit_lsm303 i2c = busio.I2C(board.SCL, board.SDA) sensor = adafruit_lsm303.LSM303(i2c) count = 0 # Get initial reading by first waiting two seconds (so we ignore # the useless initial value) and then watch for 600ms (200ms three # times) and average the values together time.sleep(1) mag_readings = [] for i in range(0, MAG_NUMBER_VALUES): mag_readings.insert(0, sensor.magnetic) time.sleep(INITIAL_MAGNET_SENSOR_DELAY) average_x = sum([r[0] for r in mag_readings]) / float(MAG_NUMBER_VALUES) average_y = sum([r[1] for r in mag_readings]) / float(MAG_NUMBER_VALUES) average_z = sum([r[2] for r in mag_readings]) / float(MAG_NUMBER_VALUES) while True: mag = sensor.magnetic if mag is not None: # Check if we're above threshold and if so, send the message try: change_rate_x = abs(mag[0] / average_x) except Exception: change_rate_x = 0 try: change_rate_y = abs(mag[1] / average_y) except Exception: change_rate_y = 0 try: change_rate_z = abs(mag[2] / average_z) except Exception: change_rate_z = 0 if (change_rate_x > MAG_CHANGE_RATE_THRESHOLD or change_rate_y > MAG_CHANGE_RATE_THRESHOLD or change_rate_z > MAG_CHANGE_RATE_THRESHOLD ) and count == 0: self.select() # Don't keep sending select messages until after the # selection delay is over (decrement this each pass through # the loop) count = SELECTION_DELAY_COUNT # Update the average # TODO: make this more elegant mag_readings.insert(0, mag) mag_readings.pop() average_x = sum([r[0] for r in mag_readings ]) / float(MAG_NUMBER_VALUES) average_y = sum([r[1] for r in mag_readings ]) / float(MAG_NUMBER_VALUES) average_z = sum([r[2] for r in mag_readings ]) / float(MAG_NUMBER_VALUES) # Wait time.sleep(MAGNET_SENSOR_DELAY) # Delay count if count > 0: count -= 1 except Exception as ex: logging.error( '\n *** ERROR, magnet sensor thread died: {}'.format(ex, ), )