at_Stop_Line = False
time_after_stop = 20
lower_red = np.array([140, 20, 20])
upper_red = np.array([255, 120, 140])
turn_right = False
turn_left = False
time_at_curve = 0
while True:
k_p = 40 #Default=40 60
k_d = 10 #Defautl=10 30
speed = 0.3 #Default=0.3 max = 1
if turn_right:
i, j = env.get_grid_coords(env.cur_pos)
x, _, z = env.get_dir_vec()
x = round(x)
z = round(z)
j_rounded = j + z
i_rounded = i + x
direction2 = None
print(x, z, 'Direzione presa')
if x == 0 and z == 1:
direction2 = 'S'
elif x == 0 and z == -1:
direction2 = 'N'
elif x == 1 and z == 0:
direction2 = 'E'
elif x == -1 and z == 0:
direction2 = 'O'
while i != i_rounded or j != j_rounded:
(3.5, 0, 1.3), (3.0, 0, 1.3)])
route = route * env.road_tile_size
pt_index = np.argmin([np.linalg.norm(pt - env.cur_pos) for pt in route])
while True:
direction = -1 if args.backwards else 1
pt_dest = route[pt_index]
dist = np.linalg.norm(pt_dest - env.cur_pos)
if dist < 0.13:
pt_index = (pt_index + direction) % len(route)
pt_dest = route[pt_index]
dist = np.linalg.norm(pt_dest - env.cur_pos)
cur_dir = np.delete(env.get_dir_vec(), 1)
v = np.delete(pt_dest - env.cur_pos, 1)
desired_dir = direction * v / np.linalg.norm(v)
rot = np.arctan2(
-cur_dir[0] * desired_dir[1] + cur_dir[1] * desired_dir[0],
np.dot(cur_dir, desired_dir))
# print('pos {}, dest {}, dist {}'.format(np.delete(env.cur_pos, 1), pt_dest, dist))
# print('cd {}, dd {}, rot {}, ang {}'.format(cur_dir, desired_dir, rot, env.cur_angle))
speed = direction * 0.3 * (1 - 0.71 * np.clip(abs(rot) - 0.35, 0, 0.4))
steering = 2 * rot
env.step([speed, steering])
env.render(mode=render_mode)
