Rev 4423 Rev 4451
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)