def driveTo(self, relativePosition, gap = 0, speed = 100): bearing = float(Bearing.toPoint(relativePosition)) * MAGIC_TURN_NUMBER dist = abs(relativePosition) - gap print "Turning:", bearing self.rotateBy(bearing) self.stop() time.sleep(0.25) print "Driving:", dist self.driveDistance(dist, speed = speed)
def bearing_to(self, point):
"""Return the bearing to another point"""
delta_long = point.long_radians - self.long_radians
y = sin(delta_long) * cos(point.lat_radians)
x = (
cos(self.lat_radians) * sin(point.lat_radians) -
sin(self.lat_radians) * cos(point.lat_radians) * cos(delta_long)
)
radians = math.atan2(y, x)
return Bearing.from_radians(radians)
x = [0]
y = [0]
N = [0]
E = [0]
xc = [0]
yc = [0]
k = 0
for i in range(1, len(lat)):
E.append(E[-1] + dE[i])
N.append(N[-1] + dN[i])
B = Bearing(lat[i - 1], lon[i - 1], lat[i], lon[i]) * np.pi / 180.0
d = HaversineFormula(lat[i - 1], lon[i - 1], lat[i], lon[i])
print d, distance[i]
if (d):
xc.append(xc[-1] + d * np.sin(B))
yc.append(yc[-1] + d * np.cos(B))
k += 1
plt.figure()
plt.subplot(1, 2, 1)
plt.grid()
plt.plot(E, N, 'o-')
plt.plot(xc, yc, 'ro-')
plt.plot(0, 0, 'Xk')
plt.subplot(1, 2, 2)
def turnToFace(self, relativePosition): bearing = float(Bearing.toPoint(relativePosition)) * MAGIC_TURN_NUMBER self.rotateBy(bearing) self.stop()
def get_wind_bearing(self):
"""Return the absolute bearing of the wind"""
wind = Bearing(self.arduino.get_wind())
bearing = Bearing(self.arduino.get_compass())
return wind + bearing
