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
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)
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("Press Enter to Exit...\n")
readin = sys.stdin.read(1)
exit(1)
print("Closed Accelerometer device")
exit(0)
# ch.setChannel(0)
# In order to attach to a network Phidget, the program must connect to a Phidget22 Network Server.
# In a normal environment this can be done automatically by enabling server discovery, which
# will cause the client to discovery and connect to available servers.
#
# To force the channel to only match a network Phidget, set remote to 1.
#
# Net.enableServerDiscovery(PhidgetServerType.PHIDGETSERVER_DEVICEREMOTE);
# ch.setIsRemote(1)
print("Waiting for the Phidget Spatial Object to be attached...")
ch.openWaitForAttachment(5000)
ch.zeroGyro()
time.sleep(3)
ch.setDataInterval(40)
time.sleep(3) # wait for the readings to stabilize
balanceControl()
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...")
time.sleep(1000)
try:
ch.close()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
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='')
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)
# Muestrear 5 segundos
time.sleep(5)
try:
Space.close()
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("Closed Spatial Device")
# Convetir los vectores a arreglos de numpy para hacer los calculos mas eficientes