message_display("W,A,S,D To move")
while True:
ticksA = motors.get_ticksA()
ticksB = motors.get_ticksB()
distances = motors.get_distance(ticksA, ticksB)
encoderA_meters = distances.return_distanceA()
encoderB_meters = distances.return_distanceB()
#encoderA_meters = mA.get_ticks()
#encoderB_meters = mB.get_ticks()
for event in pygame.event.get():
if event.type == pygame.KEYDOWN:
if event.key == pygame.K_w:
motors.driveMotors(100, 100)
message_display("Driving Foward")
if event.key == pygame.K_s:
motors.driveMotors(-100, -100)
message_display("Driving Backward")
if event.key == pygame.K_a:
motors.driveMotors(100, -100)
message_display("Turning Left")
if event.key == pygame.K_d:
motors.driveMotors(-100, 100)
message_display("Turning Right")
if event.type == pygame.KEYUP:
motors.driveMotors(0, 0)
message_display("Stopped")
message_display_lower(
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)
if heading_angle <= 180:
if disable == True:
motors.driveMotors(0, 0)
else:
motors.driveMotors(pid_wheel, robot_speed)
if heading_angle > 180:
if disable == True:
motors.driveMotors(0, 0)
else:
motors.driveMotors(robot_speed, pid_wheel)
if heading_angle == 0:
if disable == True:
motors.driveMotors(0, 0)
else:
motors.driveMotors(40, -40)
cv2.line(img, (320, 480), (320, 380), (0, 0, 255), 2)
cv2.imshow('image', img)
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()
if debug_images == True:
cv2.imshow('image', img)
if state_machine == 2:
img = vs.read()
#img = cv2.imread('saved_images/opencv_image_3.png')