예제 #1
0
def test_forward_until_scanned(u, precached):
    coords = None

    # MOVE FORWARD until sample is seen by scan cam
    u.motor_right(0.1)
    u.motor_left(0.1)
    while coords is None:
        if precached:
            coords = scan2.check_precached(scan_img_left, scan_img_right)
        else:
            coords = scan2.check_easy_sample(scan_img_left, scan_img_right)

    print "saw at thing"
    # once sample is seen, stop and move arm back
    u.motor_left(0.0)
    u.motor_right(0.0)
    time.sleep(1)
    rospy.loginfo("coords: " + str(coords))

    u.arm_target("X", 0)
    u.arm_target("Y", u.arm_max("Y"))
    while u.arm_should_be_moving("X") or u.arm_should_be_moving("Y"):
        pass

    return coords
예제 #2
0
def test_scan_easy():
    time.sleep(3)
    while not rospy.is_shutdown():
        coords = scan2.check_easy_sample(scan_img_left, scan_img_right)
        rospy.loginfo("coords: " + str(coords))
        cv2.imshow('LEFT', scan_img_left)
        cv2.imshow('RIGHT', scan_img_right)
예제 #3
0
def test_forward_until_scanned_both(u):
    coords = None
    precached = False  # is the scanned sample precached or easy?

    # MOVE FORWARD until sample is seen by scan cam
    u.motor_right(0.1)
    u.motor_left(0.1)

    while coords is None:
        coords = scan2.check_easy_sample(scan_img_left, scan_img_right)
        if coords[0] is "Left": print "Turn Left"
        elif coords[0] is "Right": print "Turn Right"

        if coords is None:
            coords = scan2.check_precached(scan_img_left, scan_img_right)

        if coords is not None:
            precached = True

    # once sample is seen, stop and move arm back
    u.motor_left(0.0)
    u.motor_right(0.0)
    time.sleep(1)
    rospy.loginfo("coords: " + str(coords))

    u.arm_target("X", 0)
    u.arm_target("Y", u.arm_max("Y"))
    while u.arm_should_be_moving("X") or u.arm_should_be_moving("Y"):
        pass

    return (coords, precached)
예제 #4
0
def test_scan_easy():
    time.sleep(3)
    while not rospy.is_shutdown():
        coords = scan2.check_easy_sample(scan_img_left, scan_img_right)
        rospy.loginfo("coords: " + str(coords))
        cv2.imshow('LEFT', scan_img_left)
        cv2.imshow('RIGHT', scan_img_right)
예제 #5
0
def test_forward_until_scanned_both(u):
    coords = None
    precached = False # is the scanned sample precached or easy?

    # MOVE FORWARD until sample is seen by scan cam
    u.motor_right(0.1)
    u.motor_left(0.1)

    while coords is None:
       coords = scan2.check_easy_sample(scan_img_left, scan_img_right)
       if coords[0] is "Left": print "Turn Left"
       elif coords[0] is "Right": print "Turn Right"
       
       if coords is None:
          coords = scan2.check_precached(scan_img_left, scan_img_right)
       
       if coords is not None:
          precached = True
   
    # once sample is seen, stop and move arm back
    u.motor_left(0.0)
    u.motor_right(0.0)
    time.sleep(1)
    rospy.loginfo("coords: " + str(coords))

    u.arm_target("X", 0)
    u.arm_target("Y", u.arm_max("Y"))
    while u.arm_should_be_moving("X") or u.arm_should_be_moving("Y"): pass   
    
    return (coords, precached)
예제 #6
0
def test_forward_until_scanned(u, precached):
    coords = None

    # MOVE FORWARD until sample is seen by scan cam
    u.motor_right(0.1)
    u.motor_left(0.1)
    while coords is None:
       if precached:
           coords = scan2.check_precached(scan_img_left, scan_img_right)
       else:
           coords = scan2.check_easy_sample(scan_img_left, scan_img_right)
   
    print "saw at thing" 
    # once sample is seen, stop and move arm back
    u.motor_left(0.0)
    u.motor_right(0.0)
    time.sleep(1)
    rospy.loginfo("coords: " + str(coords))

    u.arm_target("X", 0)
    u.arm_target("Y", u.arm_max("Y"))
    while u.arm_should_be_moving("X") or u.arm_should_be_moving("Y"): pass   
    
    return coords