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