def evade_rev_2(drive_time, drive_burst, mode):
drive_iterate = int(round(drive_time / drive_burst))
print(
"evade.evade_rev_2 > Too close!\nevade.evade_rev_2 > Performing 2st order evasive manouver\n"
)
##Bring the return of the function optimal_direction in from optimal_direction.py
opt_dir = optimal_direction.optimal_direction()
if opt_dir == 'left':
print(
"evade.evade_rev_2 > I will proceed forward and left with caution"
)
##Repeat the steps below drive_iterate times
for y in range(drive_iterate):
##Run the function collision_avoidance() to check the distance from the front sensor to the closest object
check_bk_3(drive_time, drive_burst, mode)
##Drive left and forward for drive_burst seconds
driveme_tank.turn_left_fwd(drive_burst, mode)
##If the optimal direction is right, drive forward and right
elif opt_dir == 'right':
print(
"evade.evade_rev2 > I will proceed forward and right with caution"
)
##Repeat the steps below drive_iterate times
for y in range(drive_iterate):
##Run the function collision_avoidance() to check the distance from the front sensor to the closest object
check_bk_3(drive_time, drive_burst, mode)
##Drive right and forward for drive_burst seconds
driveme_tank.turn_right_fwd(drive_burst, mode)
else:
stuck_help(
"evade.evade_rev_2 > Ooops!\nevade.evade_rev_2 > I don\'t know how to solve this problem"
)
def evade_rev_1(drive_time, drive_burst, mode):
print(
"evade.evade_rev_1 > Too close!\nevade.evade_rev_1 > Performing 1st order evasive manouver\n"
)
drive_iterate = int(round(drive_time / drive_burst))
##Bring the return of the function optimal_direction in from optimal_direction.py
opt_dir = optimal_direction.optimal_direction()
##Check the front distance
front_dist = sensors.front_distance()
if front_dist > 15 and opt_dir == 'left':
print(
"evade.evade_rev_1 > I will proceed forward and left with caution"
)
##Repeat the steps below drive_iterate times
for y in range(drive_iterate):
##Collision collision_avoidance
check_bk_2(drive_time, drive_burst, mode)
##Drive left and forward for drive_burst seconds
driveme_tank.turn_left_fwd(drive_burst, mode)
elif front_dist > 15 and opt_dir == 'right':
print(
"evade.evade_rev_1 > I will proceed forward and right with caution"
)
##Repeat the steps below drive_iterate times
for y in range(drive_iterate):
##Collision collision_avoidance
check_bk_2(drive_time, drive_burst, mode)
##Drive left and forward for drive_burst seconds
driveme_tank.turn_right_fwd(drive_burst, mode)
else:
stuck_help(
"evade.evade_rev_1 > Boxed in on 3-sides\nevade.evade_rev_1 > I don\'t know how to solve this problem"
)
def evade_fwd_1(drive_time, drive_burst, mode):
drive_iterate = int(round(drive_time / drive_burst))
print(
"evade.evade_fwd_1 > Too close!\nevade.evade_fwd_1 > Performing 1st order evasive manouver\n"
)
##Bring the return of the function optimal_direction in from optimal_direction.py
opt_dir = optimal_direction.optimal_direction()
##First, reverse
print("evade.evade_fwd_1 > I will reverse with caution.")
##Repeat the steps below drive_iterate times
for y in range(drive_iterate):
##Run the function collision_avoidance() to check the distance from the front sensor to the closest object
check_bk_1(drive_time, drive_burst, mode)
##Drive left and forward for drive_burst seconds
driveme_tank.reverse(drive_burst, mode)
##If the optimal direction is left, drive forward and left
if opt_dir == 'left':
print(
"evade.evade_fwd_1 > I will proceed forward and left with caution"
)
##Repeat the steps below drive_iterate times
for y in range(drive_iterate):
##Run the function collision_avoidance() to check the distance from the front sensor to the closest object
check_fr_2(drive_time, drive_burst, mode)
##Drive left and forward for drive_burst seconds
driveme_tank.turn_left_fwd(drive_burst, mode)
##If the optimal direction is right, drive forward and right
elif opt_dir == 'right':
print(
"evade.evade_fwd_1 > I will proceed forward and right with caution"
)
##Repeat the steps below drive_iterate times
for y in range(drive_iterate):
##Run the function collision_avoidance() to check the distance from the front sensor to the closest object
check_fr_2(drive_time, drive_burst, mode)
##Drive right and forward for drive_burst seconds
driveme_tank.turn_right_fwd(drive_burst, mode)
##If the optimal direction is reverse, continue to reverse
elif opt_dir == 'reverse':
##Repeat the steps below drive_iterate times
for y in range(drive_iterate):
##Run the function collision_avoidance() to check the distance from the front sensor to the closest object
check_bk_1(drive_time, drive_burst, mode)
##Drive left and forward for drive_burst seconds
driveme_tank.reverse(drive_burst, mode)
##If the optimal direction is test_mode (manually configured), drive forward
elif opt_dir == 'test_mode':
print("evade.evade_fwd_1 > Test mode: driving forward ")
for y in range(drive_iterate):
##Run the function collision_avoidance() to check the distance from the front sensor to the closest object
check_fr_2(drive_time, drive_burst, mode)
##Drive right and forward for drive_burst seconds
print("Evade 1: Action after checking")
##Results should be only 'left', 'right', or 'reverse'. If not, see optimal_direction.py
else:
print(
"evade.evade_fwd_1 > Unexpected result from optimal_direction.py")
#drive vechicle forward for tf
driveme_tank.forward(tf, mode)
print("driveme_tank_test > sleeping")
time.sleep(ts)
#turn vehicle left while moving forward for tf seconds
driveme_tank.reverse(tf, mode)
print("driveme_tank_test > sleeping")
time.sleep(ts)
#turn vehicle left while moving forward for tf seconds
driveme_tank.turn_left_fwd(tf, mode)
print("driveme_tank_test > sleeping")
time.sleep(ts)
#turn vehicle right while moving forward for tf seconds
driveme_tank.turn_right_fwd(tf, mode)
print("driveme_tank_test > sleeping")
time.sleep(ts)
#turn vehicle left while reversing for tf seconds
driveme_tank.turn_left_rev(tf, mode)
print("driveme_tank_test > sleeping")
time.sleep(ts)
#turn vehicle right while reversing for tf seconds
driveme_tank.turn_right_rev(tf, mode)
print("driveme_tank_test > sleeping")
time.sleep(ts)
def mode_discovery(drive_time, drive_burst, mode, check):
print("\n\nexplore > Exploring with a bias towards direction:",
mode, "\n\n")
##When drive forward is chosen, continue for twice as long as the time spent driving to the left or right.
drive_iterate = int(round(drive_time / drive_burst))
drive_iterate_f = 2 * drive_iterate
##Desired result is to drive forward drive_burst seconds before checking (e.g. 0.03)
##Drive for drive_drive seconds
##Drive_iterations = drive_time
##1 represents forward, 2 represents forward and left, 3 represents forward and right
LHB_options = [1, 1, 1, 1, 1, 1, 2, 2, 2, 3]
RHB_options = [1, 1, 1, 1, 1, 1, 2, 3, 3, 3]
party_options = [1, 2, 3, 4, 5]
##Set the bias right or left or party based on input chosen when calling mode_discovery(tf, mode)
if mode == 'left':
bias = LHB_options
elif mode == 'right':
bias = RHB_options
elif mode == 'party':
bias = party_options
else:
print(
"explore > 'mode' defined in function mode_discovery(drive_time, drive_burst, mode, check) must be 'left' or 'right' or 'party'. Please try again."
)
##Check if obstacle cheking should be on or off
def collision_avoidance(drive_time, drive_burst, mode):
if check == 'on':
evade.check_fr_1(drive_time, drive_burst, mode)
elif check == 'off':
print("explore > Driving blind!!\n")
else:
print(
"explore > 'check' defined in function mode_discovery(drive_time, drive_burst, mode, check) must be 'on' or 'off'. Please try again."
)
def collision_avoidance_rev(drive_time, drive_burst, mode):
if check == 'on':
evade.check_bk_1(drive_time, drive_burst, mode)
elif check == 'off':
print("explore > Driving blind!!\n")
else:
print(
"explore > 'check' defined in function mode_discovery(drive_time, drive_burst, mode, check) must be 'on' or 'off'. Please try again."
)
##Choose a random direction to travel in:
x = random.choice(bias)
##If 1 is chosen at random from either LHB_options or RHB_options... (depending on bias 'left' or 'right')
if x == 1:
print("explore > I will explore forward with caution")
##Repeat the steps below drive_iterate_f times
for y in range(drive_iterate_f):
##Run the function collision_avoidance() to check the distance from the front sensor to the closest object
collision_avoidance(drive_time, drive_burst, mode)
##Drive forward for drive_burst seconds
driveme_tank.forward(drive_burst, mode)
##If 2 is chosen at random from either LHB_options or RHB_options... (depending on bias 'left' or 'right')
elif x == 2:
print(
"explore > I will explore forward and left with caution"
)
##Repeat the steps below drive_iterate times
for y in range(drive_iterate):
##Run the function collision_avoidance() to check the distance from the front sensor to the closest object
collision_avoidance(drive_time, drive_burst, mode)
##Drive left and forward for drive_burst seconds
driveme_tank.turn_left_fwd(drive_burst, mode)
##If 3 is chosen at random from either LHB_options or RHB_options... (depending on bias 'left' or 'right')
elif x == 3:
print(
"explore > I will explore forward and right with caution"
)
##Repeat the steps below drive_iterate times
for y in range(drive_iterate):
##Run the function collision_avoidance() to check the distance from the front sensor to the closest object
collision_avoidance(drive_time, drive_burst, mode)
##Drive right and forward for drive_burst seconds
driveme_tank.turn_right_fwd(drive_burst, mode)
elif x == 4:
print("explore > Mars walk to the left!!!")
##Repeat the steps below drive_iterate times
for y in range(drive_iterate):
##Run the function collision_avoidance() to check the distance from the front sensor to the closest object
collision_avoidance_rev(drive_time, drive_burst, mode)
##Drive right and forward for drive_burst seconds
driveme_tank.turn_left_rev(drive_burst, mode)
elif x == 5:
print("explore > Mars walk to the right!!!")
##Repeat the steps below drive_iterate times
for y in range(drive_iterate):
##Run the function collision_avoidance() to check the distance from the front sensor to the closest object
collision_avoidance_rev(drive_time, drive_burst, mode)
##Drive right and forward for drive_burst seconds
driveme_tank.turn_right_rev(drive_burst, mode)
else:
abort_drive(
"explore > Critical error in mode_discovery. Aborting..."
)