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)
rightMotor.setSpeed(0)
old_err = old_err + (obs_sp - percent_makeup)
old_line_err = old_line_err + (line_sp - cx)
# print(obs_motor_speed/3)
time.sleep(dt)
leftMotor.run(Raspi_MotorHAT.BACKWARD)
rightMotor.run(Raspi_MotorHAT.BACKWARD)
obs_motor_speed = abs(obs_motor_speed)
else:
leftMotor.run(Raspi_MotorHAT.FORWARD)
rightMotor.run(Raspi_MotorHAT.FORWARD)
### Initialize the differential between motors ###
line_motor_diff = 0
### If the centroid is not NaN or Inf ###
if not np.isinf(cx) and not np.isnan(cx):
### Calculate the PID value for motor speed differential ###
line_motor_diff = int(calcLinePID(line_sp, cx, old_line_err, LinePIDgains, dt))
### Print values to the terminal ###
print("cx:"+str(cx)+" PID:"+str(line_motor_diff)+"\n")
### Set the left and right motor speed ###
leftMotor.setSpeed(obs_motor_speed-line_motor_diff)
rightMotor.setSpeed(obs_motor_speed+line_motor_diff)
else:
### Set the left and right motor speed ###
leftMotor.setSpeed(0)
rightMotor.setSpeed(0)
### Calculate the running error ###
old_err = old_err + (obs_sp - percent_makeup)
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)
time.sleep(dt)
percent_makeup = avoidObs(frame, px, w, h)
masked = l.color_filter(frame, ind)
roi_img = l.roi(masked)
canny_img = l.canny(roi_img)
hough_img = l.linedetect(canny_img)
# print(l.differentialSlope)
# print("Percent of Screen Taken: "+str(percent_makeup))
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(l.differentialSlope) and not np.isnan(l.differentialSlope):
line_motor_diff = int(
calcLinePID(line_sp, l.differentialSlope, 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)
time.sleep(dt)