def main():
# class MPU9250
mpu = MPU9250()
# ready mpu9250
mpu.ready()
# class RollPitchYaw
rpy = RollPitchYaw()
# get start time
startTime = time.time()
while True:
# get data(mpu9250)
accel = mpu.readAccel()
gyro = mpu.readGyro()
temp = mpu.readTemp()
magnet = mpu.readMagnet()
# calc roll, pitch, yaw
roll = rpy.calcRoll(accel)
pitch = rpy.calcPitch(accel)
yaw = rpy.calcYaw(magnet, roll, pitch)
# get now time
nowTime = time.time() - startTime
# print
print nowTime,math.degrees(roll),math.degrees(pitch),math.degrees(yaw)
# sleep
sleep(0.15)
import time
import struct
import ctypes
from MPU9250 import MPU9250
#from IMU_Driver import IMU_driver
#imu = IMU_driver()
mpu9250 = MPU9250()
while 1:
gyro = mpu9250.readGyro()
accel = mpu9250.readAccel()
print "Gyro: ", [gyro[0], gyro[1], gyro[2]]
print "Accel: ", [accel[0], accel[1], accel[2]], '\n\n'
time.sleep(0.5)
from MPU9250 import MPU9250 import grovepi import brickpi3 import time import math BP = brickpi3.BrickPi3() mpu = MPU9250() #Port Variables one = BP.PORT_1 two = BP.PORT_2 three = BP.PORT_3 four = BP.PORT_4 A = BP.PORT_A #RIGHT B = BP.PORT_B C = BP.PORT_C D = BP.PORT_D #LEFT ranger = 3 leftLine = 2 rightLine = 7 #Speed and Time Variables clawHoldPower = 20 clawDropPower = 10 clawDropTime = 0.2 speed = 15 lineTime = 0.2 lineWaitTime = 1
import time
import brickpi3
import grovepi
from TurnDegrees import Angle
from MPU9250 import MPU9250
BP = brickpi3.BrickPi3()
imu = MPU9250()
leftUltra = 4 # left sensor
middleUltra = 8 # middle sensor
rightUltra = 7 # right sensor
sensor1 = 14 # left IR sensor
sensor2 = 15 # right IR sensor
distDesired = 20
totalmag = 0
def turnLeft():
Angle(91, 1, 1)
#while(grovepi.ultrasonicRead(ultrasonic_sensor_port3) < 25 and grovepi.ultrasonicRead(ultrasonic_sensor_port2) < 25):
#BP.set_motor_power(BP.PORT_A, -30) # originally 30
#BP.set_motor_power(BP.PORT_D, 30)
def turnRight(): # if there's a wall in front, will need to implement check left or right to determine to turn left or right after encountering an obstacle
Angle(91, 2, 1)
#while(grovepi.ultrasonicRead(ultrasonic_sensor_port1) < 25 and grovepi.ultrasonicRead(ultrasonic_sensor_port2) < 25):
#BP.set_motor_power(BP.PORT_A, 30) # originally 30
#BP.set_motor_power(BP.PORT_D, -30)
def talker():
pub = rospy.Publisher('imu', Imu, queue_size=10)
rospy.init_node('ros_erle_imu', anonymous=True)
rate = rospy.Rate(10)
imu = MPU9250()
imu.initialize()
msg = Imu()
while not rospy.is_shutdown():
imu.read_acc()
m9a, m9g, m9m = imu.getMotion9()
msg.header.stamp = rospy.get_rostime()
#ORIENTATION calculation
'''You can just integrate your angular velocity to get angular
position (as Euler angles), convert the Euler angles to Quaternion,
then multiply the Quaternion to accumulate the orientation.
Suppose your input is given by a 3D vector of angular velocity:
omega = (alpha, beta, gamma), given by degrees per second.
To get the Euler angles, E, in degrees, multiply omega by the change
in time, which we can call dt '''
dt = 0.1 #10Hz in seconds
AngVelX = m9g[0] * 57.29577951308 * dt
AngVelY = m9g[1] * 57.29577951308 * dt
AngVelZ = m9g[2] * 57.29577951308 * dt
w_or = math.cos(AngVelX / 2) * math.cos(AngVelY / 2) * math.cos(
AngVelZ / 2) + math.sin(AngVelX / 2) * math.sin(
AngVelY / 2) * math.sin(AngVelZ / 2)
x_or = math.sin(AngVelX / 2) * math.cos(AngVelY / 2) * math.cos(
AngVelZ / 2) - math.cos(AngVelX / 2) * math.sin(
AngVelY / 2) * math.sin(AngVelZ / 2)
y_or = math.cos(AngVelX / 2) * math.sin(AngVelY / 2) * math.cos(
AngVelZ / 2) + math.sin(AngVelX / 2) * math.cos(
AngVelY / 2) * math.sin(AngVelZ / 2)
z_or = math.cos(AngVelX / 2) * math.cos(AngVelY / 2) * math.sin(
AngVelZ / 2) - math.sin(AngVelX / 2) * math.sin(
AngVelY / 2) * math.cos(AngVelZ / 2)
msg.orientation.x = x_or
msg.orientation.y = y_or
msg.orientation.z = z_or
msg.orientation.w = w_or
msg.orientation_covariance[0] = x_or * x_or
msg.orientation_covariance[0] = y_or * y_or
msg.orientation_covariance[0] = z_or * z_or
msg.angular_velocity.x = m9g[0]
msg.angular_velocity.y = m9g[1]
msg.angular_velocity.z = m9g[2]
msg.angular_velocity_covariance[0] = m9g[0] * m9g[0]
msg.angular_velocity_covariance[4] = m9g[1] * m9g[1]
msg.angular_velocity_covariance[8] = m9g[2] * m9g[2]
msg.linear_acceleration.x = m9a[0]
msg.linear_acceleration.y = m9a[1]
msg.linear_acceleration.z = m9a[2]
msg.linear_acceleration_covariance[0] = m9a[0] * m9a[0]
msg.linear_acceleration_covariance[4] = m9a[1] * m9a[1]
msg.linear_acceleration_covariance[8] = m9a[2] * m9a[2]
pub.publish(msg)
rate.sleep()
def start():
mpu = MPU9250()
mpu.ready()
rpy = RollPitchYaw()
def talker():
pub = rospy.Publisher('imu', Imu, queue_size=10)
rospy.init_node('ros_erle_imu', anonymous=True)
rate = rospy.Rate(10)
imu = MPU9250()
imu.initialize()
msg = Imu()
while not rospy.is_shutdown():
imu.read_acc()
msg.header.stamp = rospy.get_rostime()
m9a, m9g, m9m = imu.getMotion9()
#----------update IMU
ax = m9a[0]
ay = m9a[1]
az = m9a[2]
gx = m9g[0]
gy = m9g[1]
gz = m9g[2]
q0 = 0.0 #W
q1 = 0.0 #X
q2 = 0.0 #Y
q3 = 0.0 #Z
'''
#----------Calculate delta time
t = time()
currenttime = 0
previoustime = currenttime
currenttime = 1000000 * t + t / 1000000
dt = (currenttime - previoustime) / 1000000.0
if (dt < (1/1300.0)) :
time.sleep((1/1300.0 - dt) * 1000000)
t = time()
currenttime = 1000000 * t + t / 1000000
dt = (currenttime - previoustime) / 1000000.0
print "Delta time: d = %f" % dt
#Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if not ((ax == 0.0) and (ay == 0.0) and (az == 0.0)) :
#Normalise accelerometer measurement
recipNorm = (ax * ax + ay * ay + az * az)**-.5
ax *= recipNorm
ay *= recipNorm
az *= recipNorm
#Estimated direction of gravity and vector perpendicular to magnetic flux
halfvx = q1 * q3 - q0 * q2
halfvy = q0 * q1 + q2 * q3
halfvz = q0 * q0 - 0.5 + q3 * q3
#Error is sum of cross product between estimated and measured direction of gravity
halfex = (ay * halfvz - az * halfvy)
halfey = (az * halfvx - ax * halfvz)
halfez = (ax * halfvy - ay * halfvx)
#Compute and apply integral feedback (if enabled)
integralFBx += twoKi * halfex * dt;
integralFBy += twoKi * halfey * dt;
integralFBz += twoKi * halfez * dt;
gx += integralFBx
gy += integralFBy
gz += integralFBz
#Apply proportional feedback
gx += twoKp * halfex;
gy += twoKp * halfey;
gz += twoKp * halfez;
#Integrate rate of change of quaternion
gx *= (0.5 * dt)
gy *= (0.5 * dt)
gz *= (0.5 * dt)
qa = q0
qb = q1
qc = q2
q0 += (-qb * gx - qc * gy - q3 * gz)
q1 += (qa * gx + qc * gz - q3 * gy)
q2 += (qa * gy - qb * gz + q3 * gx)
q3 += (qa * gz + qb * gy - qc * gx)
#Normalise quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3)
q0 *= recipNorm
q1 *= recipNorm
q2 *= recipNorm
q3 *= recipNorm
'''
#Fill message
msg.orientation.x = q1
msg.orientation.y = q2
msg.orientation.z = q3
msg.orientation.w = q0
msg.orientation_covariance[0] = q1 * q1
msg.orientation_covariance[0] = q2 * q2
msg.orientation_covariance[0] = q3 * q3
msg.angular_velocity.x = m9g[0]
msg.angular_velocity.y = m9g[1]
msg.angular_velocity.z = m9g[2]
msg.angular_velocity_covariance[0] = m9g[0] * m9g[0]
msg.angular_velocity_covariance[4] = m9g[1] * m9g[1]
msg.angular_velocity_covariance[8] = m9g[2] * m9g[2]
msg.linear_acceleration.x = m9a[0]
msg.linear_acceleration.y = m9a[1]
msg.linear_acceleration.z = m9a[2]
msg.linear_acceleration_covariance[0] = m9a[0] * m9a[0]
msg.linear_acceleration_covariance[4] = m9a[1] * m9a[1]
msg.linear_acceleration_covariance[8] = m9a[2] * m9a[2]
pub.publish(msg)
rate.sleep()
import numpy as np from MPU9250 import MPU9250 from BMP280 import BMP280 m = MPU9250() b = BMP280() print(b.pressure(), 'Pa') avg_x_accel = lambda x: np.array([list(m.readAccel().values()) for _ in range(x)]).mean(axis=0) x = np.sum(avg_x_accel(10) - avg_x_accel(10)) print(x)
# save StopTime
while pi.read(GPIO_ECHO) == 1: pass # Measure the duration of echo
StopTime = time.time() # Record when it stopped
# Duration of the echo is the difference between start & stop times
TimeElapsed = StopTime - StartTime
if TimeElapsed < 0: print("Sensor error!")
# multiply by the calculated sonic speed
return (TimeElapsed * SoS) / 2 # and divide by 2, because there & back
if __name__ == '__main__':
import pigpio # Standard Raspberry Pi GPIO library
pi = pigpio.pi() # Create a Raspberry Pi object
init(pi) # Initialise ultrasonic sensor
from MPU9250 import MPU9250 # Class to create an IMU
IMU = MPU9250()
try:
dist =()
while True: # Forever
# Compensate for ambient temperature affecting the speed of sound
dist += (distance(pi, IMU.readTemperature()),) # Measure the distance
# print ("Measured Distance = %.1f cm" % dist[-1]) # print it
time.sleep(0.01) # Snooze
if len(dist) > 20:
average = round(sum(dist)/len(dist),1)
print("Averaged distance", average)
dist = (average,)
# Reset by pressing CTRL + C
except KeyboardInterrupt:
################SCANNING VARS#################
WALL_DISTANCE = 39 # Ultrasonic reading to determine something is a wall
OBJ_DISTANCE = 39 # Ultrasonic reading to determine something is a object
BP.set_sensor_type(
mag_sensor_port, BP.SENSOR_TYPE.CUSTOM, [(BP.SENSOR_CUSTOM.PIN1_ADC)]
) # Configure for an analog on sensor port pin 1, and poll the analog line on pin 1.
BP.set_sensor_type(BUTTON, BP.SENSOR_TYPE.NXT_TOUCH)
#BP.set_sensor_type(mag_sensor_port, BP.SENSOR_TYPE.NXT_LIGHT_ON)
BP.set_sensor_type(LIGHT, BP.SENSOR_TYPE.NXT_LIGHT_ON)
IR_TOLERANCE = 1
#############IMU FILTERING####################
mpu9250 = MPU9250() #initalize IMU
#Parameters
width = 2
depth = 100
dly = 0.01
adv = True
#/////////
flter = [[0.7, 1.0], [0.7, 1.0], [0.7, 1.0], [0.7, 1.0], [0.7, 1.0],
[0.7, 1.0]] # [r,q]Will need to play with each filter value
biases = AvgCali(mpu9250, depth, dly)
state = [[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[0, 0, 0, 0, 0, 0]] #Estimated error (p) and measurement state (x)
out = [0, 0, 0, 0, 0, 0]
std = FindSTD(biases, mpu9250, dly)