def __init__(self, dt, Kp_h=None, Kp_theta=None, Ki_h=None):
" Initialize the Altitude controller "
if Kp_h is not None:
self.Kp_h = Kp_h
if Ki_h is not None:
self.Ki_h = Ki_h
if Kp_theta is not None:
self.Kp_theta = Kp_theta
self.altitude_pid = PID(Kp=self.Kp_h, Ki=self.Ki_h, Kd=0.0, dt=dt)
self.pitch_pid = PID(Kp=self.Kp_theta, Ki=0.0, Kd=0.0, dt=dt)
def _setup_worker_schedule(self, worker_schedule):
log.debug('Receiving schedule...')
self._pause_all_devices()
self.current_set_temperature = float(worker_schedule[0][1])
self.current_hold_time = self.duration_str_to_delta(
worker_schedule[0][0])
self.working = True
self.start_time = datetime.now()
self.start_hold_timer = None
self.hold_pause_timer = None
self.pause_time = timedelta(seconds=0)
cycle_time = float(self._get_device(self.thermometer_name).cycle_time)
if self.pid is None:
self.pid = PID(None, self.current_set_temperature, cycle_time)
else:
self.pid = PID(self.pid.pid_params, self.current_set_temperature,
cycle_time)
self._resume_all_devices()
def __init__(self, dt, Kp_p=None, Ki_p=None, Kd_p=None):
" Initialize the Roll Angle controller. "
if Kp_p is None:
Kp_p = self.Kp_p
if Ki_p is None:
Ki_p = self.Ki_p
if Kd_p is None:
Kd_p = self.Kd_p
self.roll_pid = PID(Kp=Kp_p, Ki=0.0, Kd=0.0, dt=dt)
def pid(output, coord, object_center):
# Get the right values and initialize PID
if coord == 'pan':
pid = PID(panP, panI, panD)
screen_center = centerX
else:
pid = PID(tiltP, tiltI, tiltD)
screen_center = centerY
pid.initialize()
while True:
# Calculate the error
error = screen_center - object_center.value
# If error is 0 then object is in center or is not detected, so re-initialize PID
if abs(error) < screen_center / 10:
pid.initialize()
output.value = 0
else:
output.value = pid.update(error)
time.sleep(0.01)
def __init_pid(self):
self.__throttle_pid = PID(P=self.settings["throttle_pid_p"],
I=self.settings["throttle_pid_i"],
D=self.settings["throttle_pid_d"])
self.__throttle_pid.setOutMinLimit(-1)
self.__throttle_pid.setOutMaxLimit(1)
self.__throttle_pid.SetPoint = 0
p = self.settings.get("steering_pid_p")
self.__steering_pid = PID(P=p,
I=self.settings["steering_pid_i"],
D=self.settings["steering_pid_d"])
log.info(f"PID p:{p}")
self.__steering_pid.setOutMinLimit(-1)
self.__steering_pid.setOutMaxLimit(1)
self.__steering_pid.SetPoint = 0
def pid_process(output, p, i, d, objCoord, centerCoord):
# signal trap to handle keyboard interrupt
signal.signal(signal.SIGINT, signal_handler)
# create and init PID
p = PID(p.value, i.value, d.value)
p.initialize()
time.sleep(2)
while True:
error = centerCoord.value - objCoord.value
output.value = p.update(error)
def pid_process(output, p, i, d, objCoord, centerCoord):
# signal trap to handle keyboard interrupt
signal.signal(signal.SIGINT, signal_handler)
# create a PID and initialize it
p = PID(p.value, i.value, d.value)
p.initialize()
# loop indefinitely
while True:
# calculate the error
error = centerCoord.value - objCoord.value
# update the value
output.value = p.update(error)
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 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 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