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)
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 mright():
speed4 = fc.Speed(25)
speed4.start()
fc.turn_right(15)
x = 0
for i in range(3):
time.sleep(0.1)
speed4.deinit()
fc.stop()
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 move25():
speed4 = Speed(25)
speed4.start()
# time.sleep(2)
fc.forward(35)
fc.stop()
time.sleep(0.5)
fc.turn_right(50)
print('distance:', fc.us.get_distance())
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 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():
while True:
scan_list = fc.scan_step(35)
if not scan_list:
continue
tmp = scan_list[3:7]
print(tmp)
if tmp != [2, 2, 2, 2]:
fc.turn_right(speed)
else:
fc.forward(speed)
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 turnRight():
speed4 = Speed(25)
speed4.start()
# time.sleep(2)
fc.turn_right(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_right(self, angle: int, power: int = 5):
# need to adjust slippage for turning
slippage = 2.2
dist = utils.angle_to_dist(angle) * slippage
self.trip_meter.reset()
fc.turn_right(power)
while(self.trip_meter.distance < dist):
continue
fc.stop()
self.orientation = update_angle(self.orientation, angle)
print("new car orientation", self.orientation)
return self.orientation
def move_scan():
# speed = 10 # 30
# speed1= Speed(10)
# speed1.start()
while True:
scan_list = fc.scan_step(50)
if not scan_list:
continue
tmp = scan_list[3:7]
print(tmp)
if tmp != [2,2,2,2]:
# fc.backward(30)
fc.turn_right(speed)
# fc.stop()
else:
fc.forward(speed)
def main():
while True:
scan_list = scan_step1(40)
if not scan_list:
continue
tmp = scan_list[3:7]
# if tmp == [0,0,0,0]:
# fc.backward(speed)
if 1 not in tmp:
print(tmp)
fc.forward(speed)
else:
print(tmp)
fc.stop()
time.sleep(0.3)
fc.backward(speed)
time.sleep(0.3)
fc.turn_right(speed)
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()
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 main():
reset_servo(-65)
while True:
#reset_servo(-65)
scan_list = fc.scan_step(25)
if not scan_list:
continue
tmp = scan_list[3:7]
print(tmp)
if tmp == [2,2,2,2]:
fc.forward(speed)
# reset_servo(-65)
else:
fc.stop()
time.sleep(0.5)
fc.backward(100)
time.sleep(0.5)
fc.stop()
time.sleep(0.5)
fc.turn_right(50)
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 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()
def turnRightAngle(angle):
#2.78 by experimentation.
duration = (angle / 180.) * (1.83333)
fc.turn_right(10)
time.sleep(duration)
fc.turn_right(0)
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
