Line 65... |
Line 65... |
65 |
#### Data Logging ################################################### |
65 |
#### Data Logging ################################################### |
66 |
|
66 |
|
67 |
try: |
67 |
try: |
68 |
angles = np.zeros(5) |
68 |
angles = np.zeros(5) |
69 |
angles[4] = sensor.get_angle(verify = False) |
69 |
angles[4] = sensor.get_angle(verify = False) |
70 |
time.sleep(0.1) |
70 |
time.sleep(0.01) |
71 |
angles[3] = sensor.get_angle(verify = False) |
71 |
angles[3] = sensor.get_angle(verify = False) |
72 |
time.sleep(0.1) |
72 |
time.sleep(0.01) |
73 |
angles[2] = sensor.get_angle(verify = False) |
73 |
angles[2] = sensor.get_angle(verify = False) |
74 |
time.sleep(0.1) |
74 |
time.sleep(0.01) |
75 |
angles[1] = sensor.get_angle(verify = False) |
75 |
angles[1] = sensor.get_angle(verify = False) |
76 |
n = 0 |
76 |
n = 0 |
- |
|
77 |
speed = 0 |
- |
|
78 |
AVERAGING = 50 |
77 |
|
79 |
|
78 |
while True: |
80 |
while True: |
- |
|
81 |
for i in range(AVERAGING): |
79 |
time.sleep(0.1) |
82 |
time.sleep(0.01) |
80 |
angles[0] = sensor.get_angle(verify = False) |
83 |
angles[0] = sensor.get_angle(verify = False) |
81 |
|
84 |
|
82 |
if (angles[0] + n*360 - angles[1]) > 300: |
85 |
if (angles[0] + n*360 - angles[1]) > 300: |
83 |
n -= 1 |
86 |
n -= 1 |
84 |
angles[0] = angles[0] + n*360 |
87 |
angles[0] = angles[0] + n*360 |
- |
|
88 |
|
85 |
elif -(angles[0] - n*360 - angles[1]) > 300: # compute angular speed in backward direction. |
89 |
elif (angles[0] + n*360 - angles[1]) < -300: # compute angular speed in backward direction. |
86 |
n += 1 |
90 |
n += 1 |
87 |
angles[0] = angles[0] - n*360 |
91 |
angles[0] = angles[0] + n*360 |
- |
|
92 |
|
88 |
else: |
93 |
else: |
89 |
angles[0] = angles[0] + n*360 |
94 |
angles[0] = angles[0] + n*360 |
90 |
|
95 |
|
91 |
speed = (-angles[4] + 8*angles[3] - 8*angles[1] + angles[0])/12 |
96 |
speed += (-angles[4] + 8*angles[3] - 8*angles[1] + angles[0])/12 |
92 |
angles = np.roll(angles, 1) |
97 |
angles = np.roll(angles, 1) |
93 |
|
98 |
|
94 |
sys.stdout.write("Speed: " + str(speed) +"\t"+ str(angles[0]) + "\r\n") |
99 |
speed = speed/AVERAGING # apply averaging on acummulated value. |
95 |
sys.stdout.flush() |
100 |
print "Speed: %0.2f \t Total Angle: %0.2f \r\n" % (speed, angles[0]) |
- |
|
101 |
|
96 |
except KeyboardInterrupt: |
102 |
except KeyboardInterrupt: |
97 |
sys.exit(0) |
103 |
sys.exit(0) |