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
# The default is any channel.
#
# 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()