def __init__(self,
kp: float = 0,
ki: float = 0,
kd: float = 0,
target: float = 0,
upper_limit: float = 0,
lower_limit: float = 0,
update_rate: float = 0,
scalar: float = 0):
"""
:param kp: The proportional coefficient of the PID
:param ki: The integral coefficient of the PID
:param kd: The derivative coefficient of the PID
:param target: The initial target value which the PID will approach
:param upper_limit: The largest value which the PID should output
:param lower_limit: The smallest value which the PID should output
:param update_rate: The (approximate) time in seconds between each call to enable.
:param scalar: A value to offset the result of each update by."""
self.kp = kp
self.ki = ki
self.kd = kd
self.target = target
self.upper_limit = upper_limit
self.lower_limit = lower_limit
self.update_rate = update_rate
self.scalar = scalar
self.pid = simple_pid.PID(Kp=self.kp,
Ki=self.ki,
Kd=self.kd,
setpoint=self.target,
output_limits=(self.lower_limit,
self.upper_limit),
sample_time=self.update_rate,
auto_mode=False)
def __init__(self, set_point, channel, control_sign=1):
self.control_sign = control_sign
self.relay = MyRelay(relay_hub[0], relay_hub[1], channel)
self.thermal = MyThermo(thermal_hub[0], thermal_hub[1], channel)
self.pid = simple_pid.PID()
self.pid.setpoint = set_point
self.pid.tunings = (0.35, 0.1, 0.7)
self.pid.output_limits = (-1, 1)
self.duty_cycle = 0.5
def __init__(self, *args, kp, ki, kd, sample_time, pwm_period_s, **kwargs):
#PID(Kp=1.0, Ki=0.0, Kd=0.0, setpoint=0, sample_time=0.01, output_limits=(None, None), auto_mode=True, proportional_on_measurement=False)
super(PIDThermalController, self).__init__(*args, **kwargs)
self.pid = simple_pid.PID(kp,
ki,
kd,
setpoint=self.max,
output_limits=(0, pwm_period_s))
self.pid.sample_time = sample_time
self.pwm_period_s = pwm_period_s
def init_PIDs(self): # Assuming we call this everytime we update the targets x, y, z = self.target self.PID_X = simple_pid.PID(C.PID_P, C.PID_I, C.PID_D, setpoint=x, sample_time=None) self.PID_Y = simple_pid.PID(C.PID_P, C.PID_I, C.PID_D, setpoint=y, sample_time=None) self.PID_Z = simple_pid.PID(C.PID_P, C.PID_I, C.PID_D, setpoint=z, sample_time=None) self.PID_Y_estimate = simple_pid.PID(C.PID_P, C.PID_I, C.PID_D, setpoint=y, sample_time=None) self.PID_X_estimate = simple_pid.PID(C.PID_P, C.PID_I, C.PID_D, setpoint=x, sample_time=None) self.PID_Z_estimate = simple_pid.PID(C.PID_P, C.PID_I, C.PID_D, setpoint=z, sample_time=None)
def test_pid(P = 0.2, I = 0.0, D= 0.0, L=100):
pid = PID.PID(P, I, D)
pid.SetPoint=0.0
pid.setSampleTime(0.01)
END = L
feedback = 0
feedback_list = []
time_list = []
setpoint_list = []
for i in range(1, END):
pid.update(feedback)
output = pid.output
if pid.SetPoint > 0:
feedback += (output - (1/i))
if i>9:
pid.SetPoint = 1
time.sleep(0.02)
feedback_list.append(feedback)
setpoint_list.append(pid.SetPoint)
time_list.append(i)
time_sm = np.array(time_list)
time_smooth = np.linspace(time_sm.min(), time_sm.max(), 300)
# feedback_smooth = spline(time_list, feedback_list, time_smooth)
# Using make_interp_spline to create BSpline
helper_x3 = make_interp_spline(time_list, feedback_list)
feedback_smooth = helper_x3(time_smooth)
plt.plot(time_smooth, feedback_smooth)
plt.plot(time_list, setpoint_list)
plt.xlim((0, L))
plt.ylim((min(feedback_list)-0.5, max(feedback_list)+0.5))
plt.xlabel('time (s)')
plt.ylabel('PID (PV)')
plt.title('TEST PID')
plt.ylim((1-0.5, 1+0.5))
plt.grid(True)
plt.show()
def get_pid_ctrl(self, Tset, sample_time=15):
"""
Returns a simple_pid PID controller object
"""
# Trying Ku = 70, Tu = 186 s (this is set by LS372 scanning cycle time)
#
# go to https://en.wikipedia.org/wiki/Ziegler%E2%80%93Nichols_method
# for info on Ku and Tu and how they relate to PID gain settings
# if self.is_pid_init:
Ku = 70.
Tu = 186 #s -- this is close to Nyquist, 2 * 90 s update time
# Tu = 90#s
Kp = 0.6 * Ku
Ki = 1.2 * Ku / Tu
Kd = 0.075 * Ku * Tu
self.pid_values = [Kp, Ki, Kp]
# else:
# self.pid_values = pid.components
# Kp, Ki, Kp = self.pid_values
# Ku = Ki * Tu / 1.2
# Tu = Kd / (0.075 * Ku)
#Temperature setpoint must be in Kelvin!
#sample_time is in seconds
stime = self.sample_time if hasattr(self, 'sample_time') \
else sample_time
print(f'PID controller for {Tset*1e3} mK ...')
print(f'PID settings:')
print(f'Ku: {Ku}\tTu: {Tu}')
print(f'Kp: {Kp}\tKi: {Ki}\tKd: {Kd}')
pid = simple_pid.PID(Kp, Ki, Kd, setpoint=Tset, sample_time=stime)
# Set the maximum current
# T_base = 0.013 #K (approx. base temperature)
T_base = self.T_base #K (approx. base temperature)
# Limits the current to 1/3 of the total range?
Max_Current = 1.33*8.373*(Tset-T_base)**(0.720) #mA
print(f'Max Curent: {Max_Current} mA')
pid.output_limits = (0, Max_Current)
self.pid = pid
return pid
def __init__(self, k_x1: dict, k_x2: dict, k_x3: dict, k_x4: dict,
k_x5: dict, state_names: list):
self.k_x1 = k_x1
self.k_x2 = k_x2
self.k_x3 = k_x3
self.k_x4 = k_x4
self.k_x5 = k_x5
self.state_names = state_names
self.ref = []
coefficients_list = [
self.k_x1, self.k_x2, self.k_x3, self.k_x4, self.k_x5
]
# TODO: create a number of simple_pid objects based on the k values
self.controller_dict = {}
for k in coefficients_list:
controller = simple_pid.PID(k['kp'], k['ki'], k['kd'])
self.controller_dict.update({k['state']: controller})
def __init__(self, name: str, ip: str,
vision: mp.Queue, push: mp.Queue, event: mp.Queue,
field_width: float, field_depth: float, timeout: float = 10,
xy_speed: float = 0.4, z_speed: float = 60):
super().__init__(name, None, None, (ip, 0), timeout, True)
self._z_speed = z_speed
self._xy_speed = xy_speed
self._state: KeeperState = KeeperState.WATCHING
self._max_y = field_width / 2.0
self._max_x = field_depth / 2.0
self._vision = vision
self._push = push
self._event = event
self._y_pid: simple_pid.PID = simple_pid.PID(-50, -0.5, -2.5, setpoint=0, sample_time=1.0 / SYSTEM_FREQUENCY, output_limits=(-self._xy_speed, self._xy_speed))
if field_width > field_depth:
self._graph_pixel_size: float = 0.8 * self.GRAPH_SIZE / field_width # pixel per meter
else:
self._graph_pixel_size: float = 0.8 * self.GRAPH_SIZE / field_depth # pixel per meter
self._graph_chassis_width = self._graph_pixel_size * measure.INFANTRY_WIDTH
self._graph_chassis_length = self._graph_pixel_size * measure.INFANTRY_LENGTH
self._graph_ball_radius = int(BALL_ACTUAL_RADIUS * self._graph_pixel_size)
self._graph_base = np.zeros((self.GRAPH_SIZE, self.GRAPH_SIZE, 3), dtype=np.uint8)
cv.rectangle(self._graph_base, self._graph_offset(-0.5 * field_width * self._graph_pixel_size, -0.5 * field_depth * self._graph_pixel_size), self._graph_offset(0.5 * field_width * self._graph_pixel_size, 0.5 * field_depth * self._graph_pixel_size), (255, 0, 0), 4)
# dynamic states
self._position: rm.ChassisPosition = rm.ChassisPosition(0, 0, 0)
self._position_last_seen: Optional[float] = None
self._ball_distances: Optional[Tuple[float, float, float]] = None
self._vision_last_updated: Optional[float] = None
self._ball_last_seen: Optional[float] = None
self._armor_hit_id: Optional[int] = None
self._armor_hit_last_seen: Optional[float] = None
self._last_recenter_time: float = 0
self._cmd = rm.Commander(ip, timeout)
self._cmd.robot_mode(rm.MODE_CHASSIS_LEAD)
self._cmd.gimbal_moveto(pitch=-10)
self._init_state()
def performOpen(self, options={}):
"""Creat the PID controller"""
self.pid = simple_pid.PID(sample_time=None)
def main():
bus = smbus.SMBus(1)
slave_address = 0x07 # Chassis Arduino
u1_ref = 2 # velocity
u2_ref = 0 # heading
u3_ref = 0 # ultrasonics
u_ref = np.array([u1_ref, u2_ref, u3_ref])
kp = np.diag([3, 0.4, 2])
ki = np.diag([0, 0.0, 0.0])
kd = np.diag([0.3, 40.0, .1])
# u1_ref = 0 # velocity
# u2_ref = -60 # heading
# u3_ref = 0 # ultrasonics
# u_ref = np.array([u1_ref, u2_ref, u3_ref])
# kp = np.diag([0, 1, 0])
# ki = np.diag([0, 0.0, 0])
# kd = np.diag([0, 50, 0])
state_estimate = Filter(bus, slave_address)
time.sleep(1)
controller = PID(kp, ki, kd, 3)
test_controller1 = simple_pid.PID(3, 0, .3, setpoint=2)
test_controller2 = simple_pid.PID(0.4, 0, 40, setpoint=0)
test_controller3 = simple_pid.PID(2, 0, .1, setpoint=0)
time_start = time.time()
time_elapsed = 0
while time_elapsed < 15:
state = state_estimate.get_state()
print("State Values")
print(" Encoders: {}, {}\n Velocity: {}, {}\n Ultrason: {}, {}, {}".
format(state[0], state[1], state[2], state[3], state[4],
state[5], state[6]))
print()
u = controller.run_pid(u_ref, state)
u_int = u.astype(int)
u_omega = u_int[0]
u_phi = u_int[1] + u_int[2]
u_omega = set_range(u_omega, -6, 6)
u_phi = set_range(u_phi, -6, 6)
cmd = [u_omega, u_phi]
print("Command: ")
print(cmd)
print()
bytesToSend = ConvertInputToBytes(cmd)
time.sleep(0.2) # delay for wire to settle
bus.write_i2c_block_data(slave_address, 0, bytesToSend)
time_elapsed = time.time() - time_start
# TEST ===========================================
# e_r = state[0]
# e_l = state[1]
# v_r = state[2]
# v_l = state[3]
# ul_r = state[4]
# ul_l = state[6]
# u_test1 = test_controller1(v_r-v_l)
# u_test2 = test_controller2(e_r-e_l)
# u_test3 = test_controller3(ul_r-ul_l)
# print("Test = ")
# print(u_test1)
# print(u_test2)
# print(u_test3)
# print()
u1_ref = 0 # velocity
u2_ref = 0 # heading
u3_ref = 0
u_ref = np.array([u1_ref, u2_ref, u3_ref])
print("SLOW DOWN===============================")
while time_elapsed < 30:
# print("State Estimate:")
state = state_estimate.get_state()
print("State Values")
print(" Encoders: {}, {}\n Velocity: {}, {}\n Ultrason: {}, {}, {}".
format(state[0], state[1], state[2], state[3], state[4],
state[5], state[6]))
print()
u = controller.run_pid(u_ref, state)
u_int = u.astype(int)
u_omega = u_int[0]
u_phi = u_int[1] + u_int[2]
u_omega = set_range(u_omega, -6, 6)
u_phi = set_range(u_phi, -6, 6)
cmd = [u_omega, u_phi]
print("Command: ")
print(cmd)
print()
bytesToSend = ConvertInputToBytes(cmd)
time.sleep(0.04) # delay for wire to settle
bus.write_i2c_block_data(slave_address, 0, bytesToSend)
time_elapsed = time.time() - time_start
def activate(imshow_debug = False, timeout_time = 120.0, pitch_pid_modifier = None,
rotate_pid_modifier = None, communication_on = True, trackbars_on = False,
red = True):
self.imshow_debug = imshow_debug
self.timeout_time = timeout_time
self.pitch_pid_modifier = pitch_pid_modifier
self.rotate_pid_modifier = rotate_pid_modifier
self.communication_on = communication_on
self.trackbars_on = trackbars_on
self.red = red
'''
These are the options that should be in the GUI (defaults are created both in function and in the constructor)
imshow_debug = True this makes it print out all the debug outputs and turns on the images to display on the pi
timeout_time = float this sets the time after activation at which it times out and turns off if it doesn't do anything
pitch_pid_modifier = 4 element list of the form [kp, ki, kd, setpoint]
rotate_pid_modifier = 4 element list of the form [kp, ki, kd, setpoint]
communication_on = True this turns on i2c communication with the arduino
trackbars_on = True this turns on the trackbars and the images for adjusting color (automatically turns on imshow debug as well)
red = True this makes it go after red targets -- False makes it go after green targets
'''
#if args['config_overwrite']:
# args['config_overwrite'] = json.loads(args['config_overwrite'])
#print("Using Arguments=",args)
#imshow_debug = False
#if args['imshow_debug']:
# imshow_debug = True
#
#timeout_time = args['timeout_time']
#if timeout_time is not None:
# timeout_time = float(timeout_time)
#else:
# timeout_time = 150.0
#
#pitch_pid_modifier = args['pitch_pid_modify']
#
#rotate_pid_modifier = args['rotate_pid_modify']
#
#if args['i2c_off']:
# communication_on = False
#else:
# communication_on = True
self.activated = True
if self.trackbars_on:
self.imshow_debug = True
cmd_file = consts.resource_paths.device_cmd_fpath
#if args['dev_debug']:
# cmd_file = ''
# print('depthai will not load cmd file into device.')
labels = []
with open(consts.resource_paths.blob_labels_fpath) as fp:
labels = fp.readlines()
labels = [i.strip() for i in labels]
#print('depthai.__version__ == %s' % depthai.__version__)
#print('depthai.__dev_version__ == %s' % depthai.__dev_version__)
if not depthai.init_device(cmd_file):
print("Error initializing device. Try to reset it.")
exit(1)
#print('Available streams: ' + str(depthai.get_available_steams()))
# Do not modify the default values in the config Dict below directly. Instead, use the `-co` argument when running this script.
config = {
# Possible streams:
# ['left', 'right','previewout', 'metaout', 'depth_sipp', 'disparity', 'depth_color_h']
# If "left" is used, it must be in the first position.
# To test depth use:
#'streams': [{'name': 'depth_sipp', "max_fps": 12.0}, {'name': 'previewout', "max_fps": 12.0}, ],
#'streams': [{'name': 'previewout', "max_fps": 3.0}, {'name': 'depth_mm_h', "max_fps": 3.0}],
'streams': [{'name': 'previewout', "max_fps": 2.0}, {'name': 'metaout', "max_fps": 2.0}],
#'streams': ['metaout', 'previewout'],
'depth':
{
'calibration_file': consts.resource_paths.calib_fpath,
# 'type': 'median',
'padding_factor': 0.3
},
'ai':
{
'blob_file': consts.resource_paths.blob_fpath,
'blob_file_config': consts.resource_paths.blob_config_fpath,
'calc_dist_to_bb': True
},
'board_config':
{
'swap_left_and_right_cameras': True, # True for 1097 (RPi Compute) and 1098OBC (USB w/onboard cameras)
'left_fov_deg': 69.0, # Same on 1097 and 1098OBC
#'left_to_right_distance_cm': 9.0, # Distance between stereo cameras
'left_to_right_distance_cm': 7.5, # Distance between 1098OBC cameras
'left_to_rgb_distance_cm': 2.0 # Currently unused
}
}
#if args['config_overwrite'] is not None:
# config = utils.merge(args['config_overwrite'],config)
# print("Merged Pipeline config with overwrite",config)
if 'depth_sipp' in config['streams'] and ('depth_color_h' in config['streams'] or 'depth_mm_h' in config['streams']):
print('ERROR: depth_sipp is mutually exclusive with depth_color_h')
exit(2)
# del config["streams"][config['streams'].index('depth_sipp')]
stream_names = [stream if isinstance(stream, str) else stream['name'] for stream in config['streams']]
# create the pipeline, here is the first connection with the device
p = depthai.create_pipeline(config=config)
if p is None:
print('Pipeline is not created.')
exit(2)
t_start = time()
time_start = time()
frame_count = {}
frame_count_prev = {}
for s in stream_names:
frame_count[s] = 0
frame_count_prev[s] = 0
entries_prev = []
################## I2C COMMUNICATION SETUP ####################
if self.communication_on:
bus = smbus.SMBus(1)
# I2C address of Arduino Slave
slave_address = 0x08
# I think this is the register we're trying to read out of/into?
i2c_cmd = 0x01
################## ADDED FOR COLOR DETECTION CWM ####################
if self.imshow_debug:
cv2.namedWindow('g_image')
cv2.namedWindow('r_image')
if self.trackbars_on:
cv2.namedWindow('r1_sliders')
cv2.namedWindow('r2_sliders')
cv2.namedWindow('g_sliders')
# white blank image
blank_image = 255 * np.ones(shape=[10, 256, 3], dtype=np.uint8)
thrs=50
# set default slider values
thresholdValue = 15
r1LowHue = 0
r1LowSat = 145
r1LowVal = 145
r1UpHue = 10
r1UpSat = 255
r1UpVal = 255
r2LowHue = 170
r2LowSat = 160
r2LowVal = 135
r2UpHue = 179
r2UpSat = 255
r2UpVal = 255
gLowHue = 30
gLowSat = 50
gLowVal = 60
# gLowSat = 120
# gLowVal = 70
gUpHue = 85
gUpSat = 245
gUpVal = 255
#cv2.createTrackbar('Hue', 'image', 80, 179, nothing)
#cv2.createTrackbar('Sat', 'image', 127, 255, nothing)
#cv2.createTrackbar('Val', 'image', 222, 255, nothing)
if self.trackbars_on:
cv2.createTrackbar('filterThresh', 'r1_sliders', thresholdValue, 100, nothing)
cv2.createTrackbar('r1LowHue', 'r1_sliders', r1LowHue, 179, nothing)
cv2.createTrackbar('r1LowSat', 'r1_sliders', r1LowSat, 255, nothing)
cv2.createTrackbar('r1LowVal', 'r1_sliders', r1LowVal, 255, nothing)
cv2.createTrackbar('r1UpHue', 'r1_sliders', r1UpHue, 179, nothing)
cv2.createTrackbar('r1UpSat', 'r1_sliders', r1UpSat, 255, nothing)
cv2.createTrackbar('r1UpVal', 'r1_sliders', r1UpVal, 255, nothing)
cv2.createTrackbar('r2LowHue', 'r2_sliders', r2LowHue, 179, nothing)
cv2.createTrackbar('r2LowSat', 'r2_sliders', r2LowSat, 255, nothing)
cv2.createTrackbar('r2LowVal', 'r2_sliders', r2LowVal, 255, nothing)
cv2.createTrackbar('r2UpHue', 'r2_sliders', r2UpHue, 179, nothing)
cv2.createTrackbar('r2UpSat', 'r2_sliders', r2UpSat, 255, nothing)
cv2.createTrackbar('r2UpVal', 'r2_sliders', r2UpVal, 255, nothing)
cv2.createTrackbar('gLowHue', 'g_sliders', gLowHue, 179, nothing)
cv2.createTrackbar('gLowSat', 'g_sliders', gLowSat, 255, nothing)
cv2.createTrackbar('gLowVal', 'g_sliders', gLowVal, 255, nothing)
cv2.createTrackbar('gUpHue', 'g_sliders', gUpHue, 179, nothing)
cv2.createTrackbar('gUpSat', 'g_sliders', gUpSat, 255, nothing)
cv2.createTrackbar('gUpVal', 'g_sliders', gUpVal, 255, nothing)
## red ball mask areas
#red_mask_1 = cv2.inRange(im_hsv, (0, 120, 70), (10, 255, 255))
#red_mask_2 = cv2.inRange(im_hsv, (170, 120, 70), (179, 255, 255))
#lower_red1 = np.array([0, 120, 100])
#upper_red1 = np.array([10, 255, 255])
#lower_red2 = np.array([170, 120, 100])
#upper_red2 = np.array([179, 255, 255])
#green mask area centered around
# (80, 127, 222)
#green_mask = cv2.inRange(im_hsv, (55, 120, 70), (105, 255, 255))
# lower_green = np.array([40, 10, 200])
# upper_green = np.array([120, 245, 255])
#lower_green = np.array([55, 120, 70])
#upper_green = np.array([105, 255, 255])
#sets how much to blur
filt=39
exitNow=0
pause=0
####################################################################
####################################################################
#Setting up the targeting program:
p_kp = 0.1
p_ki = 0.0
p_kd = 0.01
yref = 150.0
r_kp = 0.1
r_ki = 0.0
r_kd = 0.01
xref = 150.0
bad_guy_center = None
if self.pitch_pid_modifier is not None:
ppid_args_length = len(self.pitch_pid_modifier)
if ppid_args_length is 4:
try:
p_kp = float(self.pitch_pid_modifier[0])
p_ki = float(self.pitch_pid_modifier[1])
p_kd = float(self.pitch_pid_modifier[2])
yref = float(self.pitch_pid_modifier[3])
except:
print("PITCH ARGUMENT ERROR -- INCORRECT PITCH PID ARGUMENTS SYNTAX -- RETURNING TO DEFAULT VALUES")
yref = 150.0
p_kp = 0.1
p_ki = 0.0
p_kd = 0.01
else:
print("ERROR -- INCORRECT LENGTH OF PITCH PID ARGUMENTS, LENGTH SHOULD BE 4, BUT LENGTH WAS: " + str(ppid_args_length))
if self.rotate_pid_modifier is not None:
rpid_args_length = len(self.rotate_pid_modifier)
if rpid_args_length is 4:
try:
r_kp = float(self.rotate_pid_modifier[0])
r_ki = float(self.rotate_pid_modifier[1])
r_kd = float(self.rotate_pid_modifier[2])
xref = float(self.rotate_pid_modifier[3])
except:
print("ROTATE ARGUMENT ERROR -- INCORRECT ROTATE PID ARGUMENTS SYNTAX -- RETURNING TO DEFAULT VALUES")
xref = 150.0
r_kp = 0.1
r_ki = 0.0
r_kd = 0.01
else:
print("ERROR -- INCORRECT LENGTH OF ROTATE PID ARGUMENTS, LENGTH SHOULD BE 4, BUT LENGTH WAS: " + str(rpid_args_length))
list_of_pid_params = [p_kp, p_ki, p_kd, yref, r_kp, r_ki, r_kd, xref]
if self.imshow_debug:
print(list_of_pid_params)
####################################################################
while True:
# retreive data from the device
# data is stored in packets, there are nnet (Neural NETwork) packets which have additional functions for NNet result interpretation
nnet_packets, data_packets = p.get_available_nnet_and_data_packets()
for i, nnet_packet in enumerate(nnet_packets):
# the result of the MobileSSD has detection rectangles (here: entries), and we can iterate threw them
for i, e in enumerate(nnet_packet.entries()):
# for MobileSSD entries are sorted by confidence
# {id == -1} or {confidence == 0} is the stopper (special for OpenVINO models and MobileSSD architecture)
if e[0]['id'] == -1.0 or e[0]['confidence'] == 0.0:
break
if i == 0:
entries_prev.clear()
# save entry for further usage (as image package may arrive not the same time as nnet package)
entries_prev.append(e)
for packet in data_packets:
if packet.stream_name not in stream_names:
continue # skip streams that were automatically added
elif packet.stream_name == 'previewout':
data = packet.getData()
# the format of previewout image is CHW (Chanel, Height, Width), but OpenCV needs HWC, so we
# change shape (3, 300, 300) -> (300, 300, 3)
data0 = data[0,:,:]
data1 = data[1,:,:]
data2 = data[2,:,:]
frame = cv2.merge([data0, data1, data2])
####################### ADDED FOR COLOR DETECTION CWM #######################
try:
imgInit = frame
imgBGR = cv2.resize(imgInit,(300, 300),cv2.INTER_AREA)
r_imgBGR = imgBGR.copy()
g_imgBGR = imgBGR.copy()
img=cv2.cvtColor(imgBGR, cv2.COLOR_BGR2HSV)
if self.trackbars_on:
thresholdValue = cv2.getTrackbarPos('filterThresh', 'r1_sliders')
r1LowHue = cv2.getTrackbarPos('r1LowHue', 'r1_sliders')
r1LowSat = cv2.getTrackbarPos('r1LowSat', 'r1_sliders')
r1LowVal = cv2.getTrackbarPos('r1LowVal', 'r1_sliders')
r1UpHue = cv2.getTrackbarPos('r1UpHue', 'r1_sliders')
r1UpSat = cv2.getTrackbarPos('r1UpSat', 'r1_sliders')
r1UpVal = cv2.getTrackbarPos('r1UpVal', 'r1_sliders')
r2LowHue = cv2.getTrackbarPos('r2LowHue', 'r2_sliders')
r2LowSat = cv2.getTrackbarPos('r2LowSat', 'r2_sliders')
r2LowVal = cv2.getTrackbarPos('r2LowVal', 'r2_sliders')
r2UpHue = cv2.getTrackbarPos('r2UpHue', 'r2_sliders')
r2UpSat = cv2.getTrackbarPos('r2UpSat', 'r2_sliders')
r2UpVal = cv2.getTrackbarPos('r2UpVal', 'r2_sliders')
gLowHue = cv2.getTrackbarPos('gLowHue', 'g_sliders')
gLowSat = cv2.getTrackbarPos('gLowSat', 'g_sliders')
gLowVal = cv2.getTrackbarPos('gLowVal', 'g_sliders')
gUpHue = cv2.getTrackbarPos('gUpHue', 'g_sliders')
gUpSat = cv2.getTrackbarPos('gUpSat', 'g_sliders')
gUpVal = cv2.getTrackbarPos('gUpVal', 'g_sliders')
lower_red1 = np.array([r1LowHue, r1LowSat, r1LowVal])
upper_red1 = np.array([r1UpHue, r1UpSat, r1UpVal])
lower_red2 = np.array([r2LowHue, r2LowSat, r2LowVal])
upper_red2 = np.array([r2UpHue, r2UpSat, r2UpVal])
lower_green = np.array([gLowHue, gLowSat, gLowVal])
upper_green = np.array([gUpHue, gUpSat, gUpVal])
red1 = [[[upper_red1[0]-((upper_red1[0]-lower_red1[0])/2),upper_red1[1]-((upper_red1[1]-lower_red1[1])/2),upper_red1[2]-((upper_red1[2]-lower_red1[2])/2)]]]
red2 = [[[upper_red2[0]-((upper_red2[0]-lower_red2[0])/2),upper_red2[1]-((upper_red2[1]-lower_red2[1])/2),upper_red2[2]-((upper_red2[2]-lower_red2[2])/2)]]]
green = [[[upper_green[0]-((upper_green[0]-lower_green[0])/2),upper_green[1]-((upper_green[1]-lower_green[1])/2),upper_green[2]-((upper_green[2]-lower_green[2])/2)]]]
red1=np.array(red1, dtype="uint8")
red2=np.array(red2, dtype="uint8")
green=np.array(green, dtype="uint8")
redColor1 = cv2.cvtColor(red1, cv2.COLOR_HSV2BGR)##Tr
redColor2 = cv2.cvtColor(red2, cv2.COLOR_HSV2BGR)##Tr
greenColor = cv2.cvtColor(green, cv2.COLOR_HSV2BGR)##Tr
gmask=np.uint8(cv2.inRange(img,lower_green,upper_green))
rmask1=np.uint8(cv2.inRange(img,lower_red1,upper_red1))
rmask2=np.uint8(cv2.inRange(img,lower_red2,upper_red2))
rmask = rmask1+rmask2
rvis = np.uint8(img.copy())
gvis = np.uint8(img.copy())
rvis[rmask==0]=(0,0,0)
gvis[gmask==0]=(0,0,0)
rgray = rvis[:,:,2]
ggray = gvis[:,:,2]
rblurred = cv2.GaussianBlur(rgray, (filt, filt), 0)
gblurred = cv2.GaussianBlur(ggray, (filt, filt), 0)
rthresh = cv2.threshold(rblurred, thresholdValue, 255, cv2.THRESH_BINARY)[1]
gthresh = cv2.threshold(gblurred, thresholdValue, 255, cv2.THRESH_BINARY)[1]
red_locations = np.where(rthresh == [255])
green_locations = np.where(gthresh == [255])
x_red_avg = np.mean(red_locations[1])
y_red_avg = np.mean(red_locations[0])
x_green_avg = np.mean(green_locations[1])
y_green_avg = np.mean(green_locations[0])
###### current position that we want to drive to the reference by moving the turret! ######
if self.red:
bad_guy_center = (x_red_avg, y_red_avg)
else:
bad_guy_center = (x_green_avg, y_red_avg)
###### ######
if self.imshow_debug:
ccr1=(int(redColor1[0][0][0]),int(redColor1[0][0][1]),int(redColor1[0][0][2]))
ccr2=(int(redColor2[0][0][0]),int(redColor2[0][0][1]),int(redColor2[0][0][2]))
ccg=(int(greenColor[0][0][0]),int(greenColor[0][0][1]),int(greenColor[0][0][2]))
cv2.circle(r_imgBGR, (25, 25), 20, ccr1, -1)
cv2.circle(r_imgBGR, (70, 25), 20, ccr2, -1)
cv2.circle(g_imgBGR, (25, 25), 20, ccg, -1)
rvisBGR=cv2.cvtColor(rvis, cv2.COLOR_HSV2BGR)
gvisBGR=cv2.cvtColor(gvis, cv2.COLOR_HSV2BGR)
rthresh = cv2.cvtColor(rthresh, cv2.COLOR_GRAY2BGR)
gthresh = cv2.cvtColor(gthresh, cv2.COLOR_GRAY2BGR)
if green_locations[0].size != 0:
green_left = np.amin(green_locations[1])
green_right = np.amax(green_locations[1])
green_top = np.amin(green_locations[0])
green_bottom = np.amax(green_locations[0])
if self.imshow_debug:
gvisBGR = cv2.rectangle(gvisBGR, (green_left, green_top), (green_right, green_bottom), (0, 255, 0), 2)
gvisBGR = cv2.putText(gvisBGR, 'GOOD GUY', (green_left, green_top-5), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1, cv2.LINE_AA)
gvisBGR = cv2.putText(gvisBGR, "Center:", (green_left, green_bottom+15), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1, cv2.LINE_AA)
gvisBGR = cv2.putText(gvisBGR, "x = " + str(round(x_green_avg)), (green_left, green_bottom+30), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 1, cv2.LINE_AA)
gvisBGR = cv2.putText(gvisBGR, "y = " + str(round(y_green_avg)), (green_left, green_bottom+45), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 1, cv2.LINE_AA)
if red_locations[0].size != 0:
red_left = np.amin(red_locations[1])
red_right = np.amax(red_locations[1])
red_top = np.amin(red_locations[0])
red_bottom = np.amax(red_locations[0])
if self.imshow_debug:
rvisBGR = cv2.rectangle(rvisBGR, (red_left, red_top), (red_right, red_bottom), (0, 0, 255), 2)
rvisBGR = cv2.putText(rvisBGR, 'BAD GUY', (red_left, red_top-5), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 1, cv2.LINE_AA)
rvisBGR = cv2.putText(rvisBGR, "Center:", (red_left, red_bottom+15), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 1, cv2.LINE_AA)
rvisBGR = cv2.putText(rvisBGR, "x = " + str(round(x_red_avg)), (red_left, red_bottom+30), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 1, cv2.LINE_AA)
rvisBGR = cv2.putText(rvisBGR, "y = " + str(round(y_red_avg)), (red_left, red_bottom+45), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 1, cv2.LINE_AA)
if self.imshow_debug:
cv2.imshow('r_image',np.hstack([r_imgBGR, rthresh, rvisBGR])) #np.hstack([original, vis]))#np.hstack([thresh, gray2]))
cv2.imshow('g_image',np.hstack([g_imgBGR, gthresh, gvisBGR])) #np.hstack([original, vis]))#np.hstack([thresh, gray2]))
if self.trackbars_on:
cv2.imshow('g_sliders', blank_image)
cv2.imshow('r1_sliders', blank_image)
cv2.imshow('r2_sliders', blank_image)
# cv2.imshow('image',np.hstack([imgBGR, visBGR])) #np.hstack([original, vis]))#np.hstack([thresh, gray2]))
except KeyboardInterrupt:
raise
except cv2.error as e:
print("Here it is \n",str(e), "\n")
if similar(str(e), " /home/pi/opencv-3.3.0/modules/imgproc/src/imgwarp.cpp:3483: error: (-215) ssize.width > 0 && ssize.height > 0 in function resize")>.8:
print("\n\n\n\n Your video appears to have ended\n\n\n")
break
###########################################################################
img_h = frame.shape[0]
img_w = frame.shape[1]
confidences = []
#cwm: Takes highest confidence value and selects only that object
for e in entries_prev:
confidences.append(e[0]['confidence'])
# print(confidences)
new_entries_prev = entries_prev
if confidences:
index_of_max_confidence = np.argmax(confidences)
new_entries_prev = []
new_entries_prev.append(entries_prev[index_of_max_confidence])
# iterate through pre-saved entries & draw rectangle & text on image:
for e in new_entries_prev:
# the lower confidence threshold - the more we get false positives
if e[0]['confidence'] > 0.5:
x1 = int(e[0]['left'] * img_w)
y1 = int(e[0]['top'] * img_h)
pt1 = x1, y1
pt2 = int(e[0]['right'] * img_w), int(e[0]['bottom'] * img_h)
if self.imshow_debug:
cv2.rectangle(frame, pt1, pt2, (0, 0, 255))
# Handles case where TensorEntry object label = 7552.
if e[0]['label'] > len(labels):
print("Label index=",e[0]['label'], "is out of range. Not applying text to rectangle.")
else:
pt_t1 = x1, y1 + 20
if self.imshow_debug:
cv2.putText(frame, labels[int(e[0]['label'])], pt_t1, cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 2)
print('\n')
print(labels[int(e[0]['label'])])
pt_t2 = x1, y1 + 40
if self.imshow_debug:
cv2.putText(frame, '{:.2f}'.format(100*e[0]['confidence']) + ' %', pt_t2, cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255))
print('{:.2f}'.format(100*e[0]['confidence']) + ' %')
if config['ai']['calc_dist_to_bb']:
pt_t3 = x1, y1 + 60
pt_t4 = x1, y1 + 80
pt_t5 = x1, y1 + 100
if self.imshow_debug:
cv2.putText(frame, 'x:' '{:7.3f}'.format(e[0]['distance_x']) + ' m', pt_t3, cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255))
cv2.putText(frame, 'y:' '{:7.3f}'.format(e[0]['distance_y']) + ' m', pt_t4, cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255))
cv2.putText(frame, 'z:' '{:7.3f}'.format(e[0]['distance_z']) + ' m', pt_t5, cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255))
print('x:' '{:7.3f}'.format(e[0]['distance_x']) + ' m')
print('y:' '{:7.3f}'.format(e[0]['distance_y']))
print('z:' '{:7.3f}'.format(e[0]['distance_z']))
if self.imshow_debug:
print('----------frame over ----------')
if self.imshow_debug:
cv2.putText(frame, "fps: " + str(frame_count_prev[packet.stream_name]), (25, 50), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 0))
cv2.imshow('previewout', frame)
elif packet.stream_name == 'left' or packet.stream_name == 'right' or packet.stream_name == 'disparity':
frame_bgr = packet.getData()
if self.imshow_debug:
cv2.putText(frame_bgr, packet.stream_name, (25, 25), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 0))
cv2.putText(frame_bgr, "fps: " + str(frame_count_prev[packet.stream_name]), (25, 50), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 0))
cv2.imshow(packet.stream_name, frame_bgr)
elif packet.stream_name.startswith('depth'):
frame = packet.getData()
#print(frame)
if self.imshow_debug:
if len(frame.shape) == 2:
if frame.dtype == np.uint8: # grayscale
cv2.putText(frame, packet.stream_name, (25, 25), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 255))
cv2.putText(frame, "fps: " + str(frame_count_prev[packet.stream_name]), (25, 50), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 255))
cv2.imshow(packet.stream_name, frame)
else: # uint16
#print("frame before integer division")
#print(frame)
frame = (65535 // frame).astype(np.uint8)
#print("frame after integer division")
#print(frame)
#colorize depth map, comment out code below to obtain grayscale
frame = cv2.applyColorMap(frame, cv2.COLORMAP_HOT)
# frame = cv2.applyColorMap(frame, cv2.COLORMAP_JET)
cv2.putText(frame, packet.stream_name, (25, 25), cv2.FONT_HERSHEY_SIMPLEX, 1.0, 255)
cv2.putText(frame, "fps: " + str(frame_count_prev[packet.stream_name]), (25, 50), cv2.FONT_HERSHEY_SIMPLEX, 1.0, 255)
cv2.imshow(packet.stream_name, frame)
else: # bgr
cv2.putText(frame, packet.stream_name, (25, 25), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (255, 255, 255))
cv2.putText(frame, "fps: " + str(frame_count_prev[packet.stream_name]), (25, 50), cv2.FONT_HERSHEY_SIMPLEX, 1.0, 255)
cv2.imshow(packet.stream_name, frame)
frame_count[packet.stream_name] += 1
# This is the end of the "while loop" -- pid and command sending will likely work here.
#################### PID AND COMMUNICATIONS #################################################
# compute new ouput from the PID according to the systems current value
# control = pid(v)
if bad_guy_center is not None:
bad_guy_x = bad_guy_center[0]
bad_guy_y = bad_guy_center[1]
pitch_controller = simple_pid.PID(p_kp, p_ki, p_kd, setpoint=yref)
pitch_command = round(pitch_controller(bad_guy_y), 2)
p_cmd = "{:.2f}".format(pitch_command)
rotate_controller = simple_pid.PID(r_kp, r_ki, r_kd, setpoint=xref)
rotate_command = round(rotate_controller(bad_guy_x), 2)
r_cmd = "{:.2f}".format(rotate_command)
# if commands go to within some range, send command to fire
if abs(pitch_command) < 0.5 and abs(rotate_command) < 0.5:
p_cmd = "f"
r_cmd = "f"
############## FIX COMMANDS TODO FOR SCOTT ################
############## MAKE I2C WORK ENCODERS ################
self.pitch_value = p_cmd
self.rotate_value = r_cmd
if self.imshow_debug:
print("pitch command = " + p_cmd)
print("rotation command = " + r_cmd)
# Then send commands to arduino over i2c wire or USB
if self.communication_on:
bytesToSend = ConvertStringToBytes(r_cmd + ',' + p_cmd)
print(bytesToSend)
bus.write_i2c_block_data(slave_address, i2c_cmd, bytesToSend)
#########################################################################################
t_curr = time()
if t_start + 1.0 < t_curr:
t_start = t_curr
for s in stream_names:
frame_count_prev[s] = frame_count[s]
frame_count[s] = 0
if cv2.waitKey(1) == ord('q'):
break
if t_curr - time_start > self.timeout_time:
print('########################################################')
print('TIMED OUT')
print('########################################################')
break
del p # in order to stop the pipeline object should be deleted, otherwise device will continue working. This is required if you are going to add code after the main loop, otherwise you can ommit it.
cv2.destroyAllWindows()
print('py: DONE.')
self.activated = False
def __init__(self,
outer_temps,
inner_temps,
info_temps,
thermistor_divider_r,
thermistor_applied_v_ch,
control_period,
outer_avg_duration,
pwm_channel,
pwm_period,
init_pwm_max,
init_pid_tunings,
results_callback=None,
):
# daemon thread so it shuts down when program ends.
threading.Thread.__init__(self, daemon=True)
# save all of the function parameters
self.outer_temps = outer_temps
self.inner_temps = inner_temps
self.info_temps = info_temps
self.thermistor_divider_r = thermistor_divider_r
self.thermistor_applied_v_ch = thermistor_applied_v_ch
self.control_period = control_period
self.outer_avg_duration = outer_avg_duration
self.pwm_channel = pwm_channel
self.pwm_period = pwm_period
self.results_callback = results_callback
analog_channel_list = []
self.outer_thermistors = []
self.inner_thermistors = []
self.info_thermistors = []
for label, channel, thermistor_type in outer_temps:
analog_channel_list.append((channel, True))
self.outer_thermistors.append(
Thermistor(thermistor_type, channel, thermistor_applied_v_ch, thermistor_divider_r, label)
)
for label, channel, thermistor_type in inner_temps:
analog_channel_list.append((channel, True))
self.inner_thermistors.append(
Thermistor(thermistor_type, channel, thermistor_applied_v_ch, thermistor_divider_r, label)
)
for label, channel, thermistor_type in info_temps:
analog_channel_list.append((channel, True))
self.info_thermistors.append(
Thermistor(thermistor_type, channel, thermistor_applied_v_ch, thermistor_divider_r, label)
)
# Added in the thermistor applied voltage channel to the channel list. It
# has good source impedance so does not need long settling.
analog_channel_list.append((thermistor_applied_v_ch, False))
# create and open the Labjack U3
self.lj_dev = U3protected()
# make the PWM controller and start it
self.pwm = PWM(self.lj_dev, pwm_channel, pwm_period)
self.pwm.start()
# make and start the analog channel reader
self.an_reader = AnalogReader(
self.lj_dev,
analog_channel_list,
ANALOG_READ_SPACING,
ANALOG_RING_BUFFER_SIZE
)
self.an_reader.start()
# delay to get at least the first reading in the readings ring buffer.
time.sleep(0.5)
# Make the PID controller object and set it's initial values
self.pid = simple_pid.PID()
self.pid_tunings = init_pid_tunings
self.pwm_max = init_pwm_max
self.enable_on_off_control = False
self.enable_control = False
Tu = 186#s
Kp = 0.6*Ku
Ki = 1.2*Ku/Tu
Kd = 0.075*Ku*Tu
#Temperature setpoint must be in Kelvin!
#sample_time is in seconds
sample_time = 15
# Iterate over a list of temperatures
Tstart = 0.03; Tstop = 0.3; NT = 28;
T_sets = [Tstart] # np.linspace(Tstart, Tstop, NT)
for T_set in T_sets:
print(f'PID controller for {T_set*1e3} mK ...')
# T_set = 0.03
pid = simple_pid.PID(Kp, Ki, Kd, setpoint = T_set, sample_time =
sample_time)
# pid.sample_time = 16
#it is important that pid.sample_time is set to the same value
#that we wait before calling pid() again
#
#I may modify the simple-pid code to make it wait dt = sample_time
#before returning a new output
#There is a power law relating Current and equilibrium Temperature
#minus T_base
# i.e. $T-T_base \prop I^n$ for some n
#I am inverting this equation and limiting the max current at 33% above
#this equilibrium value. This is to avoid massive overshoot since
#time-resolution of the CMN is poor compared to the timescale at which
#the temperature increases. #This data was taken with the still heater
def setup_control(self):
self.pids = []
self.pids.append(simple_pid.PID(1, 0.05, setpoint=2.5))
self.pids.append(simple_pid.PID(1, 0.2, setpoint=2.5))
return
frame_count[packet.stream_name] += 1
# This is the end of the "while loop" -- pid and command sending will likely work here.
#################### PID AND COMMUNICATIONS #################################################
# compute new ouput from the PID according to the systems current value
# control = pid(v)
if bad_guy_center is not None and not math.isnan(bad_guy_center[0]) and not math.isnan(bad_guy_center[1]):
bad_guy_x = bad_guy_center[0]
bad_guy_y = bad_guy_center[1]
pitch_controller = simple_pid.PID(p_kp, p_ki, p_kd, setpoint=yref)
pitch_command = round(pitch_controller(bad_guy_y), 2)
rotate_controller = simple_pid.PID(r_kp, r_ki, r_kd, setpoint=xref)
rotate_command = round(rotate_controller(bad_guy_x), 2)
if imshow_debug:
print("pitch_command = " + str(pitch_command))
print("rotate_command = " + str(rotate_command))
f_cmd = 0
# if commands go to within some range, send command to fire
if abs(pitch_command) < 0.25 and abs(rotate_command) < 0.25:
f_cmd = 1
pitch_command = 0
rotate_command = 0
import pigpio
import time
import signal
import sys
import os
import simple_pid
pi = pigpio.pi()
pid = simple_pid.PID()
# Configuration
FAN_PIN = 18 # BCM pin used to drive PWM fan
WAIT_TIME = 1 # [s] Time to wait between each refresh
pi.set_PWM_frequency(FAN_PIN, 20000)
pi.set_PWM_range(FAN_PIN, 100)
print("PWM Frequency: (2000?):", pi.get_PWM_frequency(FAN_PIN))
pid.sample_time = WAIT_TIME
pid.output_limits = (0, 100)
# Configurable temperature and fan speed
MIN_TEMP = 40
MAX_TEMP = 60
FAN_LOW = 1
FAN_HIGH = 100
FAN_OFF = 0
FAN_MAX = 100
pid.setpoint = 35
pid.tunings = (-4.0, 0.5, 0.1)
# Get CPU's temperature
t2.start()
def threadingbtn1():
t1 = threading.Thread(target=btn1clicked)
t1.setDaemon(True)
t1.start()
def threading_runpumpspeed1_val():
t3 = threading.Thread(target=runpumpspeed1_val)
t3.setDaemon(True)
t3.start()
# PID
pid = simple_pid.PID(PGain, IGain, DGain, setpoint=SetPoint1) # PID block that takes input from flow meter
def run_pump1():
while pump1status:
output1 = pid(current_value)
if not pump1status:
break
# Buttons
def btn1clicked():
pump1status = True
