def __init__(self, knownDistance, knownWidth):
self.KNOWN_DISTANCE = knownDistance
self.KNOWN_WIDTH = knownWidth
self.image_path = 'tools/calibration/opencv_image_0.png'
image_calibrate = cv2.imread(self.image_path)
detectYellow = blob.get_blob('yellow', image_calibrate)
c_marker = detectYellow.find_marker()
self.focalLength = (c_marker[1][0] * knownDistance) / knownWidth
if show_fps == True:
frames += 1
current_t = time.time()
if (current_t - initial_t) > 1:
fps = frames
print("fps: {}".format(fps))
initial_t = time.time()
frames = 0
if state == 1:
if state_machine == 1:
img = vs.read()
#img = cv2.imread('saved_images/opencv_image_3.png')
detectYellow = blob.get_blob('yellow', img)
cent_x, cent_y, heading_angle, marker, area = detectYellow.getFeatures(
160, 240)
pid_return = (pid_angle.update(heading_angle))
pid_wheel = int(robot_speed - abs(pid_return))
if debug == True:
detectYellow.drawFeatures()
cv2.putText(img, 'PID OUT: {}'.format(pid_return), (50, 460),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2,
cv2.LINE_AA)
cv2.putText(img, 'PID WHEELS: {}'.format(pid_wheel), (50, 430),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2,
cv2.LINE_AA)
def containAngle360(angle):
if(angle < 1):
angle = angle + 270
elif(angle >= 361):
angle = angle - 270
return angle
while True:
if state == 1:
if state_machine == 1:
img = vs.read()
#img = cv2.imread('saved_images/opencv_image_3.png')
detectYellow = blob.get_blob('yellow', img)
y_cent_x, y_cent_y, y_heading_angle, y_marker, y_area = detectYellow.getFeatures(160,240)
detectRed = blob.get_blob('red', img)
r_cent_x, r_cent_y, r_heading_angle, r_marker, r_area = detectRed.getFeatures(160,240)
if (y_area > 0) and (r_area > 500):
state = 2
elif (y_area > 0) and (r_area < 500):
state_machine = 2
elif (y_area < 0):
if disable == True:
motors.driveMotors(0,0)
else:
motors.driveMotors(40,-40)
#Create the PID object
pid_angle = pid.pidcontrol(mainP, mainI, mainD)
x_axis = []
y_axis = []
heading_axis = []
while True:
if state_machine == 1:
img = vs.read()
#img = cv2.imread('saved_images/opencv_image_3.png')
detectYellow = blob.get_blob('yellow', img)
y_cent_x, y_cent_y, y_heading_angle, y_marker, y_area = detectYellow.getFeatures(
160, 240)
if (y_area > 0):
state_machine = 2
elif (y_area < 0):
if disable == True:
motors.driveMotors(0, 0)
else:
motors.driveMotors(40, -40)
if debug == True:
detectYellow.drawFeatures()
#detectRed.drawFeatures()