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 Keyborad_control(): while True: global power_val key = readkey() if key == '6': if power_val <= 90: power_val += 10 print("power_val:", power_val) elif key == '4': if power_val >= 10: power_val -= 10 print("power_val:", power_val) if key == 'w': fc.forward(power_val) elif key == 'a': fc.turn_left(power_val) elif key == 's': fc.backward(power_val) elif key == 'd': fc.turn_right(power_val) else: fc.stop() if key == 'q': print("quit") break
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)
async def main_func(): global recv_dict, send_dict, gs_list while 1: gs_list = fc.get_grayscale_list() if recv_dict['CD'][0] == 'on': if fc.is_on_edge(recv_dict['CD'][1], gs_list): fc.backward(20) time.sleep(0.5) fc.stop() if recv_dict['TL'][0] == 'on': if fc.get_line_status(recv_dict['TL'][1], gs_list) == 0: fc.forward(recv_dict['PW']) elif fc.get_line_status(recv_dict['TL'][1], gs_list) == -1: fc.turn_left(recv_dict['PW']) elif fc.get_line_status(recv_dict['TL'][1], gs_list) == 1: fc.turn_right(recv_dict['PW']) if recv_dict['OA'] == 'on': scan_list = fc.scan_step(35) if scan_list: tmp = scan_list[3:7] if tmp != [2, 2, 2, 2]: fc.turn_right(recv_dict['PW']) else: fc.forward(recv_dict['PW']) elif recv_dict['OF'] == 'on': scan_list = fc.scan_step(23) if scan_list != False: 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)) 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 delta = len(scan_list) / 3 if pos < delta: fc.turn_left(recv_dict['PW']) elif pos > 2 * delta: fc.turn_right(recv_dict['PW']) else: if scan_list[int(len(scan_list) / 2 - 1)] == "0": fc.backward(recv_dict['PW']) else: fc.forward(recv_dict['PW']) elif recv_dict['RD'] == 'on': fc.scan_step(35) await asyncio.sleep(0.01)
def turnLeftAngle(angle): # I told it to turn 15, and I think it turned 25 # so I need to scale this by (15/25) = 3/5 #2.78 by experimentation. duration = (angle /180.) * (1.32) fc.turn_left(10) time.sleep(duration) fc.turn_left(0)
def move(direction): if direction == b'w': fc.forward(10) elif direction == b'a': fc.turn_left(10) elif direction == b's': fc.backward(10) elif direction == b'd': fc.turn_right(10)
def mleft(): speed4 = fc.Speed(25) speed4.start() fc.turn_left(15) x = 0 for i in range(3): time.sleep(0.1) speed4.deinit() fc.stop()
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 main(): start = [0, 0] # starting position end = [3,6] # ending position cost = 1 # cost per movement ind =0 while True: if ind == 0: maze = [[0, 0, 0, 0, 0, 0,0], [0, 0, 0, 0, 0, 0,0], [0, 0, 0, 0, 0, 0,0], [0, 0, 0, 0, 0, 0,0]] path = search(maze,cost, start, end) else: maze = new_maze path = search(maze,cost, [new_a,new_b], end) print("path now:",path) curn_node = cal_path(path) print("currnet noed",curn_node) # cal_path(path) if np.array_equal(curn_node[0], np.matrix(path[-2])) ==False: new_start = curn_node[0].reshape(-1,).tolist() print("############") print("new_start:",new_start) new_a =new_start[0][0] new_b=new_start[0][1] new_maze = maze print("rotation:",curn_node[1]) if curn_node[1]==0: new_maze[new_a+1][new_b] =1 elif curn_node[1]==1: fc.turn_right(turn_time_right) time.sleep(turn_time) fc.stop() new_maze[new_a][new_b+1] =1 print("adjust_L") elif curn_node[1]==-1: fc.turn_left(turn_time_left) time.sleep(turn_time) fc.stop() new_maze[new_a][new_b-1] =1 print("adjust_R") print(new_maze[0]) print(new_maze[1]) print(new_maze[2]) print(new_maze[3]) ind = ind+1 continue else: fc.forward(speed) time.sleep(go_time) fc.stop() print("done") break
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 turnLeft(): speed4 = Speed(25) speed4.start() # time.sleep(2) fc.turn_left(80) x = 0 for i in range(15): time.sleep(0.1) speed = speed4() x += speed * 0.1 print("%smm/s" % speed) print("%smm" % x) speed4.deinit() fc.stop()
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()
def roomba(speed=10): while True: # get scan by distance scan_list = scanner.scan_step_dist() if not scan_list: continue # the preprocessing limits the max distance to 200cm # since any -2 means no response set the value to 200cm scan_list = [200 if d == -2 else d for d in scan_list] # readings that are infront on the Picar # -54 degrees to 54 degrees ahead = scan_list[2:7] # coast clear full speed ahead if min(ahead) > 35: fc.forward(speed) continue logging.info("Too close stopping") fc.stop() left = scan_list[:5] right = scan_list[5:] # -1 = turn left, 0 forward, 1 turn right direction = 0 # evaluates which direction turn # turns in the direction with the most open space if (sum(right) > sum(left)): direction = 1 else: direction = -1 #print(sum(left), sum(right)) #print( direction) if direction == 1: logging.info("Turning right") fc.turn_right(speed) elif direction == -1: logging.info("Turning left") fc.turn_left(speed)
def turn_left(self, angle: int, power: int = 5): # need to adjust slippage for turning #slippage = 2.05 #1.74 slippage = 1.95 dist = utils.angle_to_dist(angle) * slippage self.trip_meter.reset() fc.turn_left(power) while(self.trip_meter.distance < dist): continue fc.stop() # update self.orientation = update_angle(self.orientation, -angle) print("new car orientation", self.orientation) return self.orientation
def move(direction, distance): print("Move in", direction, "direction for", distance, "cm.") speed4 = Speed(25) speed4.start() time.sleep(2) distance_travelled = 0 if direction == 'w': print("w") fc.forward(5) elif direction == 'a': fc.turn_left(10) #fc.forward(10) elif direction == 's': fc.backward(10) elif direction == 'd': fc.turn_right(10) #fc.forward(10) elif direction == "wa": print("wa") fc.turn_left(5) #fc.forward(10) elif direction == "wd": print("wd") fc.turn_right(5) #fc.forward(10) else: speed4.deinit() fc.stop() return while distance_travelled < distance: time.sleep(0.1) distance_travelled += speed4() * 0.1 print("travelled", distance_travelled, "cm") #print("eval 1: ", distance_travelled < distance ) speed4.deinit() fc.stop()
def move(direction, distance, speed): 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 while distance_travelled < distance: time.sleep(0.1) distance_travelled += speed() * 0.1 fc.stop()
def naviCar(turns): dir = [] o = start d = 'F' for t in turns: dy = t[0] - o[0] dx = t[1] - o[1] if dy == 0 and dx > 0: dir.append('R') if d == 'B' or d == 'F': fc.turn_right(pow) time.sleep(agR) d = 'R' if dy == 0 and dx < 0: dir.append('L') if d == 'F' or d == 'F': fc.turn_left(pow) time.sleep(agL) d = 'L' if dy > 0 and dx == 0: dir.append('B') if d == 'L': fc.turn_right(pow) time.sleep(agR) if d == 'R': fc.turn_left(pow) time.sleep(agL) d = 'B' if dy < 0 and dx == 0: dir.append('F') if d == 'L': fc.turn_right(pow) time.sleep(agR) if d == 'R': fc.turn_left(pow) time.sleep(agL) d = 'F' fc.stop() fc.forward(pow) time.sleep(fcm * abs(dy+dx)) o = t fc.stop() print("Direction: ", dir)
def cal_path(path_return): zero_one =np.matrix([0,1]) one_zero =np.matrix([1,0]) mone_zero =np.matrix([-1,0]) zero_mone =np.matrix([0,-1]) rotation =[] for i in range(len(path_return)-1): cure = np.matrix(path_return[i]) nextn =np.matrix(path_return[i+1]) b = (nextn-cure) if sum(rotation) ==0 and (b==one_zero).all(): detect_result() for i in range(10): scan_list = scan_step1(25) if not scan_list: continue tmp = scan_list[3:7] print(tmp) if 1 in tmp: print("cure",cure) return cure,sum(rotation) break print("for") rotation.append(0) fc.forward(speed) time.sleep(go_time) fc.stop() elif sum(rotation) ==0 and (b==zero_one).all(): print("left") rotation.append(1) fc.turn_left(turn_time_left) time.sleep(turn_time) fc.stop() detect_result() for i in range(10): scan_list = scan_step1(25) if not scan_list: continue tmp = scan_list[3:7] print(tmp) if 1 in tmp: print("cure",cure) return cure,sum(rotation) break fc.forward(speed) time.sleep(go_time) fc.stop() elif sum(rotation) ==0 and (b==zero_mone).all(): print("right") rotation.append(-1) fc.turn_right(turn_time_right) time.sleep(1) fc.stop() detect_result() for i in range(10): scan_list = scan_step1(25) if not scan_list: continue tmp = scan_list[3:7] print(tmp) if 1 in tmp: print("cure",cure) return cure,sum(rotation) break fc.forward(speed) time.sleep(go_time) fc.stop() elif sum(rotation) ==1 and (b==one_zero).all(): print("right") rotation.append(-1) fc.turn_right(turn_time_right) time.sleep(1) fc.stop() detect_result() for i in range(10): scan_list = scan_step1(25) if not scan_list: continue tmp = scan_list[3:7] print(tmp) if 1 in tmp: print("cure",cure) return cure,sum(rotation) break fc.forward(speed) time.sleep(go_time) fc.stop() elif sum(rotation) ==1 and (b==zero_one).all() : detect_result() for i in range(10): scan_list = scan_step1(25) if not scan_list: continue tmp = scan_list[3:7] print(tmp) if 1 in tmp: print("cure",cure) return cure,sum(rotation) break print("for") rotation.append(0) fc.forward(speed) time.sleep(go_time) fc.stop() elif sum(rotation) ==1 and (b==mone_zero).all(): print("left") rotation.append(1) fc.turn_left(turn_time_left) time.sleep(turn_time) fc.stop() detect_result() for i in range(10): scan_list = scan_step1(25) if not scan_list: continue tmp = scan_list[3:7] print(tmp) if 1 in tmp: print("cure",cure) return cure,sum(rotation) break fc.forward(speed) time.sleep(go_time) fc.stop() elif sum(rotation) ==-1 and (b==mone_zero).all(): print("right") rotation.append(-1) fc.turn_right(turn_time_right) time.sleep(1) fc.stop() detect_result() for i in range(10): scan_list = scan_step1(25) if not scan_list: continue tmp = scan_list[3:7] print(tmp) if 1 in tmp: print("cure",cure) return cure,sum(rotation) break fc.forward(speed) time.sleep(go_time) fc.stop() elif sum(rotation) ==-1 and (b==zero_mone).all(): detect_result() for i in range(10): scan_list = scan_step1(25) if not scan_list: continue tmp = scan_list[3:7] print(tmp) if 1 in tmp: print("cure",cure) return cure,sum(rotation) break print("for") rotation.append(0) fc.forward(speed) time.sleep(go_time) fc.stop() elif sum(rotation) ==-1 and (b==one_zero).all(): print("left") rotation.append(1) fc.turn_left(turn_time_left) time.sleep(turn_time) fc.stop() detect_result() for i in range(10): scan_list = scan_step1(25) if not scan_list: continue tmp = scan_list[3:7] print(tmp) if 1 in tmp: print("cure",cure) return cure,sum(rotation) break fc.forward(speed) time.sleep(go_time) fc.stop() return cure,rotation
def turnLeftAngle(angle): #2.78 by experimentation. duration = (angle / 180.) * (1.83333) fc.turn_left(10) time.sleep(duration) fc.turn_left(0)
def naviCar(turns): labels = load_labels(LABEL) interpreter = Interpreter(MODEL, num_threads=3) interpreter.allocate_tensors() _, input_height, input_width, _ = interpreter.get_input_details()[0]['shape'] camera = picamera.PiCamera( resolution=(CAMERA_WIDTH, CAMERA_HEIGHT), framerate=90) camera.rotation = 180 camera.video_stabilization = True dir = [] o = START d = 'F' id = 0 for t in turns: #print("==========>", t) dy = t[0] - o[0] dx = t[1] - o[1] if dy == 0 and dx > 0: dir.append('R') if d == 'B' or d == 'F': fc.turn_right(POW) time.sleep(AGR) d = 'R' if dy == 0 and dx < 0: dir.append('L') if d == 'F' or d == 'F': fc.turn_left(POW) time.sleep(AGL) d = 'L' if dy > 0 and dx == 0: dir.append('B') if d == 'L': fc.turn_right(POW) time.sleep(AGR) if d == 'R': fc.turn_left(POW) time.sleep(AGL) d = 'B' if dy < 0 and dx == 0: dir.append('F') if d == 'L': fc.turn_right(POW) time.sleep(AGR) if d == 'R': fc.turn_left(POW) time.sleep(AGL) d = 'F' fc.stop() # object detect to start id += 1 tStart = time.monotonic() image = getImage(input_height, input_width, camera, id) results = detect_objects(interpreter, image, THRES) tRef = getRespond(results, labels, 0) + (time.monotonic() - tStart) tBuffer = OBJDT tRemain = FCM * abs(dy+dx) tElapsed = 0 tDrive = 0 print('Reference Time: %.3f' % tRef) print('Initial Remained Time: %.3f' % tRemain) fc.forward(POW) while tRemain - max(tElapsed, tBuffer) > 0: tStart = time.monotonic() image = getImage(input_height, input_width, camera, 0) results = detect_objects(interpreter, image, THRES) tElapsed = (time.monotonic() - tStart) + getRespond(results, labels, 1) tDrive += tElapsed #tRemain -= max(tElapsed, tBuffer) tRemain -= tElapsed print('Loop Elapsed | Remain Time: %.3f | %.3f' % (tElapsed, tRemain)) time.sleep(tRemain) print('Turn Drive Time: %.3f' % (tDrive+tRemain)) o = t fc.stop() print("Direction: ", dir) camera.close()