def message_display_lower(text):
pygame.draw.rect(screen, black, (0, 125, 400, 75))
largeText = pygame.font.Font('freesansbold.ttf', 15)
TextSurf, TextRect = text_objects(text, largeText)
TextRect.center = ((display_width / 2), ((display_height / 2) + 35))
screen.blit(TextSurf, TextRect)
pygame.display.update()
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)
is_close = 240 - cent_y
if is_close < 50:
in_mm = pc.distance_to_camera(marker[1][0])
if debug == True:
cv2.putText(img, 'DISTANCE: {}'.format(in_mm), (50, 400),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2,
cv2.LINE_AA)
motors.driveMotors(0, 0)
time.sleep(0.5)
travel_distance = in_mm
state_machine = 2
vs.stop()
if state_machine == 2:
initial_ticksA = motors.get_ticksA()
initial_ticksB = motors.get_ticksB()
initial_position = 0.0
state_machine = 3
if state_machine == 3:
ticksA = (motors.get_ticksA()
) - initial_ticksA #attempt to reset stored ticks
ticksB = (motors.get_ticksB()
) - initial_ticksB #attempt to reset stored ticks
distances = motors.get_distance(ticksA, ticksB)
positionA = distances.return_distanceA()
#print("current pos: {}".format(positionA))
motors.driveMotors(30, robot_speed)
if (positionA - initial_position) <= -travel_distance:
motors.driveMotors(0, 0)
time.sleep(0.1)
motors.driveMotors(0,0)
time.sleep(0.5)
#check if the remaining distance to object is more than 20cm
if in_mm > 200:
travel_distance = (in_mm - 200)
else:
travel_distance = 1
state_machine = 3
vs.stop()
if debug_images == True:
cv2.imshow('image', img)
if state_machine == 3:
initial_ticksA = motors.get_ticksA()
initial_ticksB = motors.get_ticksB()
initial_position = 0.0
state_machine = 4
if state_machine == 4:
ticksA = (motors.get_ticksA()) - initial_ticksA #attempt to reset stored ticks
ticksB = (motors.get_ticksB()) - initial_ticksB #attempt to reset stored ticks
distances = motors.get_distance(ticksA,ticksB)
positionA = distances.return_distanceA()
#print("current pos: {}".format(positionA))
motors.driveMotors(30,robot_speed)
if (positionA - initial_position) <= -travel_distance:
motors.driveMotors(0,0)
time.sleep(0.1)
sys.exit()