def get_intersection_old(self):
coor = GPS.get_gps(color=self.color)
for origin, action in self.intersections:
if self._within_box(origin, coor):
if time.time() < self.wait_period + self.time_stamp:
return False, "Too Soon", coor
else:
self.time_stamp = time.time()
return True, action, coor
return False, "No Intersection", coor
def get_turn_old(self):
if self.destination is None:
raise Exception("Destination Point not set")
current_quad = self._get_quadrant(GPS.get_gps())
if self.cv.next(current_quad) == self.destination_quad:
return "LEFT"
elif self.cv.previous(current_quad) == self.destination_quad:
return "RIGHT"
else:
return "STRAIGHT"
def _near_destination(self):
current_coor = GPS.get_gps()
x = current_coor[0]
y = current_coor[1]
if x <= 0.0 or y <= 0.0:
return False
x_dif = abs(self.destination[0] - x)
y_dif = abs(self.destination[1] - y)
if x_dif < self.box_width and y_dif < self.box_height:
return True
else:
return False
# (840.0, 292.0),
# (844.0, 1520.0),
# (273.0, 941.0),
# ]
# for point in test_points:
# waypoint.set_point(point)
# current_point = GPS.get_gps()
# current_quad = waypoint._get_quadrant(current_point)
# print("Dest Point: {} Dest Point Quad: {} Current Point: {} Current Quad: {} Turn: {}, Arrived: {}" \
# .format(point, waypoint.destination_quad, \
# current_point, current_quad, waypoint.get_turn(), \
# waypoint.arrived()))
if len(sys.argv) < 3:
raise Exception("Missing Destination Point")
point = (float(sys.argv[1]), float(sys.argv[2]))
waypoint.set_point(point)
while True:
current_point = GPS.get_gps()
current_quad = waypoint._get_quadrant(current_point)
print("Dest Point: {} Dest Point Quad: {} Current Point: {} Current Quad: {} Turn: {}, Arrived: {}" \
.format(point, waypoint.destination_quad, \
current_point, current_quad, waypoint.get_turn(), \
waypoint.arrived()))
sleep(1)
