else: roll = math.atan(accY/math.sqrt((accX**2)+(accZ**2))) * radToDeg pitch = math.atan2(-accX,accZ) * radToDeg gyroXRate = gyroX/131 gyroYRate = gyroY/131 if (RestrictPitch): if((roll < -90 and kalAngleX >90) or (roll > 90 and kalAngleX < -90)): kalmanX.setAngle(roll) complAngleX = roll kalAngleX = roll gyroXAngle = roll else: kalAngleX = kalmanX.getAngle(roll,gyroXRate,dt) if(abs(kalAngleX)>90): gyroYRate = -gyroYRate kalAngleY = kalmanY.getAngle(pitch,gyroYRate,dt) else: if((pitch < -90 and kalAngleY >90) or (pitch > 90 and kalAngleY < -90)): kalmanY.setAngle(pitch) complAngleY = pitch kalAngleY = pitch gyroYAngle = pitch else: kalAngleY = kalmanY.getAngle(pitch,gyroYRate,dt) if(abs(kalAngleY)>90):
class PhidgetKalman(object):
#Read the gyro and acceleromater values from MPU6050
def __init__(self):
self.kalmanX = KalmanAngle()
self.kalmanY = KalmanAngle()
self.RestrictPitch = True #Comment out to restrict roll to ±90deg instead - please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf
self.radToDeg = 57.2957786
self.kalAngleX = 0
self.kalAngleY = 0
try:
self.accel = Accelerometer() #ch.getAcceleration()
self.gyro = Gyroscope() #ch.getAngularRate()
except Exception as e:
print(
"Cannot initalize, try checking you have the Phidget22 library installed."
)
print("Error:", e)
sys.exit()
# return self.accel, self.gyro
def start(self):
try:
self.accel.openWaitForAttachment(1000)
self.gyro.openWaitForAttachment(1000)
except Exception as e:
print(
"Issue with attaching to IMU. The IMU maybe connected to something else right now.."
)
print("Error:", e)
sys.exit()
def stop(self):
self.accel.close()
self.gyro.close()
print("Stopping")
def get_angles(self, measure_time=5):
#Keep going
time.sleep(1)
accX, accY, accZ = self.accel.getAcceleration()
if (self.RestrictPitch):
roll = math.atan2(accY, accZ) * self.radToDeg
pitch = math.atan(-accX / math.sqrt((accY**2) +
(accZ**2))) * self.radToDeg
else:
roll = math.atan(accY / math.sqrt((accX**2) +
(accZ**2))) * self.radToDeg
pitch = math.atan2(-accX, accZ) * self.radToDeg
# print(roll)
self.kalmanX.setAngle(roll)
self.kalmanX.setAngle(pitch)
gyroXAngle = roll
gyroYAngle = pitch
compAngleX = roll
compAngleY = pitch
timer = time.time()
compare_timer = time.time()
flag = 0
try:
while int(time.time()) < int(measure_time) + int(
compare_timer
): #Should only run for about 5 seconds <- Could make lower
#Read Accelerometer raw value
accX, accY, accZ = self.accel.getAcceleration()
#Read Gyroscope raw value
gyroX, gyroY, gyroZ = self.gyro.getAngularRate()
dt = time.time() - timer
timer = time.time()
if (self.RestrictPitch):
roll = math.atan2(accY, accZ) * self.radToDeg
pitch = math.atan(
-accX / math.sqrt((accY**2) +
(accZ**2))) * self.radToDeg
else:
roll = math.atan(
accY / math.sqrt((accX**2) +
(accZ**2))) * self.radToDeg
pitch = math.atan2(-accX, accZ) * self.radToDeg
gyroXRate = gyroX / 131
gyroYRate = gyroY / 131
if (self.RestrictPitch):
if ((roll < -90 and self.kalAngleX > 90)
or (roll > 90 and self.kalAngleX < -90)):
self.kalmanX.setAngle(roll)
complAngleX = roll
self.kalAngleX = roll
gyroXAngle = roll
else:
self.kalAngleX = self.kalmanX.getAngle(
roll, gyroXRate, dt)
if (abs(self.kalAngleX) > 90):
gyroYRate = -gyroYRate
self.kalAngleY = self.kalmanX.getAngle(
pitch, gyroYRate, dt)
else:
if ((pitch < -90 and self.kalAngleY > 90)
or (pitch > 90 and self.kalAngleY < -90)):
self.kalmanX.setAngle(pitch)
complAngleY = pitch
self.kalAngleY = pitch
gyroYAngle = pitch
else:
self.kalAngleY = self.kalmanX.getAngle(
pitch, gyroYRate, dt)
if (abs(self.kalAngleY) > 90):
gyroXRate = -gyroXRate
self.kalAngleX = self.kalmanX.getAngle(
roll, gyroXRate, dt)
#angle = (rate of change of angle) * change in time
gyroXAngle = gyroXRate * dt
gyroYAngle = gyroYAngle * dt
#compAngle = constant * (old_compAngle + angle_obtained_from_gyro) + constant * angle_obtained from accelerometer
compAngleX = 0.93 * (compAngleX + gyroXRate * dt) + 0.07 * roll
compAngleY = 0.93 * (compAngleY +
gyroYRate * dt) + 0.07 * pitch
if ((gyroXAngle < -180) or (gyroXAngle > 180)):
gyroXAngle = self.kalAngleX
if ((gyroYAngle < -180) or (gyroYAngle > 180)):
gyroYAngle = self.kalAngleY
self.X_angle = self.kalAngleX
self.Y_angle = self.kalAngleY
#print("Angle X: " + str(self.kalAngleX)+" " +"Angle Y: " + str(self.kalAngleY))
#print(str(roll)+" "+str(gyroXAngle)+" "+str(compAngleX)+" "+str(self.kalAngleX)+" "+str(pitch)+" "+str(gyroYAngle)+" "+str(compAngleY)+" "+str(self.kalAngleY))
time.sleep(0.005)
except KeyboardInterrupt:
print("test")
return self.X_angle, self.Y_angle
class GYRO(object):
def __init__(self):
# Register
self.power_mgmt_1 = 0x6b
self.bus = smbus.SMBus(1) # bus = smbus.SMBus(0) for Revision 1
self.address = 0x68 # via i2c detect, address research!
self.bus.write_byte_data(self.address, 0x19, 7)
# activate to communicate with the module
# first argument is address, second is offset, third is data
self.bus.write_byte_data(self.address, self.power_mgmt_1, 1)
self.bus.write_byte_data(self.address, 0x1A, 0)
self.bus.write_byte_data(self.address, 0x1B, 24)
self.bus.write_byte_data(self.address, 0x38, 1)
self.gyroscopeX = 0
self.gyroscopeY = 0
self.gyroscopeZ = 0
self.gyroscopeXScaled = 0
self.gyroscopeYScaled = 0
self.gyroscopeZScaled = 0
self.accelerationX = 0
self.accelerationY = 0
self.accelerationZ = 0
self.accelerationXScaled = 0
self.accelerationYScaled = 0
self.accelerationZScaled = 0
self.xRotation = 0
self.yRotation = 0
self.yRotationRaw = 0
# kalman
self.kalmanX = KalmanAngle()
self.kalmanY = KalmanAngle()
self.kalAngleX = 0
self.kalAngleY = 0
self.timer = time.time()
self.roll = 0
self.pitch = 0
self.kalmanX.setAngle(self.roll)
self.kalmanY.setAngle(self.pitch)
self.gyroXAngle = 0
self.gyroYAngle = 0
self.compAngleX = 0
self.compAngleY = 0
print("gyroscope initialized")
self.yRotationk_1 = 0
# Methods
def read_byte(self, reg):
return self.bus.read_byte_data(self.address, reg)
def read_word(self, reg):
h = self.bus.read_byte_data(self.address, reg)
l = self.bus.read_byte_data(self.address, reg + 1)
value = (h << 8) + l
return value
def read_word_2c(self, reg):
value = self.read_word(reg)
if value >= 0x8000:
return -((65535 - value) + 1)
else:
return value
def dist(self, a, b):
return math.sqrt((a * a) + (b * b))
def get_y_rotation(self, x, y, z):
radians = math.atan2(x, self.dist(y, z))
return -math.degrees(radians)
def get_x_rotation(self, x, y, z):
radians = math.atan2(y, self.dist(x, z))
return math.degrees(radians)
def read_gyro(self):
"""
writes the y-rotation in the yRotation variable
"""
self.gyroscopeX = self.read_word_2c(0x43)
self.gyroscopeY = self.read_word_2c(0x45)
self.gyroscopeZ = self.read_word_2c(0x47)
self.gyroscopeXScaled = self.gyroscopeX / 131
self.gyroscopeYScaled = self.gyroscopeY / 131
self.gyroscopeZScaled = self.gyroscopeZ / 131
self.accelerationX = self.read_word_2c(0x3b)
self.accelerationY = self.read_word_2c(0x3d)
self.accelerationZ = self.read_word_2c(0x3f)
self.accelerationXScaled = self.accelerationX / 16384.0
self.accelerationYScaled = self.accelerationY / 16384.0
self.accelerationZScaled = self.accelerationZ / 16384.0
self.kalmanX.setAngle(self.roll)
self.kalmanY.setAngle(self.pitch)
self.xRotation = self.roll
self.yRotation = self.pitch
self.compAngleX = self.roll
self.compAngleY = self.pitch
dt = time.time() - self.timer
self.timer = time.time()
self.roll = math.atan(
self.accelerationY / math.sqrt((self.accelerationX ** 2) + (self.accelerationZ ** 2))) * 57.2957786
self.pitch = math.atan2(-self.accelerationX, self.accelerationZ) * 57.2957786
gyroXRate = self.gyroscopeXScaled
gyroYRate = self.gyroscopeYScaled
if (self.pitch < -90 and self.kalAngleY > 90) or (self.pitch > 90 and self.kalAngleY < -90):
self.kalmanY.setAngle(self.pitch)
self.compAngleY = self.pitch
self.kalAngleY = self.pitch
self.gyroYAngle = self.pitch
else:
self.kalAngleY = self.kalmanY.getAngle(self.pitch, gyroYRate, dt)
if abs(self.kalAngleY) > 90:
gyroXRate = -gyroXRate
self.kalAngleX = self.kalmanX.getAngle(self.roll, gyroXRate, dt)
self.gyroXAngle = gyroXRate * dt
self.gyroYAngle = self.gyroYAngle * dt
self.compAngleX = 0.93 * (self.compAngleX + gyroXRate * dt) + 0.07 * self.roll
self.compAngleY = 0.93 * (self.compAngleY + gyroYRate * dt) + 0.07 * self.pitch
self.xRotation = self.kalAngleX
self.yRotationk_1 = self.yRotationRaw
self.yRotationRaw = self.kalAngleY
self.yRotation = (self.yRotationk_1 + self.yRotationRaw) / 2
def measureAngles(self):
flag = 0
kalmanX = KalmanAngle()
kalmanY = KalmanAngle()
RestrictPitch = True # Comment out to restrict roll to ±90deg instead - please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf
radToDeg = 57.2957786
kalAngleX = 0
kalAngleY = 0
# some MPU6050 Registers and their Address
ACCEL_XOUT_H = 0x3B
ACCEL_YOUT_H = 0x3D
ACCEL_ZOUT_H = 0x3F
GYRO_XOUT_H = 0x43
GYRO_YOUT_H = 0x45
GYRO_ZOUT_H = 0x47
time.sleep(1)
# Read Accelerometer raw value
accX = self.read_raw_data(ACCEL_XOUT_H)
accY = self.read_raw_data(ACCEL_YOUT_H)
accZ = self.read_raw_data(ACCEL_ZOUT_H)
# print(accX,accY,accZ)
# print(math.sqrt((accY**2)+(accZ**2)))
if (RestrictPitch):
roll = math.atan2(accY, accZ) * radToDeg
pitch = math.atan(-accX / math.sqrt((accY**2) +
(accZ**2))) * radToDeg
else:
roll = math.atan(accY / math.sqrt((accX**2) +
(accZ**2))) * radToDeg
pitch = math.atan2(-accX, accZ) * radToDeg
#print(roll)
kalmanX.setAngle(roll)
kalmanY.setAngle(pitch)
gyroXAngle = roll
gyroYAngle = pitch
compAngleX = roll
compAngleY = pitch
timer = time.time()
flag = 0
while True:
if (flag > 100): #Problem with the connection
print("There is a problem with the connection")
flag = 0
continue
try:
#Read Accelerometer raw value
accX = self.read_raw_data(ACCEL_XOUT_H)
accY = self.read_raw_data(ACCEL_YOUT_H)
accZ = self.read_raw_data(ACCEL_ZOUT_H)
#Read Gyroscope raw value
gyroX = self.read_raw_data(GYRO_XOUT_H)
gyroY = self.read_raw_data(GYRO_YOUT_H)
gyroZ = self.read_raw_data(GYRO_ZOUT_H)
dt = time.time() - timer
timer = time.time()
if (RestrictPitch):
roll = math.atan2(accY, accZ) * radToDeg
pitch = math.atan(-accX / math.sqrt((accY**2) +
(accZ**2))) * radToDeg
else:
roll = math.atan(accY / math.sqrt((accX**2) +
(accZ**2))) * radToDeg
pitch = math.atan2(-accX, accZ) * radToDeg
gyroXRate = gyroX / 131
gyroYRate = gyroY / 131
if (RestrictPitch):
if ((roll < -90 and kalAngleX > 90)
or (roll > 90 and kalAngleX < -90)):
kalmanX.setAngle(roll)
complAngleX = roll
kalAngleX = roll
gyroXAngle = roll
else:
kalAngleX = kalmanX.getAngle(roll, gyroXRate, dt)
if (abs(kalAngleY) > 90 or True):
gyroYRate = -gyroYRate
kalAngleY = kalmanY.getAngle(pitch, gyroYRate, dt)
else:
if ((pitch < -90 and kalAngleY > 90)
or (pitch > 90 and kalAngleY < -90)):
kalmanY.setAngle(pitch)
complAngleY = pitch
kalAngleY = pitch
gyroYAngle = pitch
else:
kalAngleY = kalmanY.getAngle(pitch, gyroYRate, dt)
if (abs(kalAngleX) > 90):
gyroXRate = -gyroXRate
kalAngleX = kalmanX.getAngle(roll, gyroXRate, dt)
#angle = (rate of change of angle) * change in time
gyroXAngle = gyroXRate * dt
gyroYAngle = gyroYAngle * dt
#compAngle = constant * (old_compAngle + angle_obtained_from_gyro) + constant * angle_obtained from accelerometer
compAngleX = 0.93 * (compAngleX + gyroXRate * dt) + 0.07 * roll
compAngleY = 0.93 * (compAngleY +
gyroYRate * dt) + 0.07 * pitch
if ((gyroXAngle < -180) or (gyroXAngle > 180)):
gyroXAngle = kalAngleX
if ((gyroYAngle < -180) or (gyroYAngle > 180)):
gyroYAngle = kalAngleY
#print("Angle X: " + str(complAngleX)+" " +"Angle Y: " + str(complAngleY))
self.pitch = compAngleY
self.roll = compAngleX
self.kalman_pitch = kalAngleY
self.kalman_roll = kalAngleX
self.compl_pitch = compAngleY
self.compl_roll = compAngleX
#print(str(roll)+" "+str(gyroXAngle)+" "+str(compAngleX)+" "+str(kalAngleX)+" "+str(pitch)+" "+str(gyroYAngle)+" "+str(compAngleY)+" "+str(kalAngleY))
time.sleep(0.005)
except Exception as exc:
if (flag == 100):
print(exc)
flag += 1
def RecordLoop(self):
f = open('result.csv', 'w')
rot = 0
mode = 0
kalmanX = KalmanAngle()
radToDeg = 57.2957786
kalAngleX = 0
time.sleep(1)
#Read Accelerometer raw value
accdata = self.sensor.get_accel_data()
accY = accdata["y"]
accZ = accdata["z"]
roll = math.atan2(accY, accZ) * radToDeg
kalmanX.setAngle(roll)
gyroXAngle = roll
compAngleX = roll
timer = time.time()
f.write(str(timer) + '\n')
print('recording commencing')
while self.on:
#Read Accelerometer raw value
accdata = self.sensor.get_accel_data()
accX = accdata["x"]
accY = accdata["y"]
accZ = accdata["z"]
#Read Gyroscope raw value
gyrdata = self.sensor.get_gyro_data()
gyroX = gyrdata["x"]
gyroY = gyrdata["y"]
dt = time.time() - timer
timer = time.time()
roll = math.atan2(accY, accZ) * radToDeg
gyroXRate = gyroX / 131
if ((roll < -90 and kalAngleX > 90)
or (roll > 90 and kalAngleX < -90)):
kalmanX.setAngle(roll)
complAngleX = roll
kalAngleX = roll
gyroXAngle = roll
else:
kalAngleX = kalmanX.getAngle(roll, gyroXRate, dt)
gyroXAngle = gyroXRate * dt
compAngleX = 0.93 * (compAngleX + gyroXRate * dt) + 0.07 * roll
if ((gyroXAngle < -180) or (gyroXAngle > 180)):
gyroXAngle = kalAngleX
mag = getMagnetStrength()
if (mode == 0):
if (mag > self.TRESHOLD):
rot += 1
mode = 1
else:
if (mag < self.TRESHOLD):
mode = 0
f.write(str(kalAngleX) + ',' + str(rot) + '\n')
#f.write(str(kalAngleX)+'\n')
#print(kalAngleX, getMagnetStrength())
print('done recording')
f.write(str(timer))
f.close()