Line 1... |
Line 1... |
1 |
#!/usr/bin/python |
1 |
#!/usr/bin/python |
2 |
|
2 |
|
- |
|
3 |
# plot with >> plot 'last.txt' u 1:2 w l axes x1y1, 'last.txt' u 1:4 w l axes x1y2, 'last.txt' u 1:3 |
3 |
|
4 |
|
- |
|
5 |
import os |
4 |
import time |
6 |
import time |
5 |
import datetime |
7 |
import datetime |
6 |
import sys |
8 |
import sys |
7 |
import numpy as np |
9 |
import numpy as np |
8 |
from gps import * |
10 |
from gps import * |
Line 55... |
Line 57... |
55 |
angles[2] = sensor.get_angle(verify = False) |
57 |
angles[2] = sensor.get_angle(verify = False) |
56 |
time.sleep(0.01) |
58 |
time.sleep(0.01) |
57 |
angles[1] = sensor.get_angle(verify = False) |
59 |
angles[1] = sensor.get_angle(verify = False) |
58 |
n = 0 |
60 |
n = 0 |
59 |
speed = 0 |
61 |
speed = 0 |
60 |
AVERAGING = 100 |
62 |
AVERAGING = 50 |
- |
|
63 |
|
- |
|
64 |
filen = 'log%0.0f.txt'%time.time() |
61 |
f = open('log.txt','w') |
65 |
f = open(filen,'w') |
- |
|
66 |
os.remove("last.txt") |
- |
|
67 |
os.symlink(filen, "last.txt") |
62 |
|
68 |
|
63 |
gpsp = GpsPoller() |
69 |
gpsp = GpsPoller() |
64 |
gpsp.start() |
70 |
gpsp.start() |
65 |
|
71 |
|
66 |
while True: |
72 |
while True: |
67 |
for i in range(AVERAGING): |
73 |
for i in range(AVERAGING): |
68 |
time.sleep(0.005) |
74 |
time.sleep(0.01) |
69 |
angles[0] = sensor.get_angle(verify = False) |
75 |
angles[0] = sensor.get_angle(verify = False) |
70 |
|
76 |
|
71 |
if (angles[0] + n*360 - angles[1]) > 300: |
77 |
if (angles[0] + n*360 - angles[1]) > 300: |
72 |
n -= 1 |
78 |
n -= 1 |
73 |
angles[0] = angles[0] + n*360 |
79 |
angles[0] = angles[0] + n*360 |
Line 79... |
Line 85... |
79 |
else: |
85 |
else: |
80 |
angles[0] = angles[0] + n*360 |
86 |
angles[0] = angles[0] + n*360 |
81 |
|
87 |
|
82 |
speed += (-angles[4] + 8*angles[3] - 8*angles[1] + angles[0])/12 |
88 |
speed += (-angles[4] + 8*angles[3] - 8*angles[1] + angles[0])/12 |
83 |
angles = np.roll(angles, 1) |
89 |
angles = np.roll(angles, 1) |
- |
|
90 |
|
- |
|
91 |
speed = speed/AVERAGING |
84 |
|
92 |
|
85 |
g_spd = gpsd.fix.speed |
93 |
g_spd = gpsd.fix.speed |
86 |
print "W_Spd: %0.2f \t Angle: %0.2f \t G_Spd %0.2f" % (speed, angles[0], g_spd) |
94 |
print "W_Spd: %0.2f \t Angle: %0.2f \t G_Spd %0.2f" % (speed, angles[0], g_spd) |
87 |
f.write("%0.2f %0.2f %0.2f %0.2f\r\n" %(time.time(), speed, angles[0], g_spd)) |
95 |
f.write("%0.2f %0.2f %0.2f %0.2f\r\n" %(time.time(), abs(speed), angles[0], g_spd)) |
88 |
f.flush() |
96 |
f.flush() |
89 |
|
97 |
|
90 |
|
98 |
|
91 |
except KeyboardInterrupt: |
99 |
except KeyboardInterrupt: |
92 |
gpsp.running = False |
100 |
gpsp.running = False |