| Line 7... |
Line 7... |
| 7 |
#logging.basicConfig(level=logging.DEBUG) |
7 |
#logging.basicConfig(level=logging.DEBUG) |
| 8 |
|
8 |
|
| 9 |
import time |
9 |
import time |
| 10 |
import datetime |
10 |
import datetime |
| 11 |
import sys |
11 |
import sys |
| - |
|
12 |
import numpy as np |
| 12 |
from pymlab import config |
13 |
from pymlab import config |
| 13 |
|
14 |
|
| 14 |
#### Script Arguments ############################################### |
15 |
#### Script Arguments ############################################### |
| 15 |
|
16 |
|
| 16 |
if len(sys.argv) != 2: |
17 |
if len(sys.argv) != 2: |
| Line 62... |
Line 63... |
| 62 |
print sensor.get_zero_position() |
63 |
print sensor.get_zero_position() |
| 63 |
|
64 |
|
| 64 |
#### Data Logging ################################################### |
65 |
#### Data Logging ################################################### |
| 65 |
|
66 |
|
| 66 |
try: |
67 |
try: |
| - |
|
68 |
angles = np.zeros(5) |
| - |
|
69 |
angles[4] = sensor.get_angle(verify = False) |
| - |
|
70 |
time.sleep(0.1) |
| - |
|
71 |
angles[3] = sensor.get_angle(verify = False) |
| - |
|
72 |
time.sleep(0.1) |
| - |
|
73 |
angles[2] = sensor.get_angle(verify = False) |
| - |
|
74 |
time.sleep(0.1) |
| - |
|
75 |
angles[1] = sensor.get_angle(verify = False) |
| - |
|
76 |
n = 0 |
| - |
|
77 |
|
| 67 |
while True: |
78 |
while True: |
| 68 |
# for i in range(10): |
- |
|
| 69 |
angle1 = sensor.get_angle(verify = False) |
- |
|
| 70 |
time.sleep(0.1) |
- |
|
| 71 |
angle2 = sensor.get_angle(verify = False) |
- |
|
| 72 |
time.sleep(0.1) |
79 |
time.sleep(0.1) |
| 73 |
angle3 = sensor.get_angle(verify = False) |
80 |
angles[0] = sensor.get_angle(verify = False) |
| 74 |
|
81 |
|
| - |
|
82 |
if (angles[0] + n*360 - angles[1]) > 300: |
| - |
|
83 |
n -= 1 |
| 75 |
if (angle1 < angle2): |
84 |
angles[0] = angles[0] + n*360 |
| - |
|
85 |
elif -(angles[0] - n*360 - angles[1]) > 300: # compute angular speed in backward direction. |
| - |
|
86 |
n += 1 |
| 76 |
speed = (angle2 - angle1)/0.01 |
87 |
angles[0] = angles[0] - n*360 |
| 77 |
else: |
88 |
else: |
| 78 |
speed = (360 - angle1 + angle2)/0.01 |
89 |
angles[0] = angles[0] + n*360 |
| 79 |
|
- |
|
| 80 |
|
- |
|
| 81 |
|
90 |
|
| 82 |
sys.stdout.write("Speed: " + str(speed) +"\t"+ str(angle1) +"\t"+ str(angle2) + "\t\tMagnitude: " + str(sensor.get_magnitude()) |
91 |
speed = (-angles[4] + 8*angles[3] - 8*angles[1] + angles[0])/12 |
| - |
|
92 |
angles = np.roll(angles, 1) |
| - |
|
93 |
|
| 83 |
+ "\tAGC Value: " + str(sensor.get_agc_value()) + "\tDiagnostics: " + str(sensor.get_diagnostics()) + "\r\n") |
94 |
sys.stdout.write("Speed: " + str(speed) +"\t"+ str(angles[0]) + "\r\n") |
| 84 |
sys.stdout.flush() |
95 |
sys.stdout.flush() |
| 85 |
time.sleep(0.01) |
- |
|
| 86 |
except KeyboardInterrupt: |
96 |
except KeyboardInterrupt: |
| 87 |
sys.exit(0) |
97 |
sys.exit(0) |