ios_motion_test.py

ios_motion_test.py

# coding: utf-8

import motion
from time import sleep
from motion import *
import canvas as cv
from math import pi

''' Playing with attitude tracking(roll/pitch/yaw). Changes the color of the circle based on rotation around all 3 axes.'''

## Mechanism for clearing past graphics 
cv.clear()
resp = raw_input('Enter to continue (q to exit). ').strip().lower()
if resp == 'q':
	print('Program complete.')
	exit()
	
## Set to True to turn on min/max testing
MINMAX = False

## Compares the two sets at given index, returns the unit difference (-1==less, 1==more, or 0==same)
def comp(set1, set2, i):
	diff = set1[i] - set2[i]
	return diff/abs(diff)
	
def compset(set1,set2):
	diff = [x1-x2 for x1,x2 in zip(set1,set2)]
	return diff

## Threshold for attitude delta tracking
tr = 0.005

start_updates()
print('roll\tpitch\tyaw')
last_att = [0,0,0]

## Minimum Maximum tracking functions
_min = lambda curr_min,new: new if curr_min is None else min(curr_min,new)

_max = lambda curr_max,new:new if curr_max is None else max(curr_max,new)

att_mins = [None]*3
att_maxs = [None]*3

def new_att_vals(new_att):
	global att_mins, att_maxs
	att_mins = [_min(curr_min,new) for curr_min,new in zip(att_mins, new_att)]
	
	att_maxs = [_max(curr_max,new) for curr_max,new in zip(att_maxs, new_att)]

## Converts 0->pi float to 0->1 float
def unit_pi(num):
	return abs(num)/pi

## Loop variables
interval = 0.02    # Time interval between loops
total_time = 20    # ~Total time for the looping portion to run (based on sleep time, not factoring in processing)
loops = int(total_time/interval)

## Vector graphics
dim = 512
w = h = dim

for count in xrange(loops):
	## att == roll, pitch, yaw
  att = get_attitude()
  diff = compset(att, last_att)
  
  cv.set_fill_color(*[unit_pi(x) for x in att])
  cv.fill_ellipse(0, 0, w, h)
  
  if MINMAX:
	  new_att_vals(att)
	
  '''
  ## Uncomment to track/print yaw deltas
  if diff[2] > tr:
  	print('yaw up!')
  elif diff[2] < -tr:
  	print('yaw down!')
  else:
  	print('Yaw same!')
  last_att = att
  '''
  
  #print('{:.3f}\t{:.3f}\t{:.3f}'.format(*att))
  sleep(interval)
  
stop_updates()
cv.clear()
if MINMAX:
	open('minmax.txt', 'w').write(('min {}\nmax {}'.format(min_yaw,max_yaw)))

Playing around with Python's iOS motion module.