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)
print("err:", error, "correction:", output.value)
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 __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(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 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()
class TemperatureWorker(DeviceWorker):
SSR_NAME = "SSR_NAME"
SSR_IO = "SSR_IO"
SSR_ACTIVE = "SSR_ACTIVE"
SSR_CYCLE_TIME = "SSR_CYCLE_TIME"
THERMOMETER_NAME = "THERMOMETER_NAME"
THERMOMETER_IO = "THERMOMETER_IO"
THERMOMETER_ACTIVE = "THERMOMETER_ACTIVE"
THERMOMETER_CYCLE_TIME = "THERMOMETER_CYCLE_TIME"
EVENT_ON_TEMPERATURE_REACHED = "on_temperature_reached"
def __init__(self):
DeviceWorker.__init__(self)
self.working = False
self.simulation = False
self.schedule = None
self.enabled = False
self.active = False
self.hold_pause_timer = None
self.paused = False
self.pause_time = None
self.ssr_name = os.environ.get(self.SSR_NAME)
self.thermometer_name = os.environ.get(self.THERMOMETER_NAME)
self.pid = None
self.kpid = None
self.current_temperature = 0.0
self.current_set_temperature = 0.0
self.current_hold_time = timedelta(minutes=0)
self.start_time = None
self.stop_time = None
self.start_hold_timer = None
self._setup_prometheus()
@staticmethod
def duration_str_to_delta(str):
t = datetime.strptime(str, "%H:%M:%S")
return timedelta(hours=t.hour, minutes=t.minute, seconds=t.second)
def _events(self):
events = ScheduleWorker._events(self)
events.append(self.EVENT_ON_TEMPERATURE_REACHED)
return events
def add_devices(self):
ssr_name = os.environ.get(self.SSR_NAME)
ssr_io = os.environ.get(self.SSR_IO)
ssr_active = os.environ.get(self.SSR_ACTIVE,
'false').lower() in ['1', 'true']
ssr_cycle_time = int(os.environ.get(self.SSR_CYCLE_TIME))
ssr_callback = self._ssr_callback
ssr = self._create_ssr(ssr_name, ssr_io, ssr_active, ssr_cycle_time,
ssr_callback)
self._add_device(ssr_name, ssr)
therm_name = os.environ.get(self.THERMOMETER_NAME)
therm_io = os.environ.get(self.THERMOMETER_IO)
therm_active = os.environ.get(self.THERMOMETER_ACTIVE,
'false').lower() in ['1', 'true']
therm_cycle_time = int(os.environ.get(self.THERMOMETER_CYCLE_TIME))
therm_callback = self._temperature_callback
thermometer = self._create_thermometer(therm_name, therm_io,
therm_active, therm_cycle_time,
therm_callback)
self._add_device(therm_name, thermometer)
def _create_ssr(self, name, io, active, cycle_time, callback):
return SSR(name, io, active, cycle_time, callback, self)
def _create_thermometer(self, name, io, active, cycle_time, callback):
return Probe(name, io, active, cycle_time, callback, self)
def _check_events(self):
# The schedule ping this every 5 seconds
pass
def _calculate_finish_time(self):
return self.start_hold_timer + (self.current_hold_time +
self.pause_time)
def _is_done(self):
if self.start_hold_timer is None:
return False
finish = self._calculate_finish_time()
now = datetime.now()
if finish <= now:
return True
log.debug('Time until work done: {0}'.format(str(finish - now)))
return False
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()
#schedule.every(5).seconds.do(self._check_events)
def stop_worker(self):
self._get_device(self.ssr_name).write(0.0)
self._pause_all_devices()
self.working = False
self.enabled = False
self.stop_time = datetime.now()
super(DeviceWorker, self).stop_worker()
return True
def pause_worker(self):
log.debug('Pause {0}'.format(self))
self._pause_all_devices()
if self.start_hold_time is not None:
self.hold_pause_timer = datetime.now()
self.paused = True
super(DeviceWorker, self).pause_worker()
return True
def resume_worker(self):
log.info('Resume {0}'.format(self))
if self.hold_pause_timer is not None and self.start_hold_time is not None:
self.pause_time += (datetime.now() - self.hold_pause_timer)
self.hold_pause_timer = None
self._resume_all_devices()
self.paused = False
super(DeviceWorker, self).resume_worker()
return True
def _calculate_pid(self, measured_value):
return self.pid.calculate(measured_value, self.current_set_temperature)
def _create_measurement(self, name, device_name, value, set_point, work,
remaining):
measurement = {}
measurement["name"] = name
measurement["device_name"] = device_name
measurement["value"] = value
measurement["set_point"] = set_point
measurement["work"] = work
measurement["remaining"] = remaining
return measurement
def _temperature_callback_event(self, measured_value, measurement):
pass
def _ssr_callback_event(self, measured_value, measurement):
pass
def _temperature_callback(self, measured_value):
try:
calc = 0.0
self.current_temperature = measured_value
if self.pid is not None:
calc = self._calculate_pid(measured_value)
log.debug(
'{0} reports measured value {1} ({2}) and pid calculated {3}'
.format(self.name, round(measured_value, 1),
measured_value, calc))
else:
log.debug('{0} reports measured value {1} ({2})'.format(
self.name, round(measured_value, 1), measured_value))
if self.start_hold_timer is None:
work = 'Reaching temperature {:.2f}'.format(
self.current_set_temperature)
else:
work = 'Holding temperature at {:.2f}'.format(
self.current_set_temperature)
measurement = self._create_measurement(
self.name,
self._get_device(self.thermometer_name).name,
self.current_temperature, self.current_set_temperature, work,
'{:.2f}'.format(self.current_set_temperature -
self.current_temperature))
self._temperature_callback_event(measurement, measured_value)
self._send_measurement(measurement)
if self.working and self.start_hold_timer is None and round(
measured_value, 1) >= self.current_set_temperature:
self.start_hold_timer = datetime.now()
self._send_event_to_master(self.EVENT_ON_TEMPERATURE_REACHED)
if self._is_done():
self.stop_worker()
self._send_master_is_finished()
elif self.pid is not None:
self._get_device(self.ssr_name).write(calc)
except Exception as e:
log.error(
'TemperatureWorker unable to react to temperature update, shutting down: {0}'
.format(e.args[0]))
self._stop_all_devices()
def _ssr_callback(self, heating_ratio):
device = self._get_device(self.ssr_name)
try:
log.debug('{0} reports heating ratio of {1} percent'.format(
self.name, heating_ratio))
if self.start_hold_timer is None:
work = 'Reaching temperature {:.2f}'.format(
self.current_set_temperature)
remaining = 'Unknown'
else:
work = 'Holding temperature at {:.2f}'.format(
self.current_set_temperature)
remaining = (self._calculate_finish_time() - datetime.now())
measurement = self._create_measurement(self.name, device.name,
heating_ratio,
self.current_hold_time,
work, remaining)
self._ssr_callback_event(heating_ratio, measurement)
self._send_measurement(measurement)
except Exception as e:
log.error(
'TemperatureWorker unable to react to heating update, shutting down: {0}'
.format(e.args[0]))
self._stop_all_devices()
def _setup_prometheus(self):
try:
labels = ['name', "device_name", 'device_type']
TemperatureWorker.PROM_HEATING_RATIO = Gauge(
'HEATING_RATIO', 'Heating ratio', labels)
TemperatureWorker.PROM_TEMPERATURE = Gauge('TEMPERATURE',
'Temperature', labels)
except ValueError:
pass # It is already defined
def _send_measurement(self, worker_measurement):
log.info('Send measurement triggered')
if worker_measurement.get('device_name',
'') == os.environ.get(self.SSR_NAME):
TemperatureWorker.PROM_HEATING_RATIO.labels(
self.name, self.ssr_name,
'SSR').set(worker_measurement.get('value', -1))
log.info('Sending heating ratio to Prometheus')
elif worker_measurement.get('device_name', '') == os.environ.get(
self.THERMOMETER_NAME):
TemperatureWorker.PROM_TEMPERATURE.labels(
self.name, self.thermometer_name,
'Thermometer').set(worker_measurement.get('value', -1))
log.info('Sending temperature to Prometheus')
log.info('{0}: {1} - work {2} - remaining {3}'.format(
worker_measurement.get('device_name'),
worker_measurement.get('value'), worker_measurement.get('work'),
worker_measurement.get('remaining')))
def _get_grafana_rows(self):
# 1. Define the panel in grafana
# 2. View panel json i.e. https://imgur.com/HcsW9sf
# 3. Paste JSON and convert to python dict
# This example only has 1 row with 1 panel
panels = [{
"panels": [{
"aliasColors": {},
"bars":
False,
"dashLength":
10,
"dashes":
False,
"datasource":
None,
"fill":
1,
"id":
1,
"legend": {
"avg": False,
"current": False,
"max": False,
"min": False,
"show": True,
"total": False,
"values": False
},
"lines":
True,
"linewidth":
1,
"links": [],
"nullPointMode":
"null",
"percentage":
False,
"pointradius":
5,
"points":
False,
"renderer":
"flot",
"seriesOverrides": [{
"alias": "Heating Time",
"yaxis": 2
}],
"spaceLength":
10,
"span":
12,
"stack":
False,
"steppedLine":
False,
"targets": [{
"expr": "TEMPERATURE{name=\"" + self.name + "\"}",
"format": "time_series",
"instant": False,
"interval": "",
"intervalFactor": 2,
"legendFormat": "Temperature",
"refId": "A",
"step": 1
}, {
"expr": "HEATING_RATIO{name=\"" + self.name + "\"}",
"format": "time_series",
"instant": False,
"intervalFactor": 2,
"legendFormat": "Heating Time",
"refId": "B",
"step": 1
}],
"thresholds": [],
"timeFrom":
None,
"timeShift":
None,
"title":
self.name,
"tooltip": {
"shared": True,
"sort": 0,
"value_type": "individual"
},
"type":
"graph",
"xaxis": {
"buckets": None,
"mode": "time",
"name": None,
"show": True,
"values": []
},
"yaxes": [{
"format": "celsius",
"label": None,
"logBase": 1,
"max": None,
"min": None,
"show": True
}, {
"decimals": None,
"format": "percentunit",
"label": "",
"logBase": 1,
"max": None,
"min": None,
"show": True
}]
}]
}]
return panels
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
class vehicle(metaclass=SingletonMeta):
def __init__(self):
log.info("Vehicle start")
self.__ctx = context.context()
self.settings = Settings()
self.__ctx.on_tracker_resolved += self.__tracker_handler
self.__mavlink = MavNode()
self.__steering_pid = None
self.__throttle_pid = None
self.__x_lpf = lpf(factor=0)
self.__y_lpf = lpf(factor=0)
self.__init_pid()
self.__pid_norm_pwm = map_utils(-1, 1, 2000, 1000)
self.__pid_norm_throttle = map_utils(-1, 1, 1100, 1950)
log.info("Vehicle start1")
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 start(self):
t = threading.Thread(target=self.__run)
t.setDaemon(True)
t.setName("VehicleT")
t.start()
def __run(self):
log.info("Vehicle handler start")
self.__mavlink.connect()
while True:
# log.info("vehicle")
time.sleep(1)
def __tracker_handler(self, x, y):
x = self.__x_lpf.update(x)
y = self.__y_lpf.update(y)
self.__steering_pid.update(x)
self.__throttle_pid.update(y)
sterring_pwm = int(
self.__pid_norm_pwm.map_range(self.__steering_pid.output))
throttle_pwm = int(
self.__pid_norm_throttle.map_range(self.__throttle_pid.output))
log.info(
f"Tracker {x},{y}, throttle: {throttle_pwm}, sterring: {sterring_pwm} "
)
self.__mavlink.sticks_controls(sterring_pwm, throttle_pwm)