49,18 → 49,6 |
|
|
try: |
angles = np.zeros(5) |
angles[4] = sensor.get_angle(verify = False) |
time.sleep(0.01) |
angles[3] = sensor.get_angle(verify = False) |
time.sleep(0.01) |
angles[2] = sensor.get_angle(verify = False) |
time.sleep(0.01) |
angles[1] = sensor.get_angle(verify = False) |
n = 0 |
speed = 0 |
AVERAGING = 50 |
|
filen = 'log%0.0f.txt'%time.time() |
f = open(filen,'w') |
os.remove("last.txt") |
70,29 → 58,11 |
gpsp.start() |
|
while True: |
for i in range(AVERAGING): |
time.sleep(0.01) |
angles[0] = sensor.get_angle(verify = False) |
w_spd = abs(sensor.get_speed()) |
g_spd = abs(gpsd.fix.speed()) |
|
if (angles[0] + n*360 - angles[1]) > 300: |
n -= 1 |
angles[0] = angles[0] + n*360 |
|
elif (angles[0] + n*360 - angles[1]) < -300: |
n += 1 |
angles[0] = angles[0] + n*360 |
|
else: |
angles[0] = angles[0] + n*360 |
|
speed += (-angles[4] + 8*angles[3] - 8*angles[1] + angles[0])/12 |
angles = np.roll(angles, 1) |
|
speed = speed/AVERAGING |
|
g_spd = gpsd.fix.speed |
print "W_Spd: %0.2f \t Angle: %0.2f \t G_Spd %0.2f" % (speed, angles[0], g_spd) |
f.write("%0.2f %0.2f %0.2f %0.2f\r\n" %(time.time(), abs(speed), angles[0], g_spd)) |
print "W_Spd: %.3f \t G_Spd %.3f \t multiplayer: %.3f \t multiplayer: %.3f" % (w_spd, g_spd, w_spd/g_spd, g_spd/w_spd) |
f.write("%.3f %.3f %.3f\r\n" %(time.time(), w_spd, g_spd)) |
f.flush() |
|
|