SHOW:
|
|
- or go back to the newest paste.
| 1 | """he MIT License (MIT) | |
| 2 | ||
| 3 | Copyright (c) 2014 Andrew Luo luo_andrew a|t yahoo.com | |
| 4 | ||
| 5 | Permission is hereby granted, free of charge, to any person obtaining a copy | |
| 6 | of this software and associated documentation files (the "Software"), to deal | |
| 7 | in the Software without restriction, including without limitation the rights | |
| 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | |
| 9 | copies of the Software, and to permit persons to whom the Software is | |
| 10 | furnished to do so, subject to the following conditions: | |
| 11 | - | print "\nStarting motion update, please wait 20 seconds for the values to stabilize. During this time, please avoid touching the phone" |
| 11 | + | |
| 12 | The above copyright notice and this permission notice shall be included in | |
| 13 | all copies or substantial portions of the Software. | |
| 14 | ||
| 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | |
| 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | |
| 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | |
| 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | |
| 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | |
| 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | |
| 21 | THE SOFTWARE.""" | |
| 22 | ||
| 23 | def navigate(): | |
| 24 | import _motion as motion | |
| 25 | from numpy import zeros, divide | |
| 26 | from math import cos | |
| 27 | from operator import add, sub | |
| 28 | from console import clear | |
| 29 | m = motion.MotionManager() | |
| 30 | m.stop() | |
| 31 | m.start() | |
| 32 | import time | |
| 33 | print "\nStarting motion update, please wait 10 seconds for the values to stabilize. During this time, please avoid touching the phone" | |
| 34 | """First we start updating, we allow a 5 second time out because there is a significant amount of drift initially present""" | |
| 35 | time.sleep(10) | |
| 36 | print "\nStarting calibration in 10 seconds. Please keep the device UTTERLY still in an position that you expect it to be most commonly in during the measurements later." | |
| 37 | time.sleep(6) | |
| 38 | print "\nCalibration starting in 5 seconds." | |
| 39 | time.sleep(5) | |
| 40 | print "\nCalibration starting..." | |
| 41 | caliacc = zeros(3) | |
| 42 | caliangle = zeros(3) | |
| 43 | cali=1 | |
| 44 | for u in xrange(300): | |
| 45 | - | time1=0.083*9.83 |
| 45 | + | |
| 46 | - | time2=0.083 |
| 46 | + | |
| 47 | zeroingacc= divide(caliacc, 300) | |
| 48 | - | for j in xrange(500): |
| 48 | + | |
| 49 | print zeroingacc | |
| 50 | time.sleep(10) | |
| 51 | print "\nCalibration ended, you may start to move." | |
| 52 | zeroingacc0=zeroingacc[0] | |
| 53 | zeroingacc1=zeroingacc[1] | |
| 54 | zeroingacc2=zeroingacc[2] | |
| 55 | zeroingang0=zeroingang[0] | |
| 56 | zeroingang1=zeroingang[1] | |
| 57 | zeroingang2=zeroingang[2] | |
| 58 | speed0=0 | |
| 59 | speed1=0 | |
| 60 | speed2=0 | |
| 61 | angle0=0 | |
| 62 | angle1=0 | |
| 63 | angle2=0 | |
| 64 | movement0=0 | |
| 65 | movement1=0 | |
| 66 | movement2=0 | |
| 67 | time2=0.083 #You need to change this number to make it work on your device, so just replace the while loop, and timeit | |
| 68 | time1=time2*9.83 | |
| 69 | """Apparently for small lists, direct assignment is twice as fast as map and lists! Very unexpected!""" | |
| 70 | while 1: | |
| 71 | speed0 += (-zeroingacc0 + m.user_acceleration[0]) * time1 | |
| 72 | speed1 += (-zeroingacc1 + m.user_acceleration[1]) * time1 | |
| 73 | speed2 += (-zeroingacc2 + m.user_acceleration[2]) * time1 | |
| 74 | angle0 += (m.rotation_rate[0]-zeroingang0) * time2 | |
| 75 | angle1 += (m.rotation_rate[1]-zeroingang1)* time2 | |
| 76 | angle2 += (m.rotation_rate[2]-zeroingang2)* time2 | |
| 77 | cos0=cos(angle0) | |
| 78 | cos1=cos(angle1) | |
| 79 | cos2=cos(angle2) | |
| 80 | movement0 = movement0+speed0*time2*cos0*cos2 | |
| 81 | movement1 = movement1+speed1*time2*cos1*cos2 | |
| 82 | movement2 = movement2+speed2*time2*cos0*cos1 | |
| 83 | #speed = map(lambda x, y: time*add(x, y), speed, map(sub, list(m.user_acceleration), zeroingacc)) | |
| 84 | #angle = map(add, angle, list(m.rotation_rate)) | |
| 85 | #movement = map(lambda x,y: time*add(x,y), movement, map(lambda l, o: l*o, map(cos, angle), speed)) | |
| 86 | print movement0, movement1, movement2 | |
| 87 | clear() | |
| 88 | navigate() |