def get_imu(use_dummy=False):
scl, sda = (22, 23) if sys.platform == "esp32" else (5, 4)
i2c = I2C(scl=Pin(scl), sda=Pin(sda), freq=100000, timeout=1000)
devices = i2c.scan()
print("I2C scan ->", devices)
if 40 not in devices:
if devices:
print("I2C(scl={}, sda={}) devices:".format(scl, sda), devices)
missing_imu_msg = "No IMU @ I2C(scl={}, sda={})".format(scl, sda)
if not use_dummy:
raise Exception(missing_imu_msg)
print(missing_imu_msg + ". Using dummy data.")
return bno055_fake.BNO055()
for i in range(10, 0, -1):
try:
bno = bno055.BNO055(i2c, verbose=config.TRACE_SPI)
print("Using BNO055 @ I2C(scl={}, sda={})".format(scl, sda))
bno.operation_mode(bno055.NDOF_MODE)
return bno
except OSError as err:
if i == 1 or not is_retriable_error(err):
raise err
print("Error finding BNO055: {:s}; retrying".format(str(err)),
file=sys.stderr)
time.sleep_ms(1000)
def __init__(self):
#オブジェクトの生成
self.rightmotor = motor.motor(ct.const.RIGHT_MOTOR_IN1_PIN,
ct.const.RIGHT_MOTOR_IN2_PIN,
ct.const.RIGHT_MOTOR_VREF_PIN)
self.leftmotor = motor.motor(ct.const.LEFT_MOTOR_IN1_PIN,
ct.const.LEFT_MOTOR_IN2_PIN,
ct.const.LEFT_MOTOR_VREF_PIN)
self.gps = gps.GPS()
self.bno055 = bno055.BNO055()
self.radio = radio.radio()
self.RED_LED = led.led(ct.const.RED_LED_PIN)
self.BLUE_LED = led.led(ct.const.BLUE_LED_PIN)
self.GREEN_LED = led.led(ct.const.GREEN_LED_PIN)
#開始時間の記録
self.startTime = time.time()
self.timer = 0
self.landstate = 0 #landing stateの中でモータを一定時間回すためにlandのなかでもステート管理するため
self.v_right = 100
self.v_left = 100
#変数
self.state = 0
self.laststate = 0
self.landstate = 0
#stateに入っている時刻の初期化
self.preparingTime = 0
self.flyingTime = 0
self.droppingTime = 0
self.landingTime = 0
self.pre_motorTime = 0
self.waitingTime = 0
self.runningTime = 0
self.goalTime = 0
#state管理用変数初期化
self.countPreLoop = 0
self.countFlyLoop = 0
self.countDropLoop = 0
self.countGoal = 0
self.countAreaLoopEnd = 0 # 終了判定用
self.countAreaLoopStart = 0 # 開始判定用
self.countAreaLoopLose = 0 # 見失い判定用
self.countgrass = 0
#GPIO設定
GPIO.setmode(GPIO.BCM) #GPIOの設定
GPIO.setup(ct.const.FLIGHTPIN_PIN, GPIO.IN) #フライトピン用
date = datetime.datetime.now()
self.filename = '{0:%Y%m%d}'.format(date)
self.filename_hm = '{0:%Y%m%d%H%M}'.format(date)
if not os.path.isdir(
'/home/pi/Desktop/wolvez2021/Testcode/EtoEtest/%s/' %
(self.filename)):
os.mkdir('/home/pi/Desktop/wolvez2021/Testcode/EtoEtest/%s/' %
(self.filename))
def task3_imu_read():
## BNO055 object is created before while loop
imu = bn.BNO055(bn.ndof)
while True:
## The shared variable q2 holds the current Pitch Value
q2.put(imu.get_sensor_reading(bn.EULER_P) / 16)
## The shared variable q3 holds the current Roll Value
q3.put(imu.get_sensor_reading(bn.EULER_R) / 16)
yield (0)
import motor
import encoder
import bno055 as bn
import closed_loop as loop
import utime
imu = bn.BNO055(bn.ndof)
# while(1):
# utime.sleep_ms(200)
# x = imu.get_sensor_reading(bn.EULER_H)
# x = x/16
# y = imu.get_sensor_reading(bn.EULER_R)
# y = y/16
# z = imu.get_sensor_reading(bn.EULER_P)
# z = z/16
# print("Heading : " + str(x) + " Roll Val: " + str(y) + " Pitch Val: " + str(z))
cL = loop.Closed_Loop()
cL.set_setpoint(0)
while (1):
#print("Enter a gain: ")
gain_set = input()
cL.set_cont_gain(float(gain_set))
mot1 = motor.MotorDriver()
last_time = utime.ticks_ms()
times = []
positions = []
while (1):
utime.sleep_ms(10)
#times.append(utime.ticks_ms())
import time
import RPi.GPIO as GPIO
import sys
import numpy as np
import bno055
GPIO.setmode(GPIO.BCM) #GPIOの設定
bno055 = bno055.BNO055()
GPIO.setmode(GPIO.BCM) #GPIOの設定
bno055.setupBno()
if bno055.begin() is not True:
print("Error initializing device")
exit()
try:
while True:
bno055.bnoread()
bno055.ax=round(bno055.ax,3)
bno055.ay=round(bno055.ay,3)
bno055.az=round(bno055.az,3)
bno055.gx=round(bno055.gx,3)
bno055.gy=round(bno055.gy,3)
bno055.gz=round(bno055.gz,3)
bno055.ex=round(bno055.ex,3)
bno055.ey=round(bno055.ey,3)
bno055.ez=round(bno055.ez,3)
accel="ax="+str(bno055.ax)+","\
+"ay="+str(bno055.ay)+","\
+"az="+str(bno055.az)
scl=pyb.Pin.cpu.C9,
sda=pyb.Pin.cpu.A8,
freq=400000,
timeout=10000)
i.scan()
import bno055
#s = bno055.BNO055(i)
while True:
devices = i.scan()
print("Found devices: %s" % (repr(devices)))
if 41 in devices:
break
s = bno055.BNO055(i, address=41)
time.sleep(1)
#s.operation_mode(bno055.COMPASS_MODE)
s.operation_mode(bno055.NDOF_MODE)
#s.operation_mode(bno055.M4G_MODE)
import math
while True:
try:
ex, ey, ez = s.euler()
w, x, y, z = s.quaternion()
ysqr = y * y
import bno055 from machine import I2C, Pin scl, sda = (Pin(22), Pin(23)) if sys.platform == "esp32" else (Pin(5), Pin(4)) i2c = I2C(scl=scl, sda=sda, timeout=1000) # HUZZAH8266 s = bno055.BNO055(i2c) s.operation_mode(bno055.NDOF_MODE) # s.operation_mode() s.temperature() s.accelerometer() s.magnetometer() s.gyroscope() s.euler()
#
# micropython i2c driver
#
import micropython
import bno055 as imu
from time import sleep
accel_range = int('00000000',2) ##accel = 2g
accel_range = int('00000001',2) ##accel = 4g care if not in fusion mode. top 8 bits impact power and BW
if __name__ == "__main__":
'''Reads or Writes to i2c peripheral. This loop reads and prints 10 euler vectors'''
aye = pyb.I2C (1, pyb.I2C.MASTER)
nerd = imu.BNO055(aye, 0x28, accel_range)
for x in range(10):
x_data = nerd.get_eul()
sleep(100.0/1000.0)
def scan_list():
aye = pyb.I2C (1, pyb.I2C.MASTER)
adr_list = []
adr_list = pyb.I2C.scan(aye)
return adr_list
import logging
import sys
import time
import bno055
bno = bno055.BNO055()
if not bno.begin():
raise RuntimeError('Failed to initialize BNO055! Is the sensor connected?')
status, self_test, error = bno.get_system_status()
print('System status: {0}'.format(status))
print('Self test result (0x0F is normal): 0x{0:02X}'.format(self_test))
# Print out an error if system status is in error mode.
if status == 0x01:
print('System error: {0}'.format(error))
print('See datasheet section 4.3.59 for the meaning.')
# Print BNO055 software revision and other diagnostic data.
sw, bl, accel, mag, gyro = bno.get_revision()
print('Software version: {0}'.format(sw))
print('Bootloader version: {0}'.format(bl))
print('Accelerometer ID: 0x{0:02X}'.format(accel))
print('Magnetometer ID: 0x{0:02X}'.format(mag))
print('Gyroscope ID: 0x{0:02X}\n'.format(gyro))
print('Reading BNO055 data, press Ctrl-C to quit...')
while True:
heading, roll, pitch = bno.read_euler()
print('Heading={0:0.2F} Roll={1:0.2F} Pitch={2:0.2F}'.format(