Skip to content

Commit

Permalink
parking works well
Browse files Browse the repository at this point in the history
  • Loading branch information
@Willtura
Willtura committed May 30, 2024
1 parent f45ebee commit b01b27c
Showing 1 changed file with 64 additions and 47 deletions.
111 changes: 64 additions & 47 deletions src/parking/parking_handler.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,20 +60,12 @@ def start_parking(self, phase) -> None:

counter = 0
while phase == 4:
if self.__lidar.free_range(200,240, 400):
if counter == 3:
self.__can_controller.set_steering(-0.8)
phase = 5
continue
counter += 1

print(4)
time.sleep(0.1)
corner_angle = self.__lidar.find_nearest_angle(250, 300)
corner = self.__lidar.points[corner_angle]

wall_1 = self.__lidar.find_lowest_index(200, corner_angle, corner + 20, 600)
wall_2 = 0
wall_1 = self.__lidar.find_lowest_index(200, corner_angle, corner, 800)
wall_2 = self.__lidar.find_highest_index(corner_angle, 320, corner, 800)
indices = np.where(self.__lidar.points[corner_angle:320] == np.inf)[0]

if len(indices) > 0:
Expand All @@ -82,77 +74,102 @@ def start_parking(self, phase) -> None:
if wall_1 == 0 or wall_2 == 0:
continue

if corner_angle - wall_1 < 25 and wall_2 - corner_angle > 5 and (self.__lidar.points[wall_2] - self.__lidar.points[corner_angle]) >100 and (self.__lidar.points[wall_1]- self.__lidar.points[corner_angle]) > 100:
#if corner_angle > 260:
if corner_angle - wall_1 < 45 and wall_2 - corner_angle > 5 and (self.__lidar.points[wall_2] - self.__lidar.points[corner_angle]) > 50 and (self.__lidar.points[wall_1]- self.__lidar.points[corner_angle]) > 50:
if counter == 3:
self.__can_controller.set_steering(-0.8)
phase = 5
counter += 1
time.sleep(0.1)

while phase == 5:
print(5)
lowest_angle = self.__lidar.find_lowest_index(0, 150, 30, 900)
highest_angle = self.__lidar.find_highest_index(180, 320, 30, 900)
angle = highest_angle - lowest_angle

if angle < 230:
self.__can_controller.set_brake(100)
self.__can_controller.set_throttle(0, Gear.NEUTRAL)
self.__can_controller.set_steering(0)
phase = 6
if lowest_angle == 0:
if counter == 3:
self.__can_controller.set_brake(100)
phase = 3
continue
counter += 1
time.sleep(0.1)
reverse = False

while phase == 6:
time.sleep(0.1)
print(6)
left_wall = self.__lidar.find_lowest_index(130, 200, 30, 900)
right_wall = self.__lidar.find_highest_index(160, 230, 30, 900)
left_wall = self.__lidar.find_lowest_index(130, 250, 30, 900)
right_wall = self.__lidar.find_highest_index(160, 280, 30, 900)
self.__can_controller.set_throttle(20, Gear.DRIVE)

if right_wall == 0:
continue
deviation = ((right_wall - 180) + (180 - left_wall)) / 2
if deviation > 20:
phase = 7
deviation = (right_wall + left_wall) / 2 - 180
if deviation > 2:
self.__can_controller.set_steering(0.8)
else:
self.__can_controller.set_steering(-0.8)
print(deviation)
if -20 < deviation < 20:

if -23 < deviation < 23:
self.__can_controller.set_brake(100)
self.__can_controller.set_throttle(0, Gear.NEUTRAL)
phase = 8
self.__can_controller.set_steering(0.8)
continue

if not self.__lidar.free_range(160, 220, 80) and reverse:
phase = 7
continue
elif self.__lidar.free_range(160, 220, 250):
reverse = False
self.__can_controller.set_brake(0)
self.__can_controller.set_throttle(20, Gear.DRIVE)
if self.__lidar.find_obstacle_distance(180, 230) < 100:
self.__can_controller.set_brake(100)
self.__can_controller.set_throttle(0, Gear.NEUTRAL)
phase = 7

if self.__lidar.free_range(185,240, 400):
self.__can_controller.set_brake(100)
self.__can_controller.set_throttle(0, Gear.NEUTRAL)
phase = 8
time.sleep(0.1)
if self.__lidar.free_range(160, 220, 50):
reverse = True

while phase == 7:
print(7)
left_wall = self.__lidar.find_lowest_index(130, 200, 30, 900)
right_wall = self.__lidar.find_highest_index(160, 230, 30, 900)
if left_wall == 0:
continue
left_wall = self.__lidar.find_lowest_index(130, 240, 30, 900)
right_wall = self.__lidar.find_highest_index(150, 260, 30, 900)
deviation = ((right_wall - 180) + (180 - left_wall)) / 2
if deviation < 20:
self.start_parking(6)
if -20 < deviation < 20:
print(deviation)

lowest_angle = self.__lidar.find_lowest_index(30, 150, 70, 900)
highest_angle = self.__lidar.find_highest_index(180, 320, 70, 900)
angle = highest_angle - lowest_angle
print(angle)
if angle < 230 and deviation > 40:
self.__can_controller.set_brake(100)
self.__can_controller.set_throttle(0, Gear.NEUTRAL)
phase = 8
self.__can_controller.set_throttle(0, Gear.DRIVE)
self.__can_controller.set_steering(0)
reverse = False
self.start_parking(6)

self.__can_controller.set_steering(-0.8)
self.__can_controller.set_brake(0)
self.__can_controller.set_throttle(20, Gear.REVERSE)
if deviation < 0:
self.start_parking(6)
self.__can_controller.set_steering(0.8)
else:
self.__can_controller.set_steering(-0.8)

if self.__lidar.free_range(220, 250, 300):
if -25 < deviation < 25:
self.__can_controller.set_brake(100)
self.__can_controller.set_throttle(0, Gear.NEUTRAL)
phase = 8
if self.__lidar.free_range(160, 250, 80):
self.start_parking(6)

if not reverse:
self.start_parking(6)

# if self.__lidar.free_range(160,240, 300):
# self.__can_controller.set_brake(100)
# self.start_parking(6)
# time.sleep(0.1)
if self.__lidar.free_range(160, 250, 250):
reverse = False
self.start_parking(6)
else:
self.__can_controller.set_throttle(20, Gear.REVERSE)
time.sleep(0.1)

0 comments on commit b01b27c

Please sign in to comment.