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)