def __init__(self):
self.g = [0, 0, 1]
try:
self.ch = Spatial()
except RuntimeError as e:
print "FailAccel"
self.stopped = True
self.dynAccel = [0, 0, 0]
self.previousDynAccel = [0, 0, 0]
self.velocity = [0, 0, 0]
self.previousVelocity = [0, 0, 0]
self.distance = [0, 0, 0]
self.prevTime = time.time()
self.initTime = time.time()
self.q = [0, 0, 0, 0]
self.gyroOffset = [0, 0, 0]
self.F = open(
"/home/pi/MLRobot/Data2/AccelRawDyn" +
time.strftime("%e:%H:%M:%S", time.localtime(time.time())), "w")
line = "##########Next Run######### Trial " + str(
trialNumber) + "\nTime,Ax,Ay,Az,Gx,Gy,Gz, DynX, DynY, DynZ\n"
self.F.write(line)
self.ch.setOnSpatialDataHandler(self.AccelerationChangeHandler)
self.ch.openWaitForAttachment(5000)
self.ch.setDataInterval(int(samplingTime))
self.ch.zeroGyro()
def __init__(self):
self.ch = Spatial()
super(SpatialDevice, self).__init__(self.ch)
self.last_angles = [0.0, 0.0, 0.0]
self.compass_bearing_filter = []
self.bearing_filter_size = 2
self.compass_bearing = 0
def __init__(self):
# read info from vehicle
spatial0 = Spatial()
spatial0.setOnSpatialDataHandler(self.onSpatialData)
spatial0.openWaitForAttachment(5000)
# arm vehicle to see position
print('Gyro Armed')
# - Read the actual attitude: Roll, Pitch, and Yaw
self.UpdateGyro()
self.StartingGyro = self.Gyro
print('Orientation: ', self.getStartingGyro())
# - Read the actual position North, East, and Down
self.UpdatePosition()
self.StartingPosition = self.Position
print('Position: ', self.getStartingPosition())
# - Read the actual depth:
time.sleep(3)
print("Starting gyro: ", self.StartingGyro)
class AccelerometerBind():
def __init__(self):
self.g = [0, 0, 1]
try:
self.ch = Spatial()
except RuntimeError as e:
print "FailAccel"
self.stopped = True
self.dynAccel = [0, 0, 0]
self.previousDynAccel = [0, 0, 0]
self.velocity = [0, 0, 0]
self.previousVelocity = [0, 0, 0]
self.distance = [0, 0, 0]
self.prevTime = time.time()
self.initTime = time.time()
self.q = [0, 0, 0, 0]
self.gyroOffset = [0, 0, 0]
self.F = open(
"/home/pi/MLRobot/Data2/AccelRawDyn" +
time.strftime("%e:%H:%M:%S", time.localtime(time.time())), "w")
line = "##########Next Run######### Trial " + str(
trialNumber) + "\nTime,Ax,Ay,Az,Gx,Gy,Gz, DynX, DynY, DynZ\n"
self.F.write(line)
self.ch.setOnSpatialDataHandler(self.AccelerationChangeHandler)
self.ch.openWaitForAttachment(5000)
self.ch.setDataInterval(int(samplingTime))
self.ch.zeroGyro()
def reset(self):
global trialNumber
line = "##########Next Run######### Trial " + str(
trialNumber) + "\nAx,Ay,Az,Gx,Gy,Gz, DynX, DynY, DynZ\n"
self.F.write(line)
self.dynAccel = [0, 0, 0] #x,y,z
self.previousDynAccel = [0, 0, 0]
self.velocity = [0, 0, 0]
self.previousVelocity = [0, 0, 0]
self.distance = [0, 0, 0]
self.ch.zeroGyro()
self.prevTime = time.time()
def AccelerationChangeHandler(self, e, acceleration, angularRate,
magneticField, timestamp):
accel = [0, 0, 0]
gyro = [0, 0, 0]
accel[0] = acceleration[0] * 9.81
accel[1] = acceleration[1] * 9.81
accel[2] = acceleration[2] * 9.81
gyro[0] = -angularRate[0] * 3.14159265 / 180
gyro[1] = -angularRate[1] * 3.14159265 / 180
gyro[2] = -angularRate[2] * 3.14159265 / 180
if self.stopped:
normalVector = np.cross(self.g, accel)
normalVector = normalVector / LA.norm(normalVector)
theta = acos(
np.dot(accel, self.g) / (LA.norm(accel) * LA.norm(self.g)))
self.q = [
cos(theta / 2), normalVector[0] * sin(theta / 2),
normalVector[1] * sin(theta / 2),
normalVector[2] * sin(theta / 2)
]
self.prevTime = time.time()
self.initialTime = time.time()
print self.q
if not self.stopped:
currentTime = time.time()
deltaT = currentTime - self.prevTime
self.prevTime = currentTime
if not LA.norm(gyro) < 0.005:
a = LA.norm(gyro) * deltaT
v = gyro / LA.norm(gyro)
q_update = [
cos(a / 2), v[0] * sin(a / 2), v[1] * sin(a / 2),
v[2] * sin(a / 2)
]
self.q = self.q_mult(q_update, self.q) # Get rid of gyro TODO
self.dynAccel = self.qv_mult(self.q_conjugate(
self.q), accel) # conjugate first to get reverse rotation
self.velocity = np.add(
self.velocity,
(deltaT / 2) * np.add(self.dynAccel, self.previousDynAccel))
self.distance = np.add(
self.distance,
(deltaT / 2) * np.add(self.velocity, self.previousVelocity))
self.F.write(
str(time.time() - self.initialTime) + "," +
str(acceleration[0]) + "," + str(acceleration[1]) + "," +
str(acceleration[2]) + "," + str(angularRate[0]) + "," +
str(angularRate[1]) + "," + str(angularRate[2]) + "," +
str(self.dynAccel[0]) + "," + str(self.dynAccel[1]) + "," +
str(self.dynAccel[2]) + "\n")
self.previousDynAccel = self.dynAccel
self.previousVelocity = self.velocity
print self.distance[0], self.distance[1]
def qv_mult(self, q1, v1):
q2 = [0, 0, 0, 0]
q2[0] = 0
q2[1] = v1[0]
q2[2] = v1[1]
q2[3] = v1[2]
return self.q_mult(self.q_mult(q1, q2), self.q_conjugate(q1))[1:]
def q_conjugate(self, q):
w, x, y, z = q
return (w, -x, -y, -z)
def q_mult(self, q1, q2):
w1, x1, y1, z1 = q1
w2, x2, y2, z2 = q2
w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
y = w1 * y2 + y1 * w2 + z1 * x2 - x1 * z2
z = w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2
return w, x, y, z
def stop(self):
self.stopped = True
self.ch.setDataInterval(1000)
def start(self):
self.prevTime = time.time()
self.dynAccel = [0, 0, 0] #x,y,z
self.ch.zeroGyro()
self.ch.setDataInterval(int(samplingTime))
#sleep(2.05) # wait for gyro #TODO
self.stopped = False
import sys
import time
import csv
import math
import array
from Phidget22.Devices.Spatial import *
from Phidget22.PhidgetException import *
from Phidget22.Phidget import *
from Phidget22.Net import *
from cmath import *
#from SpatialTry import *
lastAngles = [0, 0, 0]
try:
ch = Spatial()
except RuntimeError as e:
print("Runtime Exception %s" % e.details)
print("Press Enter to Exit...\n")
readin = sys.stdin.read(1)
exit(1)
def SpatialAttached(e):
try:
attached = e
attached.resetMagnetometerCorrectionParameters()
attached.setMagnetometerCorrectionParameters(
0.36341, 0.09927, 0.11774, -0.07688, 2.66012, 2.56018, 3.03476,
0.01156, -0.07656, 0.00600, 0.06844, -0.05628, 0.07447)
print("\nAttach Event Detected (Information Below)")
class SpatialDevice(Device):
def __init__(self):
self.ch = Spatial()
super(SpatialDevice, self).__init__(self.ch)
self.last_angles = [0.0, 0.0, 0.0]
self.compass_bearing_filter = []
self.bearing_filter_size = 2
self.compass_bearing = 0
def on_attach(self):
self.ch.setDataInterval(500)
self.listen("SpatialData", self.__on_data)
def __on_data(self, ch, acceleration, angularRate, magneticField,
timestamp):
self.CalculateBearing(acceleration, angularRate, magneticField,
timestamp)
self.set_event_val("acc_x", acceleration[0])
self.set_event_val("acc_y", acceleration[1])
self.set_event_val("acc_z", acceleration[2])
self.set_event_val("ang_x", angularRate[0])
self.set_event_val("ang_y", angularRate[1])
self.set_event_val("ang_z", angularRate[2])
self.set_event_val("mag_x", magneticField[0])
self.set_event_val("mag_y", magneticField[1])
self.set_event_val("mag_z", magneticField[2])
def CalculateBearing(self, acceleration, angularRate, magneticField,
timestamp):
gravity = acceleration
mag_field = magneticField
angles = []
roll_angle = math.atan2(gravity[1], gravity[2])
pitch_angle = math.atan(-gravity[0] /
(gravity[1] * math.sin(roll_angle) +
gravity[2] * math.cos(roll_angle)))
yaw_angle = math.atan2(
mag_field[2] * math.sin(roll_angle) -
mag_field[1] * math.cos(roll_angle),
mag_field[0] * math.cos(pitch_angle) +
mag_field[1] * math.sin(pitch_angle) * math.sin(roll_angle) +
mag_field[2] * math.sin(pitch_angle) * math.cos(roll_angle))
angles.append(roll_angle)
angles.append(pitch_angle)
angles.append(yaw_angle)
try:
for i in xrange(0, 3, 2):
if math.fabs(angles[i] - self.last_angles[i] > 3):
for stuff in self.compass_bearing_filter:
if angles[i] > self.last_angles[i]:
stuff[i] += 360 * math.pi / 180.0
else:
stuff[i] -= 360 * math.pi / 180.0
self.last_angles = angles
self.compass_bearing_filter.append(angles)
if len(self.compass_bearing_filter) > self.bearing_filter_size:
self.compass_bearing_filter.pop(0)
yaw_angle = pitch_angle = roll_angle = 0
for stuff in self.compass_bearing_filter:
roll_angle += stuff[0]
pitch_angle += stuff[1]
yaw_angle += stuff[2]
yaw_angle = yaw_angle / len(self.compass_bearing_filter)
pitch_angle = pitch_angle / len(self.compass_bearing_filter)
roll_angle = roll_angle / len(self.compass_bearing_filter)
# Convert radians to degrees for display
self.compass_bearing = yaw_angle * (180.0 / math.pi) % 360
self.set_event_val("compass_bearing",
round(self.compass_bearing, 3))
self.set_event_val("pitch_angle",
round(pitch_angle * (180.0 / math.pi), 3))
self.set_event_val("roll_angle",
round(roll_angle * (180.0 / math.pi), 3))
except Exception as e:
logging.error(e)
import sys
import time
from math import sqrt, cos, sin, atan2
from Phidget22.Devices.Spatial import *
from Phidget22.PhidgetException import *
from Phidget22.Phidget import *
from Phidget22.Net import *
from scipy import signal
from scipy import integrate
import numpy as np
try:
ch = Spatial()
except RuntimeError as e:
print("Runtime Exception %s" % e.details)
print("Press Enter to Exit...\n")
readin = sys.stdin.read(1)
exit(1)
def AccelerometerAttached(e):
try:
attached = e
print("\nAttach Event Detected (Information Below)")
print("===========================================")
print("Library Version: %s" % attached.getLibraryVersion())
print("Serial Number: %d" % attached.getDeviceSerialNumber())
print("Channel: %d" % attached.getChannel())
print("Channel Class: %s" % attached.getChannelClass())
print("Channel Name: %s" % attached.getChannelName())
print("Device ID: %d" % attached.getDeviceID())
import sys
import time
import numpy as np
import threading
from Phidget22.Devices.Spatial import *
from Phidget22.PhidgetException import *
from Phidget22.Phidget import *
from Phidget22.Net import *
try:
ch = Spatial()
except RuntimeError as e:
print("Runtime Exception %s" % e.details)
print("Press Enter to Exit...\n")
readin = sys.stdin.read(1)
exit(1)
def SpatialAttached(self):
try:
attached = self
print("\nAttach Event Detected (Information Below)")
print("===========================================")
print("Library Version: %s" % attached.getLibraryVersion())
print("Serial Number: %d" % attached.getDeviceSerialNumber())
print("Channel: %d" % attached.getChannel())
print("Channel Class: %s" % attached.getChannelClass())
print("Channel Name: %s" % attached.getChannelName())
print("Device ID: %d" % attached.getDeviceID())
print("Device Version: %d" % attached.getDeviceVersion())
print("Device Name: %s" % attached.getDeviceName())
magneticField(type: list[float]):The field strength values
timestamp(type: float):The timestamp value
"""
from Phidget22.Phidget import *
from Phidget22.Devices.Spatial import *
import time
import numpy as np
array = []
def onSpatialData(self, acceleration, angularRate, magneticField, timestamp):
array.append([timestamp,acceleration[0],acceleration[1], acceleration[2],angularRate[0],angularRate[1],angularRate[2], magneticField[0], magneticField[1], magneticField[2]])
ch = Spatial()
# Register for event before calling open
ch.setOnSpatialDataHandler(onSpatialData)
print("Waiting for the Phidget TemperatureSensor Object to be attached...")
ch.openWaitForAttachment(5000) # időtúllépés 5 s
ch.setDataInterval(20) # 20 ms a mintavételezési idő
ch.open()
# enter-ig olvas
try:
input("Press Enter to Stop\n")
except (Exception, KeyboardInterrupt):
pass
ch.close()
# Event Manager Definitions
#########################################################################
tiempo = []
ac0 = []
ac1 = []
ac2 = []
gr0 = []
gr1 = []
gr2 = []
mg0 = []
mg1 = []
mg2 = []
try:
Space = Spatial()
except RuntimeError as e:
print("Runtime Exception %s" % e.details)
print("Press Enter to Exit...\n")
readin = sys.stdin.read(1)
exit(1)
########################### Spatial Setup
def SpatialAttached(e):
try:
attached = e
print("\nAttach Event Detected (Information Below)")
print("===========================================")
print("Library Version: %s" % attached.getLibraryVersion())
