# header,,
print(" Accel XYZ (m/s^2) |" " Gyro XYZ (deg/s) |", end='')
print(" Mag Field XYZ (uT) |", end='')
print(' Temp (C)')
i = 0
avg = 0
save = []
sigma = 0
cov = 0
try: # keep running
while True:
while i < 100:
if rcpy.get_state() == rcpy.RUNNING:
temp = mpu9250.read_imu_temp()
data = mpu9250.read_accel_data()
save.append(data[1])
avg = avg + data[1]
i = i + 1
avg = avg / 100
print('doing')
time.sleep(0.01)
print('done')
for i in range(0, len(save)):
sigma = sigma + ((save[i] - avg)**2)
stdv = math.sqrt(sigma / 100)
cov = stdv**2
cov = [[cov, 0, 0], [0, cov, 0], [0, 0, cov]]
print('done1')
if rcpy.get_state() == rcpy.RUNNING:
print('hi')
#print(wsra)
# calculate speed of wheelbase center
travs = ([travL, travR])
cgTrav = np.average(travs)
speeds = ([whlSpdL, whlSpdR])
cgSpeedPrev = cgSpeed
cgSpeed = np.average(speeds)
cgSpeedReport = (cgSpeed + cgSpeedPrev) / 2
# print imu info
accel = round(
data_imu['tb'][0],
3) #pull the pitch/roll/yaw values from the data array
#accel_x = data_imu['accel'] #pull the accelerometer x/y/z value from the data array
ms2 = mpu9250.read_accel_data()
ms2a = ms2[1]
imu_pitch = round(data_imu['tb'][0], 2) # pitch
imu_roll = round(data_imu['tb'][1], 2) # roll
#print("accel: ", ms2a)
# get the heading from the onboard IMU "yaw" parameter which uses sensor fusion
# we cannot trust the magnetometer directly because the magnetic field from
# the robot's motors interferes with measurement.
head1 = data_imu['tb'][
2] * 180 / 3.14 #third element is yaw, in radians
print(round(head1, 2))
# calculate theta from compass heading
sc.RotationMatrix(90) #dpm testing
if (heading < 90): theta = 90 - heading # quadrant 1
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 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()