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
yawAngle /= Count
pitchAngle /= Count
rollAngle /= Count
compassBearing = yawAngle * (180.0 / math.pi)
# print("Bearing after: %.2f" % compassBearing)
except:
print()
try:
ch.setOnAttachHandler(SpatialAttached)
ch.setOnDetachHandler(SpatialDetached)
ch.setOnErrorHandler(ErrorEvent)
ch.setOnSpatialDataHandler(calculateCompassBearing)
print("Waiting for the Phidget Spatial Object to be attached...")
file = open("dir.txt", "a")
file.write("--------------------------------------------")
#file.write("\n")
ch.openWaitForAttachment(5000)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Press Enter to Exit...\n")
readin = sys.stdin.read(1)
exit(1)
print("Gathering data for 10 seconds...")
velocityX = integrate.cumtrapz(
(prevDynAccel, dynAccel), dx=0.03) + velocityX
velocityX = velocityX[0]
distance = integrate.cumtrapz(
(prevVelocity, velocityX), dx=0.03) + distance
prevDynAccel = dynAccel
prevVelocity = velocityX
distance = distance[0]
print dynAccel, velocityX, distance, timestamp
try:
ch.setOnAttachHandler(AccelerometerAttached)
ch.setOnDetachHandler(AccelerometerDetached)
ch.setOnErrorHandler(ErrorEvent)
ch.setOnSpatialDataHandler(AccelerationChangeHandler)
print("Waiting for the Phidget Accelerometer Object to be attached...")
ch.openWaitForAttachment(5000)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Press Enter to Exit...\n")
readin = sys.stdin.read(1)
exit(1)
print("Gathering data for 10 seconds...")
ch.setDataInterval(30)
time.sleep(15)
try:
ch.close()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Control output: %f\n" % steeringAngle)
if (steeringAngle < -45.0):
steeringAngle = -45.0
elif (steeringAngle > 45.0):
steeringAngle = 45.0
bc_previous_error = error
threading.Timer(dt, balanceControl).start()
try:
ch.setOnAttachHandler(SpatialAttached)
ch.setOnDetachHandler(SpatialDetached)
ch.setOnErrorHandler(ErrorEvent)
ch.setOnSpatialDataHandler(SpatialDataHandler)
# Please review the Phidget22 channel matching documentation for details on the device
# and class architecture of Phidget22, and how channels are matched to device features.
# Specifies the serial number of the device to attach to.
# For VINT devices, this is the hub serial number.
#
# The default is any device.
#
# ch.setDeviceSerialNumber(<YOUR DEVICE SERIAL NUMBER>)
# For VINT devices, this specifies the port the VINT device must be plugged into.
#
# The default is any port.
#
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()
np.savetxt('data.csv', array,fmt='%10.6f', delimiter=',', newline='\n', comments='')
gr0.append(angularRate[0])
gr1.append(angularRate[1])
gr2.append(angularRate[2])
mg0.append(fieldStrength[0])
mg1.append(fieldStrength[1])
mg2.append(fieldStrength[2])
# Programa Principal
#########################################################################
try:
Space.setOnAttachHandler(SpatialAttached)
Space.setOnDetachHandler(SpatialDetached)
Space.setOnErrorHandler(ErrorEvent)
Space.setOnSpatialDataHandler(SpatialDataHandler)
#########################################################################
print("Waiting for the Phidget Spatial Object to be attached...")
Space.openWaitForAttachment(5000)
print("Spatial attached")
#########################################################################
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Press Enter to Exit...\n")
readin = sys.stdin.read(1)
exit(1)
# Definir el tiempo de muestro del acelerometro (En ms)
T_muestreo = 50 # 20 mediciones por segundo
Space.setDataInterval(T_muestreo)