def environ_data(sender):
import motion
import location
motion.start_updates()
location.start_updates()
x = motion.get_attitude()
environ['att'].text = str(x) + '\n' + environ['att'].text
x = motion.get_gravity()
environ['grav'].text = str(x) + '\n' + environ['grav'].text
x = motion.get_user_acceleration()
environ['acc'].text = str(x) + '\n' + environ['acc'].text
x = motion.get_magnetic_field()
environ['mag'].text = str(x) + '\n' + environ['mag'].text
x = location.get_location()
coord = {'latitude': x['latitude'], 'longitude': x['longitude']}
print(coord)
y = location.reverse_geocode(coord)
print(y)
environ['geo'].text = str(x)
motion.stop_updates()
location.stop_updates()
def draw(self):
ac = motion.get_user_acceleration()
self.x = (ac[0] * scale) + self.cx
self.y = (ac[1] * scale) + self.cy
# clip to screen
self.x = min(self.size.w - 10, max(0, self.x))
self.y = min(self.size.h - 100, max(0, self.y))
# save if biggest acceleration so far
self.max = max(self.max, max(abs(ac[0] * scale), abs(ac[1] * scale)))
# redraw screen
background(1, 1, 1)
# show max accel ring
fill(1, 1, 1)
stroke(0, 0, 0)
stroke_weight(1)
ellipse(self.cx - self.max, self.cy - self.max, self.max * 2,
self.max * 2)
# center dot
fill(0, 0, 0)
rect(self.cx, self.cy, 2, 2)
# cur accel
fill(1, 0, 0)
rect(self.x, self.y, 10, 10)
# print accel value
tint(0, 0, 0)
text(str(self.max), 'Helvetica', 16, 50, 50, 6)
def draw(self):
global box_node, box
global pitch, roll, yaw, ax, ay, az, gx, gy, gz
global main_view
global ax1, fig
#処理が追い付かないのでタイマーをいれる
#time.sleep(0.01)
#加速度、ジャイロの値を更新
ax, ay, az = motion.get_user_acceleration()
gx, gy, gz = motion.get_gravity()
gravity_vectors = motion.get_attitude()
mgx, mgy, mgz, mga = motion.get_magnetic_field()
pitch, roll, yaw = [x for x in gravity_vectors]
# ラジアン→度へ変換
pitch = -pitch * 180 / 3.1415926
roll = roll * 180 / 3.1415926
yaw = -yaw * 180 / 3.1415926
#再描画
box_node.runAction_(
SCNAction.rotateToX_y_z_duration_(math.pi * pitch / 180,
math.pi * roll / 180,
-math.pi * yaw / 180, 0))
#加速度、ジャイロ、地磁気センサーの値を表示
main_view['ax'].text = str(round(ax, 2))
main_view['ay'].text = str(round(ay, 2))
main_view['az'].text = str(round(az, 2))
main_view['gx'].text = str(round(gx, 2))
main_view['gy'].text = str(round(gy, 2))
main_view['gz'].text = str(round(gz, 2))
main_view['mx'].text = str(round(mgx, 2))
main_view['my'].text = str(round(mgy, 2))
main_view['mz'].text = str(round(mgz, 2))
# graphics
self.py.pop(0)
self.ry.pop(0)
self.yy.pop(0) # left
self.py.append(pitch)
self.ry.append(roll)
self.yy.append(yaw) # add data
ax1.plot(self.xl, self.py, color='lightgreen', lw='1') # pitch graph
ax1.plot(self.xl, self.ry, color='red', lw='1') # roll graph
ax1.plot(self.xl, self.yy, color='skyblue', lw='1') # yaw graph
plt.savefig('rt.png') # save the graph on the consolen
main_view['imageview1'].image = ui.Image.named('rt.png') # imageview
plt.cla() # clear graph
plt.close() # close graph
fig = plt.figure() #
ax1 = fig.add_subplot(111) #
ax1.grid(True)
ymin = -180
ymax = 180
plt.ylim(ymin, ymax)
def main():
num_samples = 1000000
arrayA = []
arrayM = []
#arrayG = []
arrayP = []
arrayJ = []
arrayGPS = [] #GPS
dataArray = []
CMAltimeter = ObjCClass('CMAltimeter')
NSOperationQueue = ObjCClass('NSOperationQueue')
if not CMAltimeter.isRelativeAltitudeAvailable():
print('This device has no barometer.')
return
altimeter = CMAltimeter.new()
main_q = NSOperationQueue.mainQueue()
altimeter.startRelativeAltitudeUpdatesToQueue_withHandler_(
main_q, handler_block)
motion.start_updates()
location.start_updates() # GPS
print("Logging start...")
sleep(1.0)
for i in range(num_samples):
sleep(0.05)
a = motion.get_user_acceleration()
m = motion.get_magnetic_field()
j = motion.get_attitude()
gps = location.get_location() # GPS
if a[1] > 0.8:
break
dataArray.append([relativeAltitude, a[2], m[0], m[1]])
arrayA.append(a)
arrayM.append(m)
arrayJ.append(j)
arrayP.append(relativeAltitude)
arrayGPS.append(gps) #GPS
motion.stop_updates()
location.stop_updates() # GPS
altimeter.stopRelativeAltitudeUpdates()
print("Logging stop and Saving start...")
import pickle
f = open('yokohama.serialize', 'wb')
#pickle.dump([arrayA, arrayM, arrayP],f)
pickle.dump([arrayA, arrayM, arrayJ, arrayP, arrayGPS], f) #GPS
f.close
print("Saving is finished.")
x_values = [x * 0.05 for x in range(len(dataArray))]
for i, color, label in zip(range(3), 'rgb', 'XYZ'):
plt.plot(x_values, [g[i] for g in arrayM], color, label=label, lw=2)
plt.grid(True)
plt.xlabel('t')
plt.ylabel('G')
plt.gca().set_ylim([-100, 100])
plt.legend()
plt.show()
def ios(rate=quantity(1, units.second)):
"""Retrieve motion information from an iOS device.
This component requires the `motion` module provided by Pythonista.
Parameters
----------
rate: time quantity, required
Rate at which motion data will be retrieved.
Yields
------
records: dict
Records will contain information including the current acceleration due to gravity and the user, along with device attitude.
"""
import time
import motion # pylint: disable=import-error
rate = rate.to(units.seconds).magnitude
motion.start_updates()
try:
while True:
gravity = quantity(motion.get_gravity(), units.meters * units.seconds * units.seconds)
acceleration = quantity(motion.get_user_acceleration(), units.meters * units.seconds * units.seconds)
attitude = quantity(motion.get_attitude(), units.radians)
record = dict()
add_field(record, ("gravity", "x"), gravity[0])
add_field(record, ("gravity", "y"), gravity[1])
add_field(record, ("gravity", "z"), gravity[2])
add_field(record, ("acceleration", "x"), acceleration[0])
add_field(record, ("acceleration", "y"), acceleration[1])
add_field(record, ("acceleration", "z"), acceleration[2])
add_field(record, ("attitude", "roll"), attitude[0])
add_field(record, ("attitude", "pitch"), attitude[1])
add_field(record, ("attitude", "yaw"), attitude[2])
yield record
time.sleep(rate)
except GeneratorExit:
motion.stop_updates()
def update(self):
m = motion.get_magnetic_field()
self.mf.text = '{:.1f} {:.1f} {:.1f} {:.3f}'.format(
m[0], m[1], m[2], m[3])
acc = motion.get_user_acceleration()
self.acc.text = '{:.3f} {:.3f} {:.3f}'.format(acc[0], acc[1], acc[2])
grav = motion.get_gravity()
self.grav.text = '{:.3f} {:.3f} {:.3f}'.format(grav[0], grav[1],
grav[2])
#determine screen orientation. Portrait modes flip 180 deg if the device is face down. Use local z gravity to detect.
orient = int(self.wv.eval_js('window.orientation'))
if (orient == 0):
#portrait, home button on bottom
if grav[2] <= 0.:
init_angle = -90
else:
init_angle = 90
elif (orient == 90):
#landscape, home button on right
init_angle = 0
elif (orient == -90):
#landscape, home button on right
init_angle = 180
else:
#portrait, home button on top
if grav[2] <= 0.:
init_angle = 90
else:
init_angle = -90
self.ori.text = str(orient)
att = motion.get_attitude()
self.att.text = '{:.3f} {:.3f} {:.3f}'.format(att[0], att[1], att[2])
ang = (init_angle - int(math.degrees(att[2]))) % 360
self.ang.text = str(ang)
def main():
console.alert('Magentic Experiment 2',
'yo gang gang, we gonna measure this motion', 'Continue')
motion.start_updates()
sleep(0.2)
print('Capturing motion data...')
while True:
sleep(0.5)
current = motion.get_user_acceleration()
newMotion = [0, 0, 0]
for i in range(len(current)):
newMotion[i] = (current[i] * (10**2)) // 1
x = newMotion[0]
y = newMotion[1]
z = newMotion[2]
print(x, y, z)
motion.stop_updates()
print('Capture finished, plotting...')
async def gravity(self, instance, async_lib):
motion.start_updates()
try:
while True:
timestamp = time.time()
gravity = motion.get_gravity()
user_acceleration = motion.get_user_acceleration()
attitude = motion.get_attitude()
magnetic_field = motion.get_magnetic_field()
await self.gravity.write(value=gravity, timestamp=timestamp)
await self.user_acceleration.write(value=user_acceleration,
timestamp=timestamp)
await self.attitude.write(value=attitude, timestamp=timestamp)
await self.magnetic_field.write(value=magnetic_field,
timestamp=timestamp)
await async_lib.library.sleep(0.01)
finally:
# TODO: put in @gravity.shutdown when in released version
motion.stop_updates()
def draw(self):
#Box
self.cx = self.size.w * 0.5
self.cy = self.size.h * 0.5
#pitch,roll,yaw
self.cx2 = self.size.w * 0.5
self.cy2 = self.size.h * 0.5 - scale * 3.5
time.sleep(0.1)
#motion
ax, ay, az = motion.get_user_acceleration()
gx, gy, gz = motion.get_gravity()
gravity_vectors = motion.get_attitude()
mx, my, mz, ma = motion.get_magnetic_field()
pitch, roll, yaw = [x for x in gravity_vectors]
pitch = -pitch * 180 / math.pi
roll = roll * 180 / math.pi
yaw = -yaw * 180 / math.pi
#redraw screen
background(1, 1, 1)
fill(1, 1, 1)
stroke_weight(1)
#pitch,roll,yaw描画
# ellipse(self.cx2-scale*3-self.R,self.cy2-self.R,self.R*2,self.R*2)
# ellipse(self.cx2-0.0-self.R,self.cy2-self.R,self.R*2,self.R*2)
ellipse(self.cx2 + scale * 3 - self.R, self.cy2 - self.R - 130,
self.R * 2, self.R * 2)
roll_sin = math.sin(math.radians(roll))
roll_cos = math.cos(math.radians(roll))
pitch_sin = math.sin(math.radians(pitch))
pitch_cos = math.cos(math.radians(pitch))
yaw_sin = math.sin(math.radians(yaw))
yaw_cos = math.cos(math.radians(yaw))
# line(self.cx2-roll_cos*self.R-scale*3,self.cy2-roll_sin*self.R,self.cx2+roll_cos*self.R-scale*3,self.cy2+roll_sin*self.R)
# line(self.cx2-pitch_cos*self.R-0,self.cy2-pitch_sin*self.R,self.cx2+pitch_cos*self.R-0,self.cy2+pitch_sin*self.R)
line(self.cx2 - yaw_cos * self.R + scale * 3,
self.cy2 - yaw_sin * self.R - 130,
self.cx2 + yaw_cos * self.R + scale * 3,
self.cy2 + yaw_sin * self.R - 130)
yawMatrix = np.matrix([[yaw_cos, -yaw_sin, 0], [yaw_sin, yaw_cos, 0],
[0, 0, 1]])
pitchMatrix = np.matrix([[pitch_cos, 0, pitch_sin], [0, 1, 0],
[-pitch_sin, 0, pitch_cos]])
rollMatrix = np.matrix([[1, 0, 0], [0, roll_cos, -roll_sin],
[0, roll_sin, roll_cos]])
R = yawMatrix * pitchMatrix * rollMatrix
R = np.array(R)
x_3d, y_3d, z_3d = np.transpose(np.dot(self.Box, R), (2, 0, 1))
zmin = np.argmin(z_3d)
#derive the direction (NSEW)
def directionText(yaw):
directionSym = ''
#Direction
if yaw > 60 and yaw <= 120:
directionSym = 'N'
elif yaw > 120 and yaw <= 150:
directionSym = 'NE'
elif (yaw > 150 and yaw <= 180) or (yaw >= -180 and yaw <= -150):
directionSym = 'E'
elif yaw > -150 and yaw <= -120:
directionSym = 'SE'
elif yaw > -120 and yaw <= -60:
directionSym = 'S'
elif yaw > -60 and yaw <= -30:
directionSym = 'SW'
elif yaw > -30 and yaw <= 30:
directionSym = 'W'
elif yaw > 30 and yaw <= 60:
directionSym = 'NW'
else:
directionSym = ''
return directionSym
#text
tint(0, 0, 0, 1)
#text('Direction:', font_name='Helvetica', font_size=16.0, x=self.cx2+scale*3, y=self.cy2+self.R-80, alignment=5)
text(directionText(yaw),
font_name='Helvetica',
font_size=10.0,
x=self.cx2 + scale * 3,
y=self.cy2 + self.R - 100,
alignment=5)
def draw(self):
#Box
self.cx = self.size.w * 0.5
self.cy = self.size.h * 0.5
#pitch,roll,yaw
self.cx2 = self.size.w * 0.5
self.cy2 = self.size.h * 0.5 - scale * 3.5
time.sleep(0.1)
self.testCounter += 1
#motion
ax, ay, az = motion.get_user_acceleration()
gx, gy, gz = motion.get_gravity()
gravity_vectors = motion.get_attitude()
mx, my, mz, ma = motion.get_magnetic_field()
pitch, roll, yaw = [x for x in gravity_vectors]
pitch = -pitch * 180 / math.pi
roll = roll * 180 / math.pi
yaw = -yaw * 180 / math.pi
#redraw screen
fill(0.9, 0.9, 0.9)
stroke_weight(0)
#pitch,roll,yaw
ellipse(self.cx2 - scale * 3 - self.R, self.cy2 - self.R - 130,
self.R * 2, self.R * 2)
tint(0.4, 0.4, 0.4, 1)
text('Reset',
font_name='Verdana',
font_size=12.0,
x=self.cx2 - 112,
y=self.cy2 + self.R - 167,
alignment=5)
# ellipse(self.cx2-0.0-self.R,self.cy2-self.R,self.R*2,self.R*2)
stroke(0.4, 0.4, 0.4)
stroke_weight(0)
fill(0.9, 0.9, 0.9)
#compass draw
ellipse(self.cx2 + scale * 3 - self.R, self.cy2 - self.R - 130,
self.R * 2, self.R * 2)
#fill(0.7,0.7,0.7)
stroke_weight(0)
ellipse(self.cx2 + scale * 3 - self.R + 3, self.cy2 - self.R - 127,
self.R * 1.85, self.R * 1.85)
#fill(0.8,0.8,0.8)
ellipse(self.cx2 + scale * 3 - self.R + 6, self.cy2 - self.R - 124,
self.R * 1.7, self.R * 1.7)
# roll_sin = math.sin(math.radians(roll))
# roll_cos = math.cos(math.radians(roll))
# pitch_sin = math.sin(math.radians(pitch))
# pitch_cos = math.cos(math.radians(pitch))
yaw_sin = math.sin(math.radians(yaw))
yaw_cos = math.cos(math.radians(yaw))
# line(self.cx2-roll_cos*self.R-scale*3,self.cy2-roll_sin*self.R,self.cx2+roll_cos*self.R-scale*3,self.cy2+roll_sin*self.R)
# line(self.cx2-pitch_cos*self.R-0,self.cy2-pitch_sin*self.R,self.cx2+pitch_cos*self.R-0,self.cy2+pitch_sin*self.R)
stroke(0.4, 0.4, 0.4)
stroke_weight(2)
line(self.cx2 - yaw_cos * self.R + scale * 3,
self.cy2 - yaw_sin * self.R - 130,
self.cx2 + yaw_cos * self.R + scale * 3,
self.cy2 + yaw_sin * self.R - 130)
stroke(1, 0.3, 0.3)
line(300, 72.5, self.cx2 + yaw_cos * self.R + scale * 3,
self.cy2 + yaw_sin * self.R - 130)
#circle on top of compass
stroke_weight(0)
stroke(0.4, 0.4, 0.4)
#fill(0.95,0.95,0.95)
fill(1, 1, 1)
ellipse(self.cx2 + scale * 3 - self.R + 9.5, self.cy2 - self.R - 120.5,
self.R * 1.5, self.R * 1.5)
# yawMatrix = numpy.matrix([[yaw_cos, -yaw_sin, 0],[yaw_sin, yaw_cos, 0],[0, 0, 1]])
# pitchMatrix = numpy.matrix([[pitch_cos, 0, pitch_sin],[0, 1, 0],[-pitch_sin, 0, pitch_cos]])
# rollMatrix = numpy.matrix([[1, 0, 0],[0, roll_cos, -roll_sin],[0, roll_sin, roll_cos]])
#
# R = yawMatrix * pitchMatrix * rollMatrix
# R = numpy.array(R)
#x_3d,y_3d,z_3d = numpy.transpose(numpy.dot(self.Box,R),(2,0,1))
#zmin = numpy.argmin(z_3d)
#derive the direction (NSEW)
def directionText(yaw):
directionSym = ''
#Direction
if yaw > 60 and yaw <= 120:
directionSym = 'N'
elif yaw > 120 and yaw <= 150:
directionSym = 'NE'
elif (yaw > 150 and yaw <= 180) or (yaw >= -180 and yaw <= -150):
directionSym = 'E'
elif yaw > -150 and yaw <= -120:
directionSym = 'SE'
elif yaw > -120 and yaw <= -60:
directionSym = 'S'
elif yaw > -60 and yaw <= -30:
directionSym = 'SW'
elif yaw > -30 and yaw <= 30:
directionSym = 'W'
elif yaw > 30 and yaw <= 60:
directionSym = 'NW'
else:
directionSym = ''
return directionSym
#text
tint(0, 0, 0, 1)
text('Thread 1.0',
font_name='Courier',
font_size=16.0,
x=self.cx2,
y=self.cy2 + self.R + 400,
alignment=5)
tint(0.4, 0.4, 0.4, 1)
if self.measuringOn == False and self.checkedOnce == 0:
text('Tap to Start',
font_name='Verdana',
font_size=16.0,
x=self.cx2,
y=self.cy2 + self.R + 150,
alignment=5)
elif self.measuringOn == True and self.checkedOnce == 1:
if self.testCounter // 10 % 3 == 0:
text('Measuring.',
font_name='Verdana',
font_size=16.0,
x=self.cx2,
y=self.cy2 + self.R + 150,
alignment=5)
elif self.testCounter // 10 % 3 == 1:
text('Measuring..',
font_name='Verdana',
font_size=16.0,
x=self.cx2,
y=self.cy2 + self.R + 150,
alignment=5)
elif self.testCounter // 10 % 3 == 2:
text('Measuring...',
font_name='Verdana',
font_size=16.0,
x=self.cx2,
y=self.cy2 + self.R + 150,
alignment=5)
text('Tap to Stop',
font_name='Verdana',
font_size=16.0,
x=self.cx2,
y=self.cy2 + self.R + 120,
alignment=5)
elif self.measuringOn == False and self.checkedOnce == 2:
text('Calculating',
font_name='Verdana',
font_size=16.0,
x=self.cx2,
y=self.cy2 + self.R + 120,
alignment=5)
#compass text
if self.compassStat == False:
text(directionText(yaw),
font_name='Verdana',
font_size=10.0,
x=self.cx2 + scale * 3,
y=self.cy2 + self.R - 167,
alignment=5)
else:
tempYaw = yaw + 180 + 90
if tempYaw > 360:
tempYaw = tempYaw % 360
yawString = str(round(tempYaw, 2)) + chr(186)
text(yawString,
font_name='Verdana',
font_size=10.0,
x=self.cx2 + scale * 3,
y=self.cy2 + self.R - 167,
alignment=5)
if self.measuringOn == True:
if self.testCounter % 10:
current = location.get_location()
latLong = (current['latitude'], current['longitude'])
self.locations += [latLong]
elif self.checkedOnce == 2 and len(self.locations) > 2:
self.locationsReversed = copy.deepcopy(self.locations)
self.locationsReversed.reverse()
#now use the locations and map it onto a map so you know the direciton between points (NWSE)
#start saying stuff like "Point North and Move Forward"
#Now it makes sense why we use the compass.
#test texts
self.checkedOnce += 1
elif self.checkedOnce == 3:
locationList = str(self.locations[0]) + ' ' + str(
self.locations[-1])
text(locationList,
font_name='Verdana',
font_size=5.0,
x=self.cx2,
y=self.cy2 + self.R + 80,
alignment=5)
locationListReversed = str(self.locationsReversed[0]) + ' ' + str(
self.locationsReversed[-1])
text(locationListReversed,
font_name='Verdana',
font_size=5.0,
x=self.cx2,
y=self.cy2 + self.R + 60,
alignment=5)
elif self.checkedOnce == 4:
self.checkedOnce = 5
x, y = self.locations[0]
webbrowser.open(self.query % (x, y))
text(str(x),
font_name='Verdana',
font_size=5.0,
x=self.cx2,
y=self.cy2 + self.R + 80,
alignment=5)
text(str(y),
font_name='Verdana',
font_size=5.0,
x=self.cx2,
y=self.cy2 + self.R + 80,
alignment=5)
def MotionSource(name, q):
motion.start_updates()
while True:
q.put(motion.get_user_acceleration())
time.sleep(0.05)
if config['save_config_file']:
save_config(config)
print("Config file (%s) saved.\n" % (config['config_file_name'], ))
r = BasicAuthRequest(config)
location.start_updates()
motion.start_updates()
try:
while True:
x, y, z = motion.get_gravity()
gravity_data = {'x': x, 'y': y, 'z': z}
x, y, z = motion.get_user_acceleration()
acceleration_data = {'x': x, 'y': y, 'z': z}
roll, pitch, yaw = motion.get_attitude()
attitude_data = {'roll': roll, 'pitch': pitch, 'yaw': yaw}
x, y, z, accuracy = motion.get_magnetic_field()
magnetic_data = {'x': x, 'y': y, 'z': z, 'accuracy': accuracy}
location_data = location.get_location()
r.post('ios_sensor_pack/gravity/', gravity_data)
r.post('ios_sensor_pack/user_acceleration/', acceleration_data)
r.post('ios_sensor_pack/attitude/', attitude_data)
r.post('ios_sensor_pack/magnetic_field/', magnetic_data)
r.post('ios_sensor_pack/location/', location_data)
print('Sample rate: '+str(test_freq)+'Hz')
pause_time = 3.0 # Pause time before data capture
print('Pause time: '+str(pause_time))
file_name = 'test.npy'
num_points = test_time * test_freq # Number of points in anlysis
print('Creating numpy array of zeros...')
data = np.zeros((num_points, 3)) # Create Numpy array of zeros
print('Create time vector...')
time_vector = np.linspace(0.0, test_time, num=num_points).reshape((num_points,1))
print('Pause before start...')
time.sleep(pause_time) # Insert pause before capture starts
print('Starting data capture...')
n = 0
motion.start_updates()
while n < num_points:
data[n] = motion.get_user_acceleration()
n = n + 1
time.sleep(1.0/test_freq)
motion.stop_updates()
print('Adding time vector to array...')
data = np.concatenate((data, time_vector), axis=1)
print('Saving '+str(num_points)+' points to '+str(file_name))
np.save(file_name, data)
print('-------------------')
def draw(self):
#Boxの中心
self.cx = self.size.w * 0.5
self.cy = self.size.h * 0.5
#pitch,roll,yawメータの中心
self.cx2 = self.size.w * 0.5
self.cy2 = self.size.h * 0.5 - scale * 3.5
time.sleep(0.1)
#motionセンサの値更新
ax, ay, az = motion.get_user_acceleration()
gx, gy, gz = motion.get_gravity()
gravity_vectors = motion.get_attitude()
mx, my, mz, ma = motion.get_magnetic_field()
pitch, roll, yaw = [x for x in gravity_vectors]
#ラジアン→度
pitch = -pitch * 180 / 3.1415926
roll = roll * 180 / 3.1415926
yaw = -yaw * 180 / 3.1415926
#redraw screen
background(1, 1, 1)
fill(1, 1, 1)
stroke(0, 0, 0)
stroke_weight(1)
roll_sin = math.sin(math.radians(roll))
roll_cos = math.cos(math.radians(roll))
pitch_sin = math.sin(math.radians(pitch))
pitch_cos = math.cos(math.radians(pitch))
yaw_sin = -math.sin(math.radians(yaw))
yaw_cos = -math.cos(math.radians(yaw))
yawMatrix = np.matrix([[-yaw_cos, yaw_sin, 0], [yaw_sin, yaw_cos, 0],
[0, 0, 1]])
pitchMatrix = np.matrix([[pitch_cos, 0, pitch_sin], [0, 1, 0],
[-pitch_sin, 0, pitch_cos]])
rollMatrix = np.matrix([[1, 0, 0], [0, roll_cos, -roll_sin],
[0, roll_sin, roll_cos]])
R = yawMatrix * pitchMatrix * rollMatrix
R = np.array(R)
x_3d, y_3d, z_3d = np.transpose(np.dot(self.Box, R), (2, 0, 1))
#陰線処理のため1番奥の頂点を特定
zmin = np.argmin(z_3d)
#奥の頂点を含んでいない辺を描画
if zmin != 0 and zmin != 1:
line(self.cx + x_3d[0] * scale, self.cy + y_3d[0] * scale,
self.cx + x_3d[1] * scale, self.cy + y_3d[1] * scale)
if zmin != 1 and zmin != 2:
line(self.cx + x_3d[1] * scale, self.cy + y_3d[1] * scale,
self.cx + x_3d[2] * scale, self.cy + y_3d[2] * scale)
if zmin != 2 and zmin != 3:
line(self.cx + x_3d[2] * scale, self.cy + y_3d[2] * scale,
self.cx + x_3d[3] * scale, self.cy + y_3d[3] * scale)
if zmin != 3 and zmin != 0:
line(self.cx + x_3d[3] * scale, self.cy + y_3d[3] * scale,
self.cx + x_3d[0] * scale, self.cy + y_3d[0] * scale)
if zmin != 4 and zmin != 5:
line(self.cx + x_3d[4] * scale, self.cy + y_3d[0] * scale,
self.cx + x_3d[1] * scale, self.cy + y_3d[1] * scale)
if zmin != 5 and zmin != 6:
line(self.cx + x_3d[5] * scale, self.cy + y_3d[1] * scale,
self.cx + x_3d[2] * scale, self.cy + y_3d[2] * scale)
if zmin != 6 and zmin != 7:
line(self.cx + x_3d[6] * scale, self.cy + y_3d[2] * scale,
self.cx + x_3d[3] * scale, self.cy + y_3d[3] * scale)
if zmin != 7 and zmin != 4:
line(self.cx + x_3d[7] * scale, self.cy + y_3d[3] * scale,
self.cx + x_3d[0] * scale, self.cy + y_3d[0] * scale)
def update(self):
if not self.isInit:
print("Waiting for init")
return
GRAV = motion.get_gravity()
#GRAV=numpy.add(motion.get_gravity(), motion.get_user_acceleration())
GRAV = GRAV / numpy.linalg.norm(GRAV)
FORCE = motion.get_user_acceleration()
ATT = motion.get_attitude()
LOC = location.get_location()
MAG = motion.get_magnetic_field()
[roll, pitch, yaw,
hdg] = navutil.roll_pitch_yaw_hdg(ATT, MAG, GRAV, FORCE)
# Pitch and roll pointer
self.pitchMarks.rotation = roll
offsetY = self.pxPerDeg * (numpy.rad2deg(pitch))
#offsetX=-numpy.tan(roll)*offsetY
offsetX = -numpy.sin(roll) * offsetY
self.pitchMarks.position = (offsetX, offsetY * numpy.cos(roll))
self.rollPointer.rotation = roll
# Yaw pointer (ball) ...
# smooth the signal a little
self.yaw_history.append(yaw)
if len(self.yaw_history) > 5:
self.yaw_history.pop(0)
self.yawPointer.rotation = roll - numpy.mean(self.yaw_history)
#print("%d %d %d"%(numpy.rad2deg(ATT[0]), numpy.rad2deg(ATT[1]),numpy.rad2deg(ATT[2])))
if LOC != None:
self.lbl_lat.text = "LAT %2.6f deg" % LOC["latitude"]
self.lbl_lon.text = "LON %2.6f deg" % LOC["longitude"]
# Altitude display...
# hundreds
self.txt_alth.text = "%01d" % ((LOC["altitude"] / 0.3048) / 100.0)
# decis
self.txt_altd.text = "%02d" % ((LOC["altitude"] / 0.3048) % 100.0)
# Ground speed display
gs = LOC["speed"] * 3.6 / 1.852
if gs < 0.0:
self.txt_spd.text = "--"
else:
self.txt_spd.text = "%d" % gs
self.pos_geo = [
LOC["altitude"],
numpy.deg2rad(LOC["longitude"]),
numpy.deg2rad(LOC["latitude"])
]
self.pos_cart = navutil.geo2cart(self.pos_geo)
# HSI...
self.txt_g.text = "%2.1f" % (numpy.linalg.norm(GRAV + FORCE))
# HDG display
if self.hdg_mode == False:
hdg = LOC["course"]
self.txt_hdg.color = "#ff00ff"
else:
self.txt_hdg.color = "#00ff00"
if hdg < 0.0:
self.txt_hdg.text = "TRK"
self.txt_hdg.color = "#ff0000"
hdg = 0.0
else:
self.txt_hdg.text = "%03d" % (hdg)
self.rose.rotation = numpy.deg2rad(hdg)
# Waypoint info, CDI
if self.wpt != "":
[dist, dtk] = navutil.great_circle(self.pos_cart, self.wpt_cart)
fmt_dist = "%d"
if (dist / 1852.0 < 10.0):
fmt_dist = "%0.1f"
self.wpt_dist.text = fmt_dist % (dist / 1852.0)
self.wpt_dtk.text = "%03d" % numpy.rad2deg(dtk)
self.cdi_needle.rotation = -self.dtk
full_scale_defl = 12.0
defl = numpy.amax([
-full_scale_defl,
numpy.amin([full_scale_defl,
numpy.rad2deg((dtk - self.dtk))])
])
self.cdi_track.position = numpy.array(
[0.5 / full_scale_defl * self.cdi_r * defl, 0.0, 0.0])
def acceleration(self):
return {
key: val
for key, val in zip(('x', 'y',
'z'), motion.get_user_acceleration())
}
if config['save_config_file']:
save_config(config)
print("Config file (%s) saved.\n" % (config['config_file_name'], ))
r = BasicAuthRequest(config)
location.start_updates()
motion.start_updates()
try:
while True:
gravity_x, gravity_y, gravity_z = motion.get_gravity()
# gravity_data = {'x': gravity_x, 'y': gravity_y, 'z': gravity_z}
acceleration_x, acceleration_y, acceleration_z = motion.get_user_acceleration(
)
# acceleration_data = {'x': acceleration_x, 'y': acceleration_y, 'z': acceleration_z}
attitude_roll, attitude_pitch, attitude_yaw = motion.get_attitude()
# attitude_data = {'roll': attitude_roll, 'pitch': attitude_pitch, 'yaw': attitude_yaw}
magnetic_x, magnetic_y, magnetic_z, magnetic_accuracy = motion.get_magnetic_field(
)
# magnetic_data = {'x': magnetic_x, 'y': magnetic_y, 'z': magnetic_z, 'accuracy': magnetic_accuracy}
location_data = location.get_location()
ios_sensor_data = {
'gravity_x': gravity_x,
'gravity_y': gravity_y,
'gravity_z': gravity_z,
def read_data(sender):
import motion, location
import time, datetime
import io
import numpy as np
import matplotlib.pyplot as plt
val = view['switch1'].value
if val==True:
motion.start_updates()
y=0
nx = np.empty(1)
ny = np.empty(1)
nz = np.empty(1)
view['mag'].text = ''
view['accel'].text = ''
view['gyro'].text = ''
view['gravity'].text = ''
while (y<=100):
time.sleep(.05)
x = motion.get_attitude()
view['gyro'].text = str(x) + '\n' + view['gyro'].text
x = motion.get_gravity()
view['gravity'].text = str(x) + '\n' + view['gravity'].text
x = motion.get_user_acceleration()
nx = np.append(nx,x[0])
ny = np.append(ny,x[1])
nz = np.append(nz,x[2])
view['accel'].text = str(x) + '\n' + view['accel'].text
x = motion.get_magnetic_field()
view['mag'].text = str(x) + '\n' + view['mag'].text
y +=1
view['y'].text = str(y) + 'measurements'
motion.stop_updates()
plt.plot(nx)
plt.show()
plt.savefig('x.tif')
plt.close()
plt.plot(ny)
plt.show()
plt.savefig('y.tif')
plt.close()
plt.plot(nz)
plt.show()
plt.savefig('z.tif')
plt.close()
medianx = np.median(nz)
stdx = np.std(nz)
apex = np.amax(np.absolute(nz))
print (apex)
print (stdx)
if apex >= stdx*2:
if apex > stdx*5:
view['fell'].text = 'Fall'
else:
view['fell'].text = 'Trip'
fname = 'motion' + str(datetime.datetime.now()).split('.')[1] + '.txt'
with open(fname, 'w') as fo:
fo.write('gyro\n')
fo.write(view['gyro'].text)
fo.write('gravity\n')
fo.write(view['gravity'].text)
fo.write('accel\n')
fo.write(view['accel'].text)
fo.write('mag\n')
fo.write(view['mag'].text)
else:
view['mag'].text = ''
view['accel'].text = ''
view['gyro'].text = ''
view['gravity'].text = ''
view['y'].text = str(0)
def draw(self):
#Box
self.cx = self.size.w * 0.5
self.cy = self.size.h * 0.5
#pitch,roll,yaw
self.cx2 = self.size.w * 0.5
self.cy2 = self.size.h * 0.5 - scale * 3.5
time.sleep(0.1)
#motion
ax, ay, az = motion.get_user_acceleration()
gx, gy, gz = motion.get_gravity()
gravity_vectors = motion.get_attitude()
mx, my, mz, ma = motion.get_magnetic_field()
pitch, roll, yaw = [x for x in gravity_vectors]
pitch = -pitch * 180 / math.pi
roll = roll * 180 / math.pi
yaw = -yaw * 180 / math.pi
#redraw screen
fill(0.5, 0.5, 0.5)
stroke_weight(2)
#pitch,roll,yaw
# ellipse(self.cx2-scale*3-self.R,self.cy2-self.R,self.R*2,self.R*2)
# ellipse(self.cx2-0.0-self.R,self.cy2-self.R,self.R*2,self.R*2)
ellipse(self.cx2 + scale * 3 - self.R, self.cy2 - self.R - 130,
self.R * 2, self.R * 2)
fill(0.7, 0.7, 0.7)
no_stroke()
ellipse(self.cx2 + scale * 3 - self.R + 3, self.cy2 - self.R - 127,
self.R * 1.85, self.R * 1.85)
fill(0.8, 0.8, 0.8)
ellipse(self.cx2 + scale * 3 - self.R + 6, self.cy2 - self.R - 124,
self.R * 1.7, self.R * 1.7)
fill(0.9, 0.9, 0.9)
ellipse(self.cx2 + scale * 3 - self.R + 9.5, self.cy2 - self.R - 120.5,
self.R * 1.5, self.R * 1.5)
roll_sin = math.sin(math.radians(roll))
roll_cos = math.cos(math.radians(roll))
pitch_sin = math.sin(math.radians(pitch))
pitch_cos = math.cos(math.radians(pitch))
yaw_sin = math.sin(math.radians(yaw))
yaw_cos = math.cos(math.radians(yaw))
# line(self.cx2-roll_cos*self.R-scale*3,self.cy2-roll_sin*self.R,self.cx2+roll_cos*self.R-scale*3,self.cy2+roll_sin*self.R)
# line(self.cx2-pitch_cos*self.R-0,self.cy2-pitch_sin*self.R,self.cx2+pitch_cos*self.R-0,self.cy2+pitch_sin*self.R)
stroke(0, 0, 0)
stroke_weight(2)
line(self.cx2 - yaw_cos * self.R + scale * 3,
self.cy2 - yaw_sin * self.R - 130,
self.cx2 + yaw_cos * self.R + scale * 3,
self.cy2 + yaw_sin * self.R - 130)
stroke(1, 0, 0)
line(300, 72.5, self.cx2 + yaw_cos * self.R + scale * 3,
self.cy2 + yaw_sin * self.R - 130)
yawMatrix = np.matrix([[yaw_cos, -yaw_sin, 0], [yaw_sin, yaw_cos, 0],
[0, 0, 1]])
pitchMatrix = np.matrix([[pitch_cos, 0, pitch_sin], [0, 1, 0],
[-pitch_sin, 0, pitch_cos]])
rollMatrix = np.matrix([[1, 0, 0], [0, roll_cos, -roll_sin],
[0, roll_sin, roll_cos]])
R = yawMatrix * pitchMatrix * rollMatrix
R = np.array(R)
#x_3d,y_3d,z_3d = np.transpose(np.dot(self.Box,R),(2,0,1))
#zmin = np.argmin(z_3d)
#derive the direction (NSEW)
def directionText(yaw):
directionSym = ''
#Direction
if yaw > 60 and yaw <= 120:
directionSym = 'N'
elif yaw > 120 and yaw <= 150:
directionSym = 'NE'
elif (yaw > 150 and yaw <= 180) or (yaw >= -180 and yaw <= -150):
directionSym = 'E'
elif yaw > -150 and yaw <= -120:
directionSym = 'SE'
elif yaw > -120 and yaw <= -60:
directionSym = 'S'
elif yaw > -60 and yaw <= -30:
directionSym = 'SW'
elif yaw > -30 and yaw <= 30:
directionSym = 'W'
elif yaw > 30 and yaw <= 60:
directionSym = 'NW'
else:
directionSym = ''
return directionSym
#text
tint(0, 0, 0, 1)
text('Thread 1.0',
font_name='Courier',
font_size=16.0,
x=self.cx2,
y=self.cy2 + self.R + 400,
alignment=5)
if self.measuringOn == False and self.checkedOnce == 0:
text('Tap to Start',
font_name='Helvetica',
font_size=16.0,
x=self.cx2,
y=self.cy2 + self.R + 150,
alignment=5)
elif self.measuringOn == True and self.checkedOnce == 1:
if self.testCounter % 3 == 0:
text('Measuring.',
font_name='Helvetica',
font_size=16.0,
x=self.cx2,
y=self.cy2 + self.R + 150,
alignment=5)
elif self.testCounter % 3 == 1:
text('Measuring..',
font_name='Helvetica',
font_size=16.0,
x=self.cx2,
y=self.cy2 + self.R + 150,
alignment=5)
elif self.testCounter % 3 == 2:
text('Measuring...',
font_name='Helvetica',
font_size=16.0,
x=self.cx2,
y=self.cy2 + self.R + 150,
alignment=5)
text('Tap to Stop',
font_name='Helvetica',
font_size=16.0,
x=self.cx2,
y=self.cy2 + self.R + 120,
alignment=5)
elif self.measuringOn == False and self.checkedOnce == 2:
text('Calculating',
font_name='Helvetica',
font_size=16.0,
x=self.cx2,
y=self.cy2 + self.R + 120,
alignment=5)
#compass text
text(directionText(yaw),
font_name='Helvetica',
font_size=10.0,
x=self.cx2 + scale * 3,
y=self.cy2 + self.R - 100,
alignment=5)
if self.measuringOn == True:
time.sleep(0.2)
current = location.get_location()
latLong = (current['latitude'], current['longitude'])
self.locations += [latLong]
self.testCounter += 1
#sound.play_effect('arcade:Laser_2')
elif self.checkedOnce == 2:
self.locationsReversed = copy.deepcopy(self.locations)
self.locationsReversed.reverse()
#now use the locations and map it onto a map so you know the direciton between points (NWSE)
#start saying stuff like "Point North and Move Forward"
#Now it makes sense why we use the compass.
#test texts
self.checkedOnce += 1
elif self.checkedOnce == 3 or self.checkedOnce == 4:
locationList = str(self.locations[0]) + ' ' + str(
self.locations[-1])
text(locationList,
font_name='Helvetica',
font_size=5.0,
x=self.cx2,
y=self.cy2 + self.R + 80,
alignment=5)
locationListReversed = str(self.locationsReversed[0]) + ' ' + str(
self.locationsReversed[-1])
text(locationListReversed,
font_name='Helvetica',
font_size=5.0,
x=self.cx2,
y=self.cy2 + self.R + 60,
alignment=5)
def __collect(self):
uacc = np.zeros(4)
uacc[1:] = motion.get_user_acceleration()
qp = quat_from_euler(*motion.get_attitude())
q = quat_conj(qp)
return quat_product(quat_product(q,3.28*9.81*uacc),qp)[1:]
