def start(self): servo.enable() for i in self.clocks: i.start() mpu9250.initialize(enable_dmp=True, dmp_sample_rate=100, enable_fusion=True, enable_magnetometer=True)
def __init__(self): if rcpy.get_state() != rcpy.RUNNING: rcpy.set_state(rcpy.RUNNING) mpu9250.initialize(enable_dmp=True, dmp_sample_rate=100, enable_fusion=True, enable_magnetometer=True) self.i = 0
def train(): rcpy.set_state(rcpy.RUNNING) mpu9250.initialize(enable_magnetometer=False) while True: data = mpu9250.read() print(data['gyro'][0], data['gyro'][1], data['gyro'][2]) time.sleep(0.1)
def __init__(self): mpu9250.initialize(enable_magnetometer = True) print("Press Ctrl-C to exit") # header print(" Accel XYZ (m/s^2) |" " Gyro XYZ (deg/s) |", end='') print(" Mag Field XYZ (uT) |", end='') print(' Temp (C)')
def __init__(self): # set state to rcpy.RUNNING rcpy.set_state(rcpy.RUNNING) # no magnetometer mpu9250.initialize( enable_dmp=True, dmp_sample_rate=200, enable_fusion=True, enable_magnetometer=True ) # start the sensor rcpy.set_state(rcpy.RUNNING)
def main(): # defaults enable_magnetometer = False show_compass = False show_gyro = False show_accel = False show_quat = False show_tb = False sample_rate = 100 enable_fusion = False show_period = False newline = '\r' show_tb = True # set state to rcpy.RUNNING rcpy.set_state(rcpy.RUNNING) # magnetometer ? mpu9250.initialize(enable_dmp=True, dmp_sample_rate=sample_rate, enable_fusion=enable_fusion, enable_magnetometer=enable_magnetometer) try: # keep running while True: # running if rcpy.get_state() == rcpy.RUNNING: t0 = time.perf_counter() data = mpu9250.read() t1 = time.perf_counter() dt = t1 - t0 t0 = t1 print(data) # print('{0[0]:6.2f} {0[1]:6.2f} {0[2]:6.2f} |' # .format(data['tb']), end='') # no need to sleep except KeyboardInterrupt: # Catch Ctrl-C pass finally: # say bye print("\nBye Beaglebone!")
def test2(): N = 200 try: # with dmp, no magnetometer mpu9250.initialize(enable_magnetometer = False, enable_dmp = True) conf = mpu9250.get() assert conf == {'orientation': 136, 'accel_dlpf': 2, 'gyro_dlpf': 2, 'compass_time_constant': 5.0, 'enable_fusion': False, 'enable_dmp': True, 'enable_magnetometer': False, 'accel_fsr': 1, 'dmp_sample_rate': 100, 'show_warnings': False, 'gyro_fsr': 2, 'dmp_interrupt_priority': 98} print('\n Accel XYZ(m/s^2) | Gyro XYZ (rad/s) | Temp (C)') for i in range(N): data = mpu9250.read() print(('\r{0[0]:6.2f} {0[1]:6.2f} {0[2]:6.2f} |' '{1[0]:6.1f} {1[1]:6.1f} {1[2]:6.1f} |') .format(data['accel'], data['gyro']), end='') # with dmp, with magnetometer mpu9250.initialize(enable_magnetometer = True, enable_dmp = True) conf = mpu9250.get() assert conf == {'orientation': 136, 'accel_dlpf': 2, 'gyro_dlpf': 2, 'compass_time_constant': 5.0, 'enable_fusion': False, 'enable_dmp': True, 'enable_magnetometer': True, 'accel_fsr': 1, 'dmp_sample_rate': 100, 'show_warnings': False, 'gyro_fsr': 2, 'dmp_interrupt_priority': 98} print('\n Accel XYZ(m/s^2) | Gyro XYZ (rad/s) | Mag Field XYZ(uT) | Temp (C)') for i in range(N): data = mpu9250.read() print(('\r{0[0]:6.2f} {0[1]:6.2f} {0[2]:6.2f} |' '{1[0]:6.1f} {1[1]:6.1f} {1[2]:6.1f} |' '{2[0]:6.1f} {2[1]:6.1f} {2[2]:6.1f} |') .format(data['accel'], data['gyro'], data['mag']), end='') except (KeyboardInterrupt, SystemExit): pass finally: mpu9250.power_off()
def set(self, exclude=(), **kwargs): #print('MPU9250.set()') # initialize? initialize = kwargs.pop('initialize', False) if not initialize: # look for change initialize = set( ('accel_fsr', 'gyro_fsr', 'accel_dlpf', 'gyro_dlpf', 'enable_magnetometer', 'orientation', 'compass_time_constant', 'dmp_interrupt_priority', 'period', 'show_warnings', 'enable_dmp', 'enable_fusion')).intersection(kwargs.keys()) # call super super().set(exclude + ('data', '_shared_state'), **kwargs) # do initialize? if initialize: # initialize mpu9250 mpu9250.initialize( accel_fsr=self.accel_fsr, gyro_fsr=self.gyro_fsr, accel_dlpf=self.accel_dlpf, gyro_dlpf=self.gyro_dlpf, enable_magnetometer=self.enable_magnetometer, orientation=self.orientation, compass_time_constant=self.compass_time_constant, dmp_interrupt_priority=self.dmp_interrupt_priority, dmp_sample_rate=int(1 / self.period), show_warnings=self.show_warnings, enable_dmp=self.enable_dmp, enable_fusion=self.enable_fusion)
#!/usr/bin/python3 # This program accesses info from the Blue's onboard sensor, MPU9250 # It reads temp, accelerometer, gyro, and magnetometer data from the sensor. # Uses RCPY library. See guitar.ucsd.edu/rcpy/rcpy.pdf for documentation # Import external librarires import time # for time.sleep function import numpy as np # for working with matrices import rcpy # import rcpy library (this automatically initializes robotics cape) import rcpy.mpu9250 as mpu9250 # for mpu sensor functions rcpy.set_state(rcpy.RUNNING) # set state to rcpy.RUNNING mpu9250.initialize(enable_magnetometer=True) # by default, mag is not initialized. mpu9250.initialize() # initialize the sensor def getAccel(): axes = mpu9250.read_accel_data() # returns x, y, z acceleration (m/s^2) axes = np.round(axes, 3) # round values to 3 decimals return(axes) def getAll(): data = mpu9250.read() # this command returns a string with many parameters. return(data) def getTemp(): temp = mpu9250.read_imu_temp() # returns just temperature (deg C) return(temp)
def start(self): rcpy.set_state(rcpy.RUNNING) time.sleep(.5) mpu9250.initialize(enable_magnetometer = False) time.sleep(.5)
def test1(): N = 1 try: # no magnetometer mpu9250.initialize(enable_magnetometer = False) conf = mpu9250.get() assert conf == {'orientation': 136, 'accel_dlpf': 2, 'gyro_dlpf': 2, 'compass_time_constant': 5.0, 'enable_fusion': False, 'enable_dmp': False, 'enable_magnetometer': False, 'accel_fsr': 1, 'dmp_sample_rate': 100, 'show_warnings': False, 'gyro_fsr': 2, 'dmp_interrupt_priority': 98} print('\n Accel XYZ(m/s^2) | Gyro XYZ (rad/s) | Temp (C)') for i in range(N): (ax,ay,az) = mpu9250.read_accel_data() (gx,gy,gz) = mpu9250.read_gyro_data() temp = mpu9250.read_imu_temp() print(('\r{:6.2f} {:6.2f} {:6.2f} |' + '{:6.1f} {:6.1f} {:6.1f} | {:6.1f}') .format(ax, ay, az, gx, gy, gz, temp), end='') time.sleep(1) with pytest.raises(mpu9250.error): mpu9250.read_mag_data() # consolidated read function for i in range(N): data = mpu9250.read() print(('\r{0[0]:6.2f} {0[1]:6.2f} {0[2]:6.2f} |' '{1[0]:6.1f} {1[1]:6.1f} {1[2]:6.1f} |') .format(data['accel'], data['gyro']), end='') time.sleep(1) # with magnetometer mpu9250.initialize(enable_magnetometer = True) conf = mpu9250.get() assert conf == {'orientation': 136, 'accel_dlpf': 2, 'gyro_dlpf': 2, 'compass_time_constant': 5.0, 'enable_fusion': False, 'enable_dmp': False, 'enable_magnetometer': True, 'accel_fsr': 1, 'dmp_sample_rate': 100, 'show_warnings': False, 'gyro_fsr': 2, 'dmp_interrupt_priority': 98} print('\n Accel XYZ(m/s^2) | Gyro XYZ (rad/s) | Mag Field XYZ(uT) | Temp (C)') for i in range(N): (ax,ay,az) = mpu9250.read_accel_data() (gx,gy,gz) = mpu9250.read_gyro_data() (mx,my,mz) = mpu9250.read_mag_data() temp = mpu9250.read_imu_temp() print(('\r{:6.2f} {:6.2f} {:6.2f} |' + '{:6.1f} {:6.1f} {:6.1f} |' '{:6.1f} {:6.1f} {:6.1f} | {:6.1f}') .format(ax, ay, az, gx, gy, gz, mx, my, mz, temp), end='') time.sleep(1) # consolidated read function for i in range(N): data = mpu9250.read() print(('\r{0[0]:6.2f} {0[1]:6.2f} {0[2]:6.2f} |' '{1[0]:6.1f} {1[1]:6.1f} {1[2]:6.1f} |' '{2[0]:6.1f} {2[1]:6.1f} {2[2]:6.1f} |') .format(data['accel'], data['gyro'], data['mag']), end='') time.sleep(1) except (KeyboardInterrupt, SystemExit): pass finally: mpu9250.power_off()
from time import sleep import time import getopt, sys import math import socket import json import rcpy import rcpy.mpu9250 as mpu9250 UDP_IP_ADDRESS = "192.168.8.98" UDP_PORT_NO = 5005 mpu9250.initialize(enable_dmp=True, dmp_sample_rate=4, enable_magnetometer=True) while (1): enable_magnetometer = True enable_fusion = True ImuArr = [] # set state to rcpy.RUNNING rcpy.set_state(rcpy.RUNNING) if rcpy.get_state() == rcpy.RUNNING: data = mpu9250.read() quaternation = data['quat'] q1 = data['quat'][0] q2 = data['quat'][1] q3 = data['quat'][2]
import getopt, sys import controller as c import math # import rcpy library # This automatically initizalizes the robotics cape import rcpy import rcpy.mpu9250 as mpu9250 import rcpy.servo as servo import rcpy.clock as clock print("1") rcpy.set_state(rcpy.RUNNING) mpu9250.initialize(enable_dmp=True, dmp_sample_rate=100, enable_fusion=True, enable_magnetometer=True) srvo = servo.Servo(1) clck = clock.Clock(srvo, 0.02) servo.enable() clck.start() srvo.set(-1.0) theta_max = math.pi / 2.5 theta_min = -math.pi / 2.5 theta_dot_max = 5 theta_dot_min = -5 throttle_max = 20 throttle_min = 0 theta_tuple = (theta_min, theta_max) theta_dot_tuple = (theta_dot_min, theta_dot_max)
def main(): # exit if no options if len(sys.argv) < 2: usage() sys.exit(2) # Parse command line try: opts, args = getopt.getopt(sys.argv[1:], "hmpcagtqfos:", ["help"]) except getopt.GetoptError as err: # print help information and exit: print('rcpy_test_dmp: illegal option {}'.format(sys.argv[1:])) usage() sys.exit(2) # defaults enable_magnetometer = False show_compass = False show_gyro = False show_accel = False show_quat = False show_tb = False sample_rate = 100 enable_fusion = False show_period = False for o, a in opts: if o in ("-h", "--help"): usage() sys.exit() elif o in "-s": sample_rate = int(a) elif o == "-m": enable_magnetometer = True elif o == "-c": show_compass = True elif o == "-a": show_accel = True elif o == "-g": show_gyro = True elif o == "-q": show_quat = True elif o == "-t": show_tb = True elif o == "-f": enable_fusion = True elif o == "-p": show_period = True else: assert False, "Unhandled option" if show_compass and not enable_magnetometer: print('rcpy_test_dmp: -c can only be used with -m ') usage() sys.exit(2) # set state to rcpy.RUNNING rcpy.set_state(rcpy.RUNNING) # magnetometer ? mpu9250.initialize(enable_dmp=True, dmp_sample_rate=sample_rate, enable_fusion=enable_fusion, enable_magnetometer=enable_magnetometer) # message print("Press Ctrl-C to exit") # header if show_accel: print(" Accel XYZ (m/s^2) |", end='') if show_gyro: print(" Gyro XYZ (deg/s) |", end='') if show_compass: print(" Mag Field XYZ (uT) |", end='') print("Head(rad)|", end='') if show_quat: print(" Quaternion |", end='') if show_tb: print(" Tait Bryan (rad) |", end='') if show_period: print(" Ts (ms)", end='') print() try: # keep running while True: # running if rcpy.get_state() == rcpy.RUNNING: t0 = time.perf_counter() data = mpu9250.read() t1 = time.perf_counter() dt = t1 - t0 t0 = t1 print('\r', end='') if show_accel: print('{0[0]:6.2f} {0[1]:6.2f} {0[2]:6.2f} |'.format( data['accel']), end='') if show_gyro: print('{0[0]:6.1f} {0[1]:6.1f} {0[2]:6.1f} |'.format( data['gyro']), end='') if show_compass: print('{0[0]:7.1f} {0[1]:7.1f} {0[2]:7.1f} |'.format( data['mag']), end='') print(' {:6.2f} |'.format(data['head']), end='') if show_quat: print('{0[0]:6.1f} {0[1]:6.1f} {0[2]:6.1f} {0[3]:6.1f} |'. format(data['quat']), end='') if show_tb: print('{0[0]:6.2f} {0[1]:6.2f} {0[2]:6.2f} |'.format( data['tb']), end='') if show_period: print(' {:7.2f}'.format(1000 * dt), end='') # no need to sleep except KeyboardInterrupt: # Catch Ctrl-C pass finally: # say bye print("\nBye Beaglebone!")
#this is just a test of the kalman dilter which works decently, the next steps are adding a taylor expansion to implement an EKF and defining the kalman gain as a function of time import time import getopt, sys import math from sympy import diff # import rcpy library # This automatically initizalizes the robotics cape import rcpy import rcpy.mpu9250 as mpu9250 import numpy rcpy.set_state(rcpy.RUNNING) mpu9250.initialize(enable_magnetometer=True) E = [[1, 0, 0], [0, 1, 0], [0, 0, 1]] X = [[0], [0], [0]] Q = [[2, 0, 0], [0, 2, 0], [0, 0, 2]] def Kalman(data_gyro, data_accel, r): global X global E global Q X_old = X E_old = E data_gyro = numpy.transpose(data_gyro) data_accel = numpy.transpose(data_accel) I = [[1, 0, 0], [0, 1, 0], [0, 0, 1]] E_est = numpy.matmul(I, E_old, numpy.transpose(I), casting='unsafe') + Q K = numpy.matmul(E_est, numpy.transpose(I), numpy.linalg.inv((E)), casting='unsafe')
#!/usr/bin/env python3 # Read and display IMU # Based on: https://raw.githubusercontent.com/mcdeoliveira/rcpy/master/examples/rcpy_test_imu.py # import python libraries import time import getopt, sys # import rcpy library # This automatically initizalizes the robotics cape import rcpy import rcpy.mpu9250 as mpu9250 rcpy.set_state(rcpy.RUNNING) mpu9250.initialize(enable_magnetometer = True) print("Press Ctrl-C to exit") # header print(" Accel XYZ (m/s^2) |" " Gyro XYZ (deg/s) |", end='') print(" Mag Field XYZ (uT) |", end='') print(' Temp (C)') try: # keep running while True: if rcpy.get_state() == rcpy.RUNNING: temp = mpu9250.read_imu_temp() data = mpu9250.read() print(('\r{0[0]:6.2f} {0[1]:6.2f} {0[2]:6.2f} |' '{1[0]:6.1f} {1[1]:6.1f} {1[2]:6.1f} |' '{2[0]:6.1f} {2[1]:6.1f} {2[2]:6.1f} |'
def main(): # Parse command line try: opts, args = getopt.getopt(sys.argv[1:], "hm", ["help"]) except getopt.GetoptError as err: # print help information and exit: print('rcpy_test_imu: illegal option {}'.format(sys.argv[1:])) usage() sys.exit(2) # defaults enable_magnetometer = False for o, a in opts: if o in ("-h", "--help"): usage() sys.exit() elif o == "-m": enable_magnetometer = True else: assert False, "Unhandled option" # set state to rcpy.RUNNING rcpy.set_state(rcpy.RUNNING) # no magnetometer mpu9250.initialize(enable_magnetometer = enable_magnetometer) # message print("try 'python rcpy_test_imu -h' to see other options") print("Press Ctrl-C to exit") # header print(" Accel XYZ (m/s^2) |" " Gyro XYZ (deg/s) |", end='') if enable_magnetometer: print(" Mag Field XYZ (uT) |", end='') print(' Temp (C)') try: # keep running while True: # running if rcpy.get_state() == rcpy.RUNNING: temp = mpu9250.read_imu_temp() data = mpu9250.read() if enable_magnetometer: print(('\r{0[0]:6.2f} {0[1]:6.2f} {0[2]:6.2f} |' '{1[0]:6.1f} {1[1]:6.1f} {1[2]:6.1f} |' '{2[0]:6.1f} {2[1]:6.1f} {2[2]:6.1f} |' ' {3:6.1f}').format(data['accel'], data['gyro'], data['mag'], temp), end='') else: print(('\r{0[0]:6.2f} {0[1]:6.2f} {0[2]:6.2f} |' '{1[0]:6.1f} {1[1]:6.1f} {1[2]:6.1f} |' ' {2:6.1f}').format(data['accel'], data['gyro'], temp), end='') # sleep some time.sleep(.5) except KeyboardInterrupt: # Catch Ctrl-C pass finally: # say bye print("\nBye Beaglebone!")
def turn(angle=None, mode=None, speed=30, direction=None): turn = True rcpy.set_state(rcpy.RUNNING) mpu9250.initialize(enable_dmp=True) while turn: if rcpy.state() == rcpy.RUNNING: imu_data = mpu9250.read() # Read Data from Sensors imu = imu_data[ 'tb'] # Get imu Value and Convert to Degrees (0 to 180 , -180 to 0) imu_deg = (math.degrees(round(imu[2], 2))) % 360 angle = angle % 360 if mode == "point" or mode == None: if angle == 0: data_l = [146, 32] # Brake data_r = [146, 32] # Check Angle is from 0 to 180 or 180 to 360, to determine which direction to turn elif 0 < angle and 180 > angle: # If converted angle is between 0 and 180, point turn counter-clockwise data_l = [255, 0, 128 - speed] # Rotate Left data_r = [255, 1, 128 + speed] elif 180 < angle and 360 > angle: # If converted angle is between 180 and 360, point turn clockwise data_l = [255, 0, 128 + speed] # Rotate Right data_r = [255, 1, 128 - speed] if (imu_deg - 2) <= angle and angle <= ( imu_deg + 2): # Once Angle is within Range Stop Motors data_l = [146, 32] # Brake data_r = [146, 32] elif mode == "swing": if direction == "forward": if angle == 0: data_l = [146, 32] # Brake data_r = [146, 32] # Check Angle is from 0 to 180 or 180 to 360, to determine which direction to turn elif 0 < angle and 180 > angle: # If converted angle is between 0 and 180, point turn counter-clockwise data_l = [255, 0, 128] # Rotate Left data_r = [255, 1, 128 + speed] elif 180 < angle and 360 > angle: # If converted angle is between 180 and 360, point turn clockwise data_l = [255, 0, 128 + speed] # Rotate Right data_r = [255, 1, 128] if (imu_deg - 2) <= angle and angle <= ( imu_deg + 2): # Once Angle is within Range Stop Motors data_l = [146, 32] # Brake data_r = [146, 32] elif direction == "backward": if angle == 0: data_l = [146, 32] # Brake data_r = [146, 32] # Check Angle is from 0 to 180 or 180 to 360, to determine which direction to turn elif 0 < angle and 180 > angle: # If converted angle is between 0 and 180, point turn counter-clockwise data_l = [255, 0, 128 - speed] # Rotate Left data_r = [255, 1, 128] elif 180 < angle and 360 > angle: # If converted angle is between 180 and 360, point turn clockwise data_l = [255, 0, 128] # Rotate Right data_r = [255, 1, 128 - speed] if (imu_deg - 2) <= angle and angle <= ( imu_deg + 2): # Once Angle is within Range Stop Motors data_l = [146, 32] # Brake data_r = [146, 32] else: continue ser_motor.write(data_l) # Send Data to Motor Controllers ser_motor.write(data_r) data_l = [146, 32] # Brake data_r = [146, 32] ser_motor.write(data_l) # Send Data to Motor Controllers ser_motor.write(data_r) turn = False print("Done!") pass
def main(): # exit if no options if len(sys.argv) < 2: usage() sys.exit(2) # Parse command line try: opts, args = getopt.getopt(sys.argv[1:], "hmpcagtqfos:", ["help"]) except getopt.GetoptError as err: # print help information and exit: print('rcpy_test_dmp: illegal option {}'.format(sys.argv[1:])) usage() sys.exit(2) # defaults enable_magnetometer = False show_compass = False show_gyro = False show_accel = False show_quat = False show_tb = False sample_rate = 100 enable_fusion = False show_period = False for o, a in opts: if o in ("-h", "--help"): usage() sys.exit() elif o in "-s": sample_rate = int(a) elif o == "-m": enable_magnetometer = True elif o == "-c": show_compass = True elif o == "-a": show_accel = True elif o == "-g": show_gyro = True elif o == "-q": show_quat = True elif o == "-t": show_tb = True elif o == "-f": enable_fusion = True elif o == "-p": show_period = True else: assert False, "Unhandled option" if show_compass and not enable_magnetometer: print('rcpy_test_dmp: -c can only be used with -m ') usage() sys.exit(2) # set state to rcpy.RUNNING rcpy.set_state(rcpy.RUNNING) # magnetometer ? mpu9250.initialize(enable_dmp = True, dmp_sample_rate = sample_rate, enable_fusion = enable_fusion, enable_magnetometer = enable_magnetometer) # message print("Press Ctrl-C to exit") # header if show_accel: print(" Accel XYZ (m/s^2) |", end='') if show_gyro: print(" Gyro XYZ (deg/s) |", end='') if show_compass: print(" Mag Field XYZ (uT) |", end='') print("Head(rad)|", end='') if show_quat: print(" Quaternion |", end='') if show_tb: print(" Tait Bryan (rad) |", end='') if show_period: print(" Ts (ms)", end='') print() try: # keep running while True: # running if rcpy.get_state() == rcpy.RUNNING: t0 = time.perf_counter() data = mpu9250.read() t1 = time.perf_counter() dt = t1 - t0 t0 = t1 print('\r', end='') if show_accel: print('{0[0]:6.2f} {0[1]:6.2f} {0[2]:6.2f} |' .format(data['accel']), end='') if show_gyro: print('{0[0]:6.1f} {0[1]:6.1f} {0[2]:6.1f} |' .format(data['gyro']), end='') if show_compass: print('{0[0]:7.1f} {0[1]:7.1f} {0[2]:7.1f} |' .format(data['mag']), end='') print(' {:6.2f} |' .format(data['head']), end='') if show_quat: print('{0[0]:6.1f} {0[1]:6.1f} {0[2]:6.1f} {0[3]:6.1f} |' .format(data['quat']), end='') if show_tb: print('{0[0]:6.2f} {0[1]:6.2f} {0[2]:6.2f} |' .format(data['tb']), end='') if show_period: print(' {:7.2f}'.format(1000*dt), end='') # no need to sleep except KeyboardInterrupt: # Catch Ctrl-C pass finally: # say bye print("\nBye Beaglebone!")