def run():
arduino = Arduino_Connection(com="com19") # find right com channel
#setup controller
KP = 0.75
KI = 0
KD = 0.5
controller = PID(KP, KI, KD)
camera = Camera(webcam_number=1, return_frame=True)
position, robot_angle, frame = camera.get_robot_position()
cv2.imshow('frame', frame)
while 1:
position, robot_angle, frame = camera.get_robot_position()
cv2.imshow('frame', frame)
desired_angle = 0
control(arduino, controller, robot_angle, desired_angle, debug=True)
time.sleep(0.1)
k = cv2.waitKey(5) & 0xFF
if k == 27:
break
def main():
camera = Camera(1, True)
while True:
blocks, frame = camera.get_block_coords()
position, angle, frame2 = camera.get_robot_position()
nav = Navigate()
if blocks and position:
block_data = nav.calculate_distances_angles(
blocks, position, angle)
print(nav.add_block_to_rejects(block_data, position, angle))
cv2.imshow('frame', frame)
cv2.imshow('frame2', frame2)
k = cv2.waitKey(5) & 0xFF
if k == 27:
break
def run():
arduino = Arduino_Connection(com="com19") # find right com channel
#setup controller
KP = 0.75
KI = 0
KD = 0.5
controller = PID(KP, KI, KD)
camera = Camera(webcam_number=1, return_frame=True)
position, robot_angle, frame = camera.get_robot_position()
cv2.imshow('frame', frame)
while 1:
position, robot_angle, frame = camera.get_robot_position(robot_position_colour_bounds=np.array([[43, 70], [145, 171], [0, 8], [15, 15]]))
blocks, blocks_frame = camera.get_block_coords([95, 105])
cv2.imshow('blocks frame', blocks_frame)
nav = Navigate()
block_data = None
if blocks and position:
block_data = nav.calculate_distances_angles(blocks, position, robot_angle)
best_block = nav.choose_next_block(block_data)
#print("best")
print(block_data[best_block][3])
cv2.circle(blocks_frame, (int(block_data[best_block][0]), int(block_data[best_block][1])), 5, (255,0,0), 3)
cv2.imshow('frame', frame)
if block_data and robot_angle:
desired_angle = block_data[best_block][3] + robot_angle
control(arduino, controller, robot_angle, desired_angle, debug=True)
time.sleep(0.1)
k = cv2.waitKey(5) & 0xFF
if k == 27:
break
def main():
camera = Camera(1, True)
while True:
blocks, frame = camera.get_block_coords([91, 114])
position, angle, frame2 = camera.get_robot_position()
nav = Navigate()
#print(blocks)
if blocks and position:
block_data = nav.calculate_distances_angles(blocks, position, angle)
best_block = nav.choose_next_block(block_data)
#print("best")
#print(block_data[best_block][3])
cv2.circle(frame2, (int(block_data[best_block][0]), int(block_data[best_block][1])), 5, (255,0,0), 3)
print(nav.block_in_range(best_block, block_data, position, angle))
cv2.imshow('frame', frame)
cv2.imshow('frame2', frame2)
k = cv2.waitKey(5) & 0xFF
if k == 27:
break
def run():
arduino = Arduino_Connection(com="com19") # find right com channel
#setup controller
KP = 0.75
KI = 0.1
KD = 0.75
controller = PID(KP, KI, KD)
camera = Camera(webcam_number=1, return_frame=True)
position, robot_angle, frame = camera.get_robot_position()
navigate = Navigate()
pick_up_drive = pick_up()
cv2.imshow('frame', frame)
blocks = None
while blocks == None:
blocks, blocks_frame = camera.get_block_coords() #[95, 105])
block_data = navigate.calculate_distances_angles(blocks, position,
robot_angle)
navigate.reject_line(block_data)
corner = False
while not corner:
position, robot_angle, frame = camera.get_robot_position(
) #robot_position_colour_bounds=np.array([[43, 70], [145, 171], [0, 8], [15, 15]]))
cv2.circle(frame, (500, 60), 3, (255, 0, 0), 2)
cv2.imshow('frame', frame)
#print(corner)
if position:
desired_angle, corner = navigate.go_to_point(
position, robot_angle, [500, 60])
if not corner:
#print(robot_angle)
control(arduino, controller, robot_angle, desired_angle)
time.sleep(0.1)
k = cv2.waitKey(5) & 0xFF
if k == 27:
break
top_line = False
while not top_line:
position, robot_angle, frame = camera.get_robot_position(
) #robot_position_colour_bounds=np.array([[43, 70], [145, 171], [0, 8], [15, 15]]))
cv2.circle(frame, (520, 130), 3, (255, 0, 0), 2)
cv2.imshow('frame', frame)
#print(corner)
if position:
desired_angle, top_line = navigate.go_to_point(
position, robot_angle, [520, 130])
if not top_line:
#print(robot_angle)
control(arduino, controller, robot_angle, desired_angle)
time.sleep(0.1)
k = cv2.waitKey(5) & 0xFF
if k == 27:
break
mid_line = False
while not mid_line:
position, robot_angle, frame = camera.get_robot_position(
) #robot_position_colour_bounds=np.array([[43, 70], [145, 171], [0, 8], [15, 15]]))
cv2.circle(frame, (520, 430), 3, (255, 0, 0), 2)
cv2.imshow('frame', frame)
#print(corner)
if position:
desired_angle, mid_line = navigate.go_to_point(
position, robot_angle, [520, 280])
if not mid_line:
#print(robot_angle)
control(arduino, controller, robot_angle, desired_angle)
time.sleep(0.1)
k = cv2.waitKey(5) & 0xFF
if k == 27:
break
bottom_line = False
while not bottom_line:
position, robot_angle, frame = camera.get_robot_position(
) #robot_position_colour_bounds=np.array([[43, 70], [145, 171], [0, 8], [15, 15]]))
cv2.circle(frame, (520, 430), 3, (255, 0, 0), 2)
cv2.imshow('frame', frame)
#print(corner)
if position:
desired_angle, bottom_line = navigate.go_to_point(
position, robot_angle, [520, 430])
if not bottom_line:
#print(robot_angle)
control(arduino, controller, robot_angle, desired_angle)
time.sleep(0.1)
k = cv2.waitKey(5) & 0xFF
if k == 27:
break
accepted_blocks = 0
rejected_blocks = 0
detect_reject = False
while 1:
position, robot_angle, frame = camera.get_robot_position(
) #robot_position_colour_bounds=np.array([[43, 70], [145, 171], [0, 8], [15, 15]]))
cv2.imshow('blocks frame', blocks_frame)
#block_data = None
if blocks and position:
block_data = navigate.calculate_distances_angles(
blocks, position, robot_angle)
best_block = navigate.choose_next_block(block_data)
#print("best")
#print(block_data[best_block][3])
cv2.circle(blocks_frame, (int(
block_data[best_block][0]), int(block_data[best_block][1])), 5,
(255, 0, 0), 3)
cv2.imshow('frame', frame)
for block in block_data:
rejects = navigate.return_rejects()
arrived = False
for reject in rejects:
if abs(block_data[block][0] - reject[0]) < 30 and abs(
block_data[block][0] - reject[0]) < 30:
arrived = True
break
else:
arrived = False
while not arrived:
position, robot_angle, frame = camera.get_robot_position(
robot_position_colour_bounds=np.array(
[[43, 70], [145, 171], [0, 8], [15, 15]]))
cv2.circle(
frame,
(int(block_data[block][0]), int(block_data[block][1])), 3,
(255, 0, 0), 2)
cv2.imshow('frame', frame)
#print(corner)
if position:
desired_angle, arrived = navigate.go_to_point(
position, robot_angle,
[int(block_data[block][0]),
int(block_data[block][1])])
if not arrived:
#print(robot_angle)
control(arduino, controller, robot_angle,
desired_angle)
time.sleep(0.1)
k = cv2.waitKey(5) & 0xFF
if k == 27:
break
top_green = False
while not top_green:
position, robot_angle, frame = camera.get_robot_position(
) #robot_position_colour_bounds=np.array([[43, 70], [145, 171], [0, 8], [15, 15]]))
cv2.circle(frame, (50, 200), 3, (255, 0, 0), 2)
cv2.imshow('frame', frame)
#print(corner)
if position:
desired_angle, top_green = navigate.go_to_point(
position, robot_angle, [50, 200])
if not top_green:
#print(robot_angle)
control(arduino, controller, robot_angle, desired_angle)
time.sleep(0.1)
k = cv2.waitKey(5) & 0xFF
if k == 27:
break
green = False
while not green:
position, robot_angle, frame = camera.get_robot_position(
) #robot_position_colour_bounds=np.array([[43, 70], [145, 171], [0, 8], [15, 15]]))
cv2.circle(frame, (50, 250), 3, (255, 0, 0), 2)
cv2.imshow('frame', frame)
#print(corner)
if position:
desired_angle, green = navigate.go_to_point(
position, robot_angle, [50, 250])
if not green:
#print(robot_angle)
control(arduino, controller, robot_angle, desired_angle)
time.sleep(0.1)
k = cv2.waitKey(5) & 0xFF
if k == 27:
break
arduino.open_back_gate()
end = False
while not end:
position, robot_angle, frame = camera.get_robot_position(
) #robot_position_colour_bounds=np.array([[43, 70], [145, 171], [0, 8], [15, 15]]))
cv2.circle(frame, (50, 500), 3, (255, 0, 0), 2)
cv2.imshow('frame', frame)
#print(corner)
if position:
desired_angle, end = navigate.go_to_point(
position, robot_angle, [50, 500])
if not end:
#print(robot_angle)
control(arduino, controller, robot_angle, desired_angle)
time.sleep(0.1)
k = cv2.waitKey(5) & 0xFF
if k == 27:
break
arduino.drive(0, 0)
arduino.close_back_gate()
break
time.sleep(0.1)
k = cv2.waitKey(5) & 0xFF
if k == 27:
break
def run():
arduino = Arduino_Connection(com="com19") # find right com channel
#setup controller
KP = 0.75
KI = 0
KD = 0.75
controller = PID(KP, KI, KD)
camera = Camera(webcam_number=1, return_frame=True)
position, robot_angle, frame = camera.get_robot_position()
navigate = Navigate()
pick_up_drive = pick_up()
cv2.imshow('frame', frame)
blocks = None
while blocks == None:
blocks, blocks_frame = camera.get_block_coords() #[95, 105])
block_data = navigate.calculate_distances_angles(blocks, position,
robot_angle)
navigate.reject_line(block_data)
position, robot_angle, frame = camera.get_robot_position(
) #robot_position_colour_bounds=np.array([[43, 70], [145, 171], [0, 8], [15, 15]]))
cv2.imshow('blocks frame', blocks_frame)
#block_data = None
if blocks and position:
block_data = navigate.calculate_distances_angles(
blocks, position, robot_angle)
best_block = navigate.choose_next_block(block_data)
#print("best")
#print(block_data[best_block][3])
cv2.circle(
blocks_frame,
(int(block_data[best_block][0]), int(block_data[best_block][1])),
5, (255, 0, 0), 3)
cv2.imshow('frame', frame)
"""Go to all blocks that are randomly scattered"""
for block in block_data:
rejects = navigate.return_rejects()
arrived = False
for reject in rejects:
if abs(block_data[block][0] - reject[0]) < 30 and abs(
block_data[block][0] - reject[0]) < 30:
arrived = True
break
else:
arrived = False
while not arrived:
position, robot_angle, frame = camera.get_robot_position(
robot_position_colour_bounds=np.array([[43, 70], [145, 171],
[0, 8], [15, 15]]))
cv2.circle(frame,
(int(block_data[block][0]), int(block_data[block][1])),
3, (255, 0, 0), 2)
cv2.imshow('frame', frame)
#print(corner)
if position and robot_angle:
desired_angle, arrived = navigate.go_to_point(
position, robot_angle,
[int(block_data[block][0]),
int(block_data[block][1])])
if not arrived:
#print(robot_angle)
control(arduino, controller, robot_angle, desired_angle)
time.sleep(0.1)
k = cv2.waitKey(5) & 0xFF
if k == 27:
break
"""Drop off blocks"""
top_green = False
while not top_green:
position, robot_angle, frame = camera.get_robot_position(
) #robot_position_colour_bounds=np.array([[43, 70], [145, 171], [0, 8], [15, 15]]))
cv2.circle(frame, (75, 200), 3, (255, 0, 0), 2)
cv2.imshow('frame', frame)
#print(corner)
if position:
desired_angle, top_green = navigate.go_to_point(
position, robot_angle, [50, 200])
if not top_green:
#print(robot_angle)
control(arduino, controller, robot_angle, desired_angle)
time.sleep(0.1)
k = cv2.waitKey(5) & 0xFF
if k == 27:
break
for i in range(60):
position, robot_angle, frame = camera.get_robot_position()
cv2.imshow('frame', frame)
if robot_angle:
control(arduino, controller, robot_angle, -math.pi)
time.sleep(0.1)
arduino.turn_out_left()
time.sleep(2)
arduino.open_back_gate()
green = False
while not green:
position, robot_angle, frame = camera.get_robot_position(
) #robot_position_colour_bounds=np.array([[43, 70], [145, 171], [0, 8], [15, 15]]))
cv2.circle(frame, (70, 400), 3, (255, 0, 0), 2)
cv2.imshow('frame', frame)
#print(corner)
if position:
desired_angle, green = navigate.go_to_point(
position, robot_angle, [70, 400])
if not green:
#print(robot_angle)
control(arduino, controller, robot_angle, desired_angle)
time.sleep(0.1)
k = cv2.waitKey(5) & 0xFF
if k == 27:
break
while position[1] < 370:
position, robot_angle, frame = camera.get_robot_position()
cv2.imshow('frame', frame)
while position == None:
position, robot_angle, frame = camera.get_robot_position()
time.sleep(0.1)
"""Set to True to try get blocks on line or False to ignore them"""
line = False
if line:
arduino.close_back_gate()
corner = False
while not corner:
position, robot_angle, frame = camera.get_robot_position(
) #robot_position_colour_bounds=np.array([[43, 70], [145, 171], [0, 8], [15, 15]]))
cv2.circle(frame, (500, 90), 3, (255, 0, 0), 2)
cv2.imshow('frame', frame)
#print(corner)
if position:
desired_angle, corner = navigate.go_to_point(
position, robot_angle, [500, 90])
if not corner:
#print(robot_angle)
control(arduino, controller, robot_angle, desired_angle)
time.sleep(0.1)
k = cv2.waitKey(5) & 0xFF
if k == 27:
break
for i in range(50):
position, robot_angle, frame = camera.get_robot_position()
cv2.imshow('frame', frame)
if robot_angle:
control(arduino, controller, robot_angle, 0)
time.sleep(0.1)
arduino.turn_out_right()
while position[1] < 430:
position, robot_angle, frame = camera.get_robot_position()
cv2.imshow('frame', frame)
while position == None:
position, robot_angle, frame = camera.get_robot_position()
control(arduino, controller, robot_angle, 1.6)
time.sleep(0.1)
top_green = False
while not top_green:
position, robot_angle, frame = camera.get_robot_position(
) #robot_position_colour_bounds=np.array([[43, 70], [145, 171], [0, 8], [15, 15]]))
cv2.circle(frame, (75, 200), 3, (255, 0, 0), 2)
cv2.imshow('frame', frame)
#print(corner)
if position:
desired_angle, top_green = navigate.go_to_point(
position, robot_angle, [50, 200])
if not top_green:
#print(robot_angle)
control(arduino, controller, robot_angle, desired_angle)
time.sleep(0.1)
k = cv2.waitKey(5) & 0xFF
if k == 27:
break
for i in range(60):
position, robot_angle, frame = camera.get_robot_position()
cv2.imshow('frame', frame)
if robot_angle:
control(arduino, controller, robot_angle, -math.pi)
time.sleep(0.1)
arduino.turn_out_left()
time.sleep(2)
arduino.open_back_gate()
while position[1] < 370:
position, robot_angle, frame = camera.get_robot_position()
cv2.imshow('frame', frame)
while position == None:
position, robot_angle, frame = camera.get_robot_position()
time.sleep(0.1)
end = False
while not end:
position, robot_angle, frame = camera.get_robot_position(
) #robot_position_colour_bounds=np.array([[43, 70], [145, 171], [0, 8], [15, 15]]))
cv2.circle(frame, (50, 500), 3, (255, 0, 0), 2)
cv2.imshow('frame', frame)
#print(corner)
if position:
desired_angle, end = navigate.go_to_point(position, robot_angle,
[50, 500])
if not end:
#print(robot_angle)
control(arduino, controller, robot_angle, desired_angle)
time.sleep(0.1)
k = cv2.waitKey(5) & 0xFF
if k == 27:
break
arduino.drive(0, 0)
arduino.close_back_gate()
def run():
arduino = Arduino_Connection(com="com19") # find right com channel
#setup controller
KP = 0.75
KI = 0
KD = 0.75
controller = PID(KP, KI, KD)
camera = Camera(webcam_number=1, return_frame=True)
position, robot_angle, frame = camera.get_robot_position()
navigate = Navigate()
pick_up_drive = pick_up()
cv2.imshow('frame', frame)
blocks, blocks_frame = camera.get_block_coords() #[95, 105])
block_data = navigate.calculate_distances_angles(blocks, position,
robot_angle)
nav.reject_line(block_data)
corner = False
while not corner:
position, robot_angle, frame = camera.get_robot_position(
) #robot_position_colour_bounds=np.array([[43, 70], [145, 171], [0, 8], [15, 15]]))
cv2.circle(frame, (520, 60), 3, (255, 0, 0), 2)
cv2.imshow('frame', frame)
#print(corner)
if position:
desired_angle, corner = navigate.go_to_point(
position, robot_angle, [520, 60])
if not corner:
#print(robot_angle)
control(arduino, controller, robot_angle, desired_angle)
time.sleep(0.1)
k = cv2.waitKey(5) & 0xFF
if k == 27:
break
top_line = False
while not top_line:
position, robot_angle, frame = camera.get_robot_position(
) #robot_position_colour_bounds=np.array([[43, 70], [145, 171], [0, 8], [15, 15]]))
cv2.circle(frame, (530, 130), 3, (255, 0, 0), 2)
cv2.imshow('frame', frame)
#print(corner)
if position:
desired_angle, top_line = navigate.go_to_point(
position, robot_angle, [530, 130])
if not top_line:
#print(robot_angle)
control(arduino, controller, robot_angle, desired_angle)
time.sleep(0.1)
k = cv2.waitKey(5) & 0xFF
if k == 27:
break
mid_line = False
while not mid_line:
position, robot_angle, frame = camera.get_robot_position(
) #robot_position_colour_bounds=np.array([[43, 70], [145, 171], [0, 8], [15, 15]]))
cv2.circle(frame, (530, 430), 3, (255, 0, 0), 2)
cv2.imshow('frame', frame)
#print(corner)
if position:
desired_angle, mid_line = navigate.go_to_point(
position, robot_angle, [530, 280])
if not mid_line:
#print(robot_angle)
control(arduino, controller, robot_angle, desired_angle)
time.sleep(0.1)
k = cv2.waitKey(5) & 0xFF
if k == 27:
break
bottom_line = False
while not bottom_line:
position, robot_angle, frame = camera.get_robot_position(
) #robot_position_colour_bounds=np.array([[43, 70], [145, 171], [0, 8], [15, 15]]))
cv2.circle(frame, (530, 430), 3, (255, 0, 0), 2)
cv2.imshow('frame', frame)
#print(corner)
if position:
desired_angle, bottom_line = navigate.go_to_point(
position, robot_angle, [530, 430])
if not bottom_line:
#print(robot_angle)
control(arduino, controller, robot_angle, desired_angle)
time.sleep(0.1)
k = cv2.waitKey(5) & 0xFF
if k == 27:
break
accepted_blocks = 0
rejected_blocks = 0
detect_reject = False
while 1:
position, robot_angle, frame = camera.get_robot_position(
) #robot_position_colour_bounds=np.array([[43, 70], [145, 171], [0, 8], [15, 15]]))
cv2.imshow('blocks frame', blocks_frame)
block_data = None
if blocks and position:
block_data = navigate.calculate_distances_angles(
blocks, position, robot_angle)
best_block = navigate.choose_next_block(block_data)
#print("best")
#print(block_data[best_block][3])
cv2.circle(blocks_frame, (int(
block_data[best_block][0]), int(block_data[best_block][1])), 5,
(255, 0, 0), 3)
cv2.imshow('frame', frame)
#if navigate.block_in_range(block_data, position, robot_angle):
# pick_up_drive.drive_to_collect()
if block_data and robot_angle:
desired_angle = block_data[best_block][3] + robot_angle
control(arduino, controller, robot_angle, desired_angle)
state = arduino.get_block_state()
if state != Block_States.NO_BLOCK:
#state = arduino.get_block_state()
#print("state: ")
#print(state)
if state == 2:
accepted_blocks += 1
print(state)
detect_reject = True
if state == 3:
rejected_blocks += 1
print(state)
detect_reject = True
if detect_reject:
rejected = navigate.add_block_to_rejects(block_data, position,
robot_angle)
if rejected == True:
detect_reject = False
if accepted_blocks >= 5:
while True:
relative_angle, arrived = navigate.go_to_point(
position, robot_angle, [50, 300])
if not arrived:
desired_angle = relative_angle + robot_angle
control(arduino,
controller,
robot_angle,
desired_angle,
debug=True)
for reject in navigate.reject_blocks:
print(reject)
time.sleep(0.1)
k = cv2.waitKey(5) & 0xFF
if k == 27:
break
