def main():
while True:
scan_list = fc.scan_step(23)
# print(scan_list)
if not scan_list:
continue
scan_list = [str(i) for i in scan_list]
scan_list = "".join(scan_list)
paths = scan_list.split("2")
length_list = []
for path in paths:
length_list.append(len(path))
# print(length_list)
if max(length_list) == 0:
fc.stop()
else:
i = length_list.index(max(length_list))
pos = scan_list.index(paths[i])
pos += (len(paths[i]) - 1) / 2
# pos = int(pos)
delta = len(scan_list) / 3
# delta *= us_step/abs(us_step)
if pos < delta:
fc.turn_left(speed)
elif pos > 2 * delta:
fc.turn_right(speed)
else:
if scan_list[int(len(scan_list)/2-1)] == "0":
fc.backward(speed)
else:
fc.forward(speed)
def main():
time.sleep(5)
while True:
scan_list = fc.scan_step(buf)
if not scan_list:
continue
tmp = scan_list[2:8]
tmpL = scan_list[2:5]
tmpR = scan_list[5:8]
print(tmp)
if tmp != [2, 2, 2, 2, 2, 2]:
print("Back Off")
fc.backward(pow)
time.sleep(bak)
if sum(tmpL) > sum(tmpR):
print("Turn Left")
fc.turn_left(pow * 2)
else:
print("Turn Right")
fc.turn_right(pow * 2)
else:
fc.forward(pow)
def mapTheWorld():
N = 139//2
mask = np.ones((N, 2*N, 3))
world = np.zeros((1024, 1024, 3))
grid = np.zeros((100, 100), 'int32')
X = (511, 1023) # Car position in world coordinates
R = np.radians(90) # Car heading in world coordinates
goalLocation = (511, 0)
step = 0
while step < 5:
print("step {0} car {1} heading {2}".format(step, X, np.degrees(R)))
m = sweep(72, True)
# The scan area to the car is always in local coordinates
# We will map them back into world coordinates
# Note that the scale is the same so all we have to deal with
# is translation and rotation
pts1 = np.float32([[N, 0], [N, N], [2*N, N]])
pts2 = np.float32([
[X[0] + N * np.cos(R),
X[1] - N * np.sin(R)],
[X[0], X[1]],
[X[0] + N * np.cos(R - np.pi/2),
X[1] - N * np.sin(R - np.pi/2)]])
print(pts1)
print(pts2)
mat = cv2.getAffineTransform(pts1, pts2)
dst = cv2.warpAffine(m.astype('float'), mat, world.shape[1::-1])
mask_t = cv2.warpAffine(mask, mat, world.shape[1::-1])
print(mask_t.shape)
print(world.shape)
print(dst.shape)
world2 = world.copy()
for c in range(3):
world2[:,:,c] = (1 - mask_t[:,:,c])*world[:,:,c] + \
mask_t[:,:,c]*dst[:,:,c]
world = world2
#draw a picture of car and current heading on
# pic for display
#world2 = cv2.circle(world2, X, radius=3, color=(255, 255, 255), thickness=1)
#cv2.imwrite("world_{0}.png".format(step), world2)
# Try to take a forward step and remap
fc.forward(10)
time.sleep(0.5)
fc.forward(0)
X = (X[0], X[1] - 15)
#X = (X[0] + 15, X[1] - 15)
#R = R + np.pi / 3
step = step + 1
def move_and_detect(direction, distance, speed, camera, input_height,
input_width, interpreter, labels, threshold):
print("Move in", direction, "direction for", distance, "cm.")
distance_travelled = 0
if direction == 'w':
fc.forward(10)
elif direction == 'a':
fc.turn_left(10)
elif direction == 's':
fc.backward(10)
elif direction == 'd':
fc.turn_right(10)
else:
fc.stop()
return
stop_sign_detected = False
should_stop = True
while distance_travelled < distance:
stop = False
start_time = time.monotonic()
image = capture_frame(camera, input_height, input_width)
results = detect_objects(interpreter, image, threshold)
if (should_stop):
saw_stop_sign = "stop sign" in get_detected_object_labels(
results, labels)
if (saw_stop_sign):
print("Stop sign detected")
if (stop_sign_detected and not saw_stop_sign):
stop = True
stop_sign_detected = False
elif (not stop_sign_detected and saw_stop_sign):
stop_sign_detected = True
elapsed_seconds = (time.monotonic() - start_time)
print("Elapsed time (ms): ", elapsed_seconds * 1000)
print_object_labels(results, labels)
distance_travelled += speed() * elapsed_seconds
if (stop):
print("Stop for 3s")
fc.stop()
time.sleep(3)
should_stop = True
stop = False
fc.forward(10)
fc.stop()
def Remote_control(control_flag, speed=50):
speed = int(speed)
if control_flag == 'forward':
fc.forward(speed)
elif control_flag == 'backward':
fc.backward(speed)
elif control_flag == 'turn_left':
fc.turn_left(speed)
elif control_flag == 'turn_right':
fc.turn_right(speed)
else:
fc.stop()
def main_loop():
if lettura_media(0) > MIN_DIST_FOLLOW:
fc.forward(20)
while lettura_media(0) > MIN_DIST_FOLLOW:
pass
fc.stop()
lval = scan_sx()
rval = scan_dx()
turn_time = 0.5 + random.random() * 1
if rval < lval:
fc.turn_right(20)
else:
fc.turn_left(20)
time.sleep(turn_time)
def main():
time.sleep(5)
fc.forward(pow)
time.sleep(3)
fc.backward(pow)
time.sleep(2)
fc.turn_left(pow)
time.sleep(2)
fc.turn_right(pow)
time.sleep(2)
fc.stop()
