def __init__(self, address=0x68, updateoffset=True, cycletime=0.05):
threading.Thread.__init__(self)
self.address = address
#MPU6050 is an specific interface to the hw used.
#if the imu is different from MPU6050 it is enough to use the
#correct interface
self.IMU = MPU6050.MPU6050(address)
#print('IMU calibrating...')
if updateoffset:
self.IMU.updateOffsets('IMU_offset.txt')
self.IMU.readOffsets('IMU_offset.txt')
self.roll = 0
self.pitch = 0
self.yaw = 0
self.x_acc = 0
self.y_acc = 0
self.z_acc = 0
self.r_rate = 0
self.p_rate = 0
self.y_rate = 0
self.cycling = True
self.cycletime = cycletime
self.datalog = ''
def __init__(self,
address=0x68,
cycletime=0.01,
imulog=False,
simulation=True):
threading.Thread.__init__(self)
self.logger = logging.getLogger('myQ.sensor')
self.address = address
self.cycletime = cycletime
self.imulog = imulog
self.simulation = simulation
self.datalog = ''
self.cycling = True
#those values are calculated
self.roll = 0
self.pitch = 0
self.yaw = 0
self.roll_g = 0
self.pitch_g = 0
self.yaw_g = 0
self.roll_a = 0
self.pitch_a = 0
self.yaw_a = 0
#those values are directly fro IMU
self.x_acc = 0
self.y_acc = 0
self.z_acc = 0
self.r_rate = 0
self.p_rate = 0
self.y_rate = 0
self.temp = 0
try:
#here just check that library is available
#MPU6050 is a specific interface to the hw used.
#if the imu is different from MPU6050 it is necessary to call the
#correct interface here
if self.simulation is False:
from MPU6050 import MPU6050
self.IMU = MPU6050(address)
self.IMU.readOffsets('IMU.cfg')
self.logger.debug('IMU initiazized...')
except ImportError:
self.simulation = True
self.logger.error('Error: IMU NOT initiazized. %s')
def Gyro(i2c, oled, t=50):
from MPU6050 import MPU6050
accelerometer = MPU6050(i2c)
data_org = accelerometer.get_values()
x, y = 128 // 2, 64 // 2
h, w = 10, 20
sensitivity = 60
for i in range(t):
data = accelerometer.get_values()
y_a, x_a = data['AX'] - data_org['AX'], data['AY'] - data_org['AY']
x, y = x - x_a / 32767 * sensitivity, y - y_a / 32767 * sensitivity
x, y = int(min(max(x, 0), 128 - w)), int(min(max(y, 0), 64 - h))
oled.fill(0)
oled.rect(x, y, w, h)
oled.show()
def imu_thread():
DEBUG_MODE = False
mpu = MPU6050(i2c_bus, device_address, x_accel_offset, y_accel_offset,
z_accel_offset, x_gyro_offset, y_gyro_offset, z_gyro_offset,
enable_debug_output)
mpu.dmp_initialize()
mpu.set_DMP_enabled(True)
mpu_int_status = mpu.get_int_status()
print(hex(mpu_int_status))
packet_size = mpu.DMP_get_FIFO_packet_size()
print(packet_size)
FIFO_count = mpu.get_FIFO_count()
print(FIFO_count)
while True:
FIFO_count = mpu.get_FIFO_count()
mpu_int_status = mpu.get_int_status()
if (FIFO_count
== 1024) or (mpu_int_status & 0x10): # Check if overflowed
mpu.reset_FIFO()
# Check if fifo data is ready
elif (mpu_int_status & 0x02):
# Wait until packet_size number of bytes are ready for reading, default
# is 42 bytes
while FIFO_count < packet_size:
FIFO_count = mpu.get_FIFO_count()
FIFO_buffer = mpu.get_FIFO_bytes(packet_size)
quat = mpu.DMP_get_quaternion_int16(FIFO_buffer)
grav = mpu.DMP_get_gravity(quat)
rpy = mpu.DMP_get_euler_roll_pitch_yaw(quat, grav)
a_raw = mpu.get_acceleration()
imu_fifo.put({
'ax': 9.80665 * (a_raw[0] / 16384.0),
'ay': 9.80665 * (a_raw[1] / 16384.0),
'az': 9.80665 * (a_raw[2] / 16384.0),
'yaw': rpy.z
})
if DEBUG_MODE == True:
print('ax: ' + str(9.80665 * (a_raw[0] / 16384.0)))
print('az: ' + str(9.80665 * (a_raw[2] / 16384.0)))
print('yaw: ' + str(rpy.z) + '\n\n')
def __init__(self,
wheelPositionRef=0,
VelocityRef=0,
TurnMotorRight=0,
TurnMotorLeft=0,
id=0,
value=0,
lastAccelerometerAngleX=0,
LoopTimeRatioSeg=0,
sensor=0,
filteredX=0):
self.wheelPositionRef = wheelPositionRef
self.VelocityRef = VelocityRef
self.TurnMotorRight = TurnMotorRight
self.TurnMotorLeft = TurnMotorLeft
self.id = id
self.value = value
self.lastAccelerometerAngleX = lastAccelerometerAngleX
self.LoopTimeRatioSeg = LoopTimeRatioSeg
self.sensor = MPU6050(0x68)
self.filteredX = filteredX
def __init__(self):
self.startTime = time() * 1000000.0
self.lastUpdate = 0.0
self.sampleFreq = 1.0
#define MPU6050
self.accgyro = MPU6050()
#define HMC5883L
self.magn = HMC5883()
#define MS5611
#self.baro = BMP180()
#initialize quarternion
self.q0 = 1.0
self.q1 = 0.0
self.q2 = 0.0
self.q3 = 0.0
#initialize accgyro
self.accgyro.setI2CMasterModeEnabled(0)
self.accgyro.setI2CBypassEnabled(1)
self.accgyro.setFullScaleGyroRange(0x03)
sleep(0.005)
self.magn.init(False) #initialize magn
#calibrate it thru self test, not recommended to change gain after calibration
self.magn.calibrate(1, 64) #use gain 1=default
self.magn.setMode(0)
sleep(0.010)
self.magn.setDOR(4)
#initialize baro to a specific pressure and altitude
#self.baro.zeroCal(self.sea_press,self.altitude)
#zeroGyro
self.zeroGyro()
def __init__(self, specification = None, scheduler = None, stopped = False):
""" Initializes INU object
@param specification
@param scheduler
@param stopped
"""
if (specification != None):
self.specification = specification
else:
self.specification = Specification.Specification.GetInstance()
if(scheduler != None):
self.scheduler = scheduler
else:
self.scheduler = Scheduler.GetInstance()
ipath = os.path.join(Specification.Specification.filebase, 'imu')
if not os.path.exists(ipath):
os.makedirs(ipath)
self.filebase = os.path.join(ipath,'IMU_offset.txt')
self.metrics = {}
self.callbacks = {}
self.mpi = 3.14159265359
self.radian = 180 / self.mpi
if (IMU.isAvailable()):
self.device = MPU6050()
self.__initOrientations()
self.device.readOffsets(self.filebase)
self.config = self.device.getConfig()
self.initRaw()
self.initNorm()
self.initAngles()
self.initLowpass()
self.initHighpass()
self.initComplement()
self.inittime=time.time()
self.tottime=0
self.scheduler.addTask('imu_watcher', self.calculate, 0.02, stopped)
def __init__(self):
i2c_bus = 1
device_address = 0x68
# The offsets are different for each device and should be changed
# accordingly using a calibration procedure
x_accel_offset = -5489
y_accel_offset = -1441
z_accel_offset = 1305
x_gyro_offset = -2
y_gyro_offset = -72
z_gyro_offset = -5
enable_debug_output = False
self.mpu = MPU6050(i2c_bus, device_address, x_accel_offset,
y_accel_offset, z_accel_offset, x_gyro_offset,
y_gyro_offset, z_gyro_offset, enable_debug_output)
self.mpu.dmp_initialize()
self.mpu.set_DMP_enabled(True)
self.packet_size = self.mpu.DMP_get_FIFO_packet_size()
self.FIFO_count = self.mpu.get_FIFO_count()
self.FIFO_buffer = [0] * 64
self.FIFO_count_list = list()
def __init__(self):
self.mpu6050 = MPU6050()
time.sleep(0.01)
t = (currentTime - oldTime)
t = t if (t > 0) else 0.001 # To ensure that t is always non-zero
oldTime = currentTime
old_omega = omega
omega = (gyro.getX() - gyro_offset) * (2 * math.pi / 180)
theta += omega * t
omega_dot = (omega - old_omega) / t
v_dot = -(gyro.getYaccel() - accel_offset)
v += v_dot * t
X += v * t * math.sin(theta)
Y += v * t * math.cos(theta)
gyro = MPU6050(gyro_bus, gyro_address)
#-------------- Sabertooth ---------------
serialObj = serial.Serial(port='/dev/ttyAMA0', baudrate=9600)
sabertooth_address = 128
sabertooth = Sabertooth(sabertooth_address, serialObj)
#---------------- Encoder ----------------
GPIO_A = 17
GPIO_B = 27
encoder_scale = (2 * math.pi) / 2048
def encoder_counter(delta):
queue.put(delta)
new_r = a * (new_r + fgx * dt) + (1 - a) * r
new_p = a * (new_p + fgy * dt) + (1 - a) * p
#note the yaw angle can be calculated only using the
# gyro that's why a=1
a = 1
new_y = a * (new_y + fgz * dt) + (1 - a) * y
return new_r, new_p, new_y
#import pylab
from MPU6050 import MPU6050
#logger = logging.getLogger(__name__)
IMU = MPU6050()
IMU.updateOffsets('IMU_offset.txt')
IMU.readOffsets('IMU_offset.txt')
print('IMU ready!')
#Kalman filter parameters for a 1-dim case x 3
#dimension with error distribution = gaussian
#
A = numpy.eye(3)
H = numpy.eye(3)
B = numpy.eye(3) * 0
Q = numpy.eye(3) * 0.001
#play with Q to tune the smoothness
R = numpy.eye(3) * 0.01
xhat = numpy.matrix([[0], [0], [0]])
P = numpy.eye(3)
PIDo = 0.0
eInteg = 0.0 # previous Integration
ePrev = 0.0 # previous error
FIX = -12.89
ZLoop = True
#-------------System Initialization --------------------
bus = smbus.SMBus(1) #Init I2C module
now = time.time()
m = MOTOR.servo() # Start Servoblaster deamon and reset servos
wi_net = Server() # Start net service for remote control
wi_net.start()
time0 = now
sensor = MPU6050(bus, address, "MPU6050") #Init IMU module
sensor.read_raw_data() # Reads current data from the sensor
k = Kalman()
#=========================================================
def PID_L(current, target):
global eInteg
global ePrev
error = target - current
pid = KP * error + KI * eInteg + KD * (error - ePrev)
eInteg = eInteg + error
ePrev = error
return pid
gyro_scale = 131.0 accel_scale = 16384.0 RAD_TO_DEG = 57.29578 M_PI = 3.14159265358979323846 address = 0x68 # This is the address value read via the i2cdetect command bus = smbus.SMBus(1) # or bus = smbus.SMBus(1) for Revision 2 boards now = time.time() K = 0.98 K1 = 1 - K time_diff = 0.01 sensor = MPU6050(bus, address, "MPU6050") sensor.read_raw_data() # Reads current data from the sensor rate_gyroX = 0.0 rate_gyroY = 0.0 rate_gyroZ = 0.0 gyroAngleX = 0.0 gyroAngleY = 0.0 gyroAngleZ = 0.0 raw_accX = 0.0 raw_accY = 0.0 raw_accZ = 0.0 rate_accX = 0.0
def mpuClass(mpu_addr, x_accel_offset, y_accel_offset, z_accel_offset,
x_gyro_offset, y_gyro_offset, z_gyro_offset):
mpu = MPU6050(i2c_bus, mpu_addr, x_accel_offset, y_accel_offset,
z_accel_offset, x_gyro_offset, y_gyro_offset, z_gyro_offset,
enable_debug_output)
return mpu
#!/usr/bin/python
from MPU6050 import MPU6050 as MPU6050
from time import sleep
mpu = MPU6050()
while (True):
ax, ay, az, gx, gy, gz = mpu.getMotion6()
print "---------------------Accelerometer-------------------------------"
print " X = %f Y = %f Z = %f" % (ax, ay, az)
print "----------------------Gyroscope----------------------------------"
print " X = %f Y = %f Z = %f" % (gx, gy, gz)
sleep(0.5)
import numpy as np
import datetime as datetime
import sys
from KalmanFilter import apply_single_kalman_filter
from MPU6050 import MPU6050
from time import sleep
import time
from transformations import apply_first_transformation, generate_two_matrices, \
apply_all_transformations, z_transform, Measurement
from fourier import apply_fourier
from websocket_client import send_measurements, send_fourier, set_ip, start_connection
sensor = MPU6050(0x68)
"""Creates a new instance of the MPU6050 class for the first sensor"""
sensor2 = MPU6050(0x69)
"""Creates a new instance of the MPU6050 class for the second sensor"""
x_mat = np.empty(0)
y_mat = np.empty(0)
z_mat = np.empty(0)
x_mat2 = np.empty(0)
y_mat2 = np.empty(0)
"""Matrices use for rotations"""
third_matrix_values = 500
"""Quantity of values to get before calculating the third matrix"""
third_matrix_interval = 0.05
"""Interval between two accelerations when calculating the third matrix"""
__author__ = 'Geir'
from MPU6050 import MPU6050
from time import clock
mpu = mpu = MPU6050(1, 0x68, -5489, -1441, 1305, -2, -72, -5, True)
mpu.dmp_initialize()
mpu.set_DMP_enabled(True)
mpu_int_status = mpu.get_int_status()
print(hex(mpu_int_status))
while False:
print(mpu.get_acceleration())
print(mpu.get_rotation())
packet_size = mpu.DMP_get_FIFO_packet_size()
print(packet_size)
FIFO_count = mpu.get_FIFO_count()
print(FIFO_count)
count = 0
FIFO_buffer = [0] * 64
overflow = 0
no_overflow = 0
crazy_high_number = 0
start_time = clock()
# FIFO_list = list()
FIFO_count_list = list()
while count < 10000:
FIFO_count = mpu.get_FIFO_count()
# FIFO_list.append(FIFO_count)
import time
from MPU6050 import MPU6050
from BME280 import BME280
m = MPU6050()
b = BME280()
while(1):
time.sleep(0.5)
m.get_data()
print("accel scaled: ", m.accelerometer.scaled)
print("status: ", m.status)
print("")
b.get_data()
print("height: ", b.height)
print("status: ", b.status)
print("")
def main(mode):
if mode == "infer":
shot_types = [
"Pull-hook", "Hook", "Pull", "Fade", "Straight", "Draw", "Push",
"Slice", "Push-slice"
]
X_mean = np.load("norm_data/X_mean.npy")
X_std = np.load("norm_data/X_std.npy")
y_mean = np.load("norm_data/y_mean.npy")
y_std = np.load("norm_data/y_std.npy")
model = BadModel(5, 1, 3)
model.load_state_dict(torch.load("weights/best_model.pth")["model"])
model.eval()
try:
mpu = MPU6050(0x68)
mpu.set_accel_range(MPU6050.ACCEL_RANGE_16G)
mpu.set_gyro_range(MPU6050.GYRO_RANGE_2000DEG)
data_func = mpu.get_movement_data
except NameError as ex:
print("#############################")
print("##### MPU6050 NOT FOUND #####")
print("##### USING FAKE DATA #####")
print("#############################")
data_func = get_fake_data
live = False
verbose = False
print_interval = 100
draw_interval = 1 # how many collections between plots
# plt.ion()
columns = ["Ax", "Ay", "Az", "Gx", "Gy", "Gz"]
data = []
detect_swing = False
try:
print("Starting collection.")
count = 0
start = time.time()
while True:
# ax, ay, az, gx, gy, gz
values = data_func()
data.append(values)
if live and count % draw_interval == 0:
plot_data(data, columns)
if verbose and count % print_interval == 0:
print_values(values, count)
# if we've hit the max window size and there is no swing detected, discard first entry
# if there is a swing detected and our list is full, then we've collected the full sample and can save
if len(data) >= WINDOW_SIZE:
if detect_swing:
data_matrix = np.array(data)
if mode == "collect":
print("Saving...")
# plot_data(data, columns)
save_swing(data_matrix, columns)
print("Continuing...")
detect_swing = False
data = []
count = 0 # TODO: REMOVE probably
else:
data.pop(0)
if mode == "infer":
data_matrix = data_matrix.T
data_matrix = (data_matrix - X_mean) / X_std
print(data_matrix.shape)
pred = model(
torch.from_numpy(data_matrix).unsqueeze(0).float())
pred = pred.detach().numpy()
dist = pred[0][-1] * y_std + y_mean
shot = np.argmax(pred[:, :-1])
print(
f"Predicted swing type: {shot_types[shot]}, {int(dist)}yds"
)
detect_swing = False
data = []
if abs(values[0]) >= HIT_THRESH and not detect_swing:
print("Swing detected")
# detected a swing, during the midpoint (probably)
detect_swing = True
# empty the first half of the data list, allow recording for the second half of the swing signal
data = data[-WINDOW_SIZE // 2:]
count += 1
except KeyboardInterrupt:
print("Stopping collection.")
pass
runtime = time.time() - start
print("Sample rate (Hz):", count / runtime)
from Adafruit_PWM_Servo_Driver import PWM
import time
import smbus
from MPU6050 import MPU6050
# ===========================================================================
# Example Code
# ===========================================================================
# Initialise the PWM device using the default address
pwm = PWM(0x40, debug=True)
# Note if you'd like more debug output you can instead run:
#pwm = PWM(0x40, debug=True)
accel = MPU6050()
servoMin = 150 # Min pulse length out of 4096
servoMax = 600 # Max pulse length out of 4096
def limit(min, max, value):
if (value < min):
return min
elif (value > max):
return max
else:
return value
def setServoPulse(channel, pulse):
#!/usr/bin/python
# Import the MPU6050 class from the MPU6050.py file
from MPU6050 import MPU6050
from time import sleep
# Create a new instance of the MPU6050 class
sensor = MPU6050(0x68)
while True:
accel_data = sensor.get_accel_data()
gyro_data = sensor.get_gyro_data()
temp = sensor.get_temp()
print("Accelerometer data")
print("x: " + str(accel_data['x']))
print("y: " + str(accel_data['y']))
print("z: " + str(accel_data['z']))
print("Gyroscope data")
print("x: " + str(gyro_data['x']))
print("y: " + str(gyro_data['y']))
print("z: " + str(gyro_data['z']))
print("Temp: " + str(temp) + " C")
sleep(0.5)
print('Quitting program.')
cleanup()
def cleanup():
"""Resource cleanup."""
imu.close()
print('Resource cleanup completed.')
exit(0)
signal(SIGINT, signal_handler)
###
### IMU Configuration
imu = MPU6050(ad0_bit=0)
imu.initialize()
imu.set_digital_filter(0)
rate = imu.set_sample_rate(100)
imu.set_accel_range(0)
imu.set_gyro_range(0)
###
### Main Program
print('Press CTRL + C to exit.')
print('Internal sampling rate: ' + str(rate) + ' Hz')
is_connected = imu.test_connection()
print('Connection test result ' + str(is_connected))
while True:
#MPU6050
i2c_bus = 1 #pin3=SDA pin5=SCL
device_address = 0x68
#キャリブレーション数値設定 水平に置き MPU6050_cal.py を実行し計算結果を代入
x_accel_offset = -2085
y_accel_offset = -2322
z_accel_offset = 1122
x_gyro_offset = 71
y_gyro_offset = -24
z_gyro_offset = 25
enable_debug_output = False
mpu = MPU6050(i2c_bus, device_address, x_accel_offset, y_accel_offset,
z_accel_offset, x_gyro_offset, y_gyro_offset, z_gyro_offset,
enable_debug_output)
mpu.dmp_initialize()
mpu.set_DMP_enabled(True) #DMP (Digital Motion Processor)
mpu_int_status = mpu.get_int_status()
packet_size = mpu.DMP_get_FIFO_packet_size()
FIFO_buffer = [0] * 64
FIFO_count_list = list()
#傾斜cm取得
def roll_MPU(ant_h):
FIFO_count = mpu.get_FIFO_count()
mpu_int_status = mpu.get_int_status()
if (FIFO_count == 1024) or (mpu_int_status & 0x10):
mpu.reset_FIFO()
Kd = pot.read()*scale / 4095
oled.draw_text(0,30 , 'kd = {:5.2f}'.format(Kd))
oled.display()
while trigger():
pass
oled.draw_text(0,30 , 'Trying to balance 2')
oled.display()
alpha = 0.95
pitch = 0
e_int = 0
e_diff = 0
pwm = 0
target = -6
imu = MPU6050(1,False)
# def updatePitch(pitch,dt,alpha):
# theta = imu.pitch()
# pitch_dot = imu.get_gy()
# pitch = alpha*(pitch + pitch_dot*dt*0.00001) + (1-alpha)*theta
# return(pitch, pitch_dot)
tic1 = pyb.micros()
while True:
dt = pyb.micros() - tic1
if dt > 5000:
theta = imu.pitch()