gray = cv2.cvtColor(roi, cv2.COLOR_BGR2GRAY)
blurred = cv2.GaussianBlur(gray, (5, 5), 0)
edged = cv2.Canny(blurred, 120, 85)
lines = cv2.HoughLinesP(edged, 1, np.pi / 180, 1, 10, 10)
theta = 0
if lines.any():
for x in range(0, len(lines)):
for x1, y1, x2, y2 in lines[x]:
if x1 > xsize * .25 and x2 < xsize * .75:
# print(x1,y1,x2,y2)
cv2.line(edged, (x1, y1), (x2, y2), (0, 255, 0), 2)
theta = theta + math.atan2((y2 - y1), (x2 - x1))
obs_motor_speed = calcOBSPID(obs_sp, percent_makeup, old_err, ObsPIDgains,
dt)
obs_motor_speed = int(obs_motor_speed / 3)
old_err = old_err + (obs_sp - percent_makeup)
if not np.isinf(theta) and not np.isnan(theta):
line_motor_diff = int(
calcLinePID(line_sp, theta, old_line_err, LinePIDgains, dt))
#print(line_motor_diff)
else:
line_motor_diff = int(0)
# print(obs_motor_speed/3)
leftMotor.setSpeed(70 - line_motor_diff)
rightMotor.setSpeed(70 + line_motor_diff)
x, y, w, h = cv2.boundingRect(c)
if w < 50 or h < 50:
w = 0
h = 0
if h and w:
percent_makeup = (h * w) / (480 * 640) * 100
else:
percent_makeup = 0
##########################################################################################
obs_motor_speed = int(
calcOBSPID(obs_sp, percent_makeup, old_err, ObsPIDgains, dt))
if obs_motor_speed > MAX_SPEED:
obs_motor_speed = MAX_SPEED
print("% makeup: " + str(percent_makeup) + " PID Motor Speed: " +
str(obs_motor_speed))
if not np.isinf(cx) and not np.isnan(cx):
line_motor_diff = int(
calcLinePID(line_sp, cx, old_line_err, LinePIDgains, dt))
print("cx:" + str(cx) + " PID:" + str(line_motor_diff) + "\n")
leftMotor.setSpeed(obs_motor_speed - line_motor_diff)
rightMotor.setSpeed(obs_motor_speed + line_motor_diff)
else:
leftMotor.setSpeed(0)
#leftMotor.run(Raspi_MotorHAT.FORWARD)
#rightMotor.run(Raspi_MotorHAT.FORWARD)
ret, frame = cap.read()
xsize = int(frame.shape[1] / 2)
ysize = int(frame.shape[0] / 4)
px = np.array(frame[int(ysize), int(xsize)])
percent_makeup = avoidObs(frame, px, w, h)
print("Percent of Screen Taken: " + str(percent_makeup))
obsPIDvalue = calcOBSPID(obs_sp, percent_makeup, old_err, PIDgains, dt)
obs_motor_speed = int(obsPIDvalue)
old_err = old_err + (obs_sp - percent_makeup)
print(obs_motor_speed)
if obs_motor_speed < 0:
leftMotor.run(Raspi_MotorHAT.BACKWARD)
rightMotor.run(Raspi_MotorHAT.BACKWARD)
else:
leftMotor.run(Raspi_MotorHAT.FORWARD)
rightMotor.run(Raspi_MotorHAT.FORWARD)
# if abs(obs_motor_speed) < 30:
# obs_motor_speed = np.sign(obs_motor_speed)*30