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))
