def get_intersection(self): coor, old_coor = GPS.get_gps_all(color=self.color) for origin, turn in self.intersections: if self._within_box(origin, coor): if time.time() < self.wait_period + self.time_stamp: return False, None else: self.time_stamp = time.time() return True, turn return False, None
def _handle_intersection(self): intersection, turn = self.intersections.get_intersection() #print("intersection {}, turn {}".format(intersection, turn)) if self.handling_intersection and not self.hard_coded_turns: cur_pos, old_pos = GPS.get_gps_all() #des_pos = self._find_closest(cur_pos, self.current_turn, self.current_turn_direction) #angle, _ = Translate.get_angle(cur_pos, old_pos, des_pos) # should break us out of the intersection handling mode. # this checks to see if the point we are closest to is the last point in the intersection #if des_pos == TURNS[self.current_turn][self.current_turn_direction][-1]: # self.handling_intersection = False #self.angle = angle/2 # self.angle = self.intersections_pid.run_pid(angle) des_pos = TURNS[self.current_turn][self.current_turn_direction][-1] at_end = self.intersections._within_box(cur_pos, des_pos) des_pos = TURNS[self.current_turn][self.current_turn_direction][0] end_straight = self.intersections._within_box( cur_pos, des_pos, 100, 100) if at_end or self.inter_start_time + 7 < time.time(): print("ending intersection handling at ", time.time(), "and at_end is", at_end) print(self.inter_start_time + 5, time.time()) self.handling_intersection = False return if end_straight: self.go_straight = False if self.go_straight: self.angle = 0 print("going straight") return if self.current_turn in ["2", "3", "4", "5"]: if self.current_turn_direction == "LEFT": self.angle = -20 elif self.current_turn_direction == "RIGHT": self.angle = 30 elif self.current_turn_direction == "STRAIGHT": self.car.mouse_straight(wait=8000, duration=20000, angle=0, speed=(self.speed + 0.3)) elif self.current_turn_direction == "LEFT": self.angle = -20 elif self.current_turn_direction == "RIGHT": self.angle = 30 #print("Cur Pos: {}, Old Pos: {}, Des Pos; {}".format(cur_pos, old_pos, des_pos)) #print("Turn angle: {}".format(self.angle)) elif intersection: self.handling_intersection = True self.current_turn = turn self.inter_start_time = time.time() self.go_straight = True if self.current_turn in ["2", "3", "4", "5"]: self.current_turn_direction = self.waypoint.get_turn() elif self.current_turn in ["0", "7"]: self.current_turn_direction = "LEFT" self.car.stop() time.sleep(1) elif self.current_turn in ["1", "6"]: self.current_turn_direction = "RIGHT" self.car.stop() time.sleep(1) print("Turning {} at {} at {}".format(self.current_turn_direction, self.current_turn, self.inter_start_time)) if self.hard_coded_turns: self.handling_intersection = False if self.current_turn in ["2", "3", "4", "5" ]: #at the four way, tune seperately if self.current_turn_direction == "RIGHT": self.car.right(wait=0.9, duration=2.5, angle=30, speed=(self.speed + 0.3)) elif self.current_turn_direction == "LEFT": self.car.left(wait=0 + 1.8, duration=1.8, angle=-20, speed=(self.speed + 0.3)) elif self.current_turn_direction == "STRAIGHT": self.car.straight(wait=0 + .75, duration=1.75, angle=0, speed=(self.speed + 0.34)) elif self.current_turn_direction == "RIGHT": self.car.right(wait=0.0 + .1, duration=2, angle=30, speed=(self.speed + 0.34)) elif self.current_turn_direction == "LEFT": self.car.left(wait=0.2 + 1, duration=2, angle=-20, speed=(self.speed + 0.34))