Esempio n. 1
0
def turn_to_sample(u, coords, precached):
    print "turning"
    rospy.loginfo("COORDS[0]: " + str(coords[0]))
    while coords[0] > 40 or coords[0] < -40:
        if coords[0] > 0:
            rospy.loginfo("turn right")
            u.motor_left(0.22)
            u.motor_right(-0.22)
            time.sleep(1)
            u.motor_left(0.0)
            u.motor_right(0.0)
            if precached:
                coords = scan.check_precached(scan_img_left, scan_img_right)
            else:
                coords = scan.check_easy_sample(scan_img_left, scan_img_right)
        elif coords[0] <= 0:
            rospy.loginfo("turn left")
            u.motor_left(-0.22)
            u.motor_right(0.22)
            time.sleep(1)
            u.motor_right(0.0)
            u.motor_left(0.0)
            if precached:
                coords = scan.check_precached(scan_img_left, scan_img_right)
            else:
                coords = scan.check_easy_sample(scan_img_left, scan_img_right)
        while coords is None:
            if precached:
                coords = scan.check_precached(scan_img_left, scan_img_right)
            else:
                coords = scan.check_easy_sample(scan_img_left, scan_img_right)
Esempio n. 2
0
def turn_to_sample(u, coords, precached):
    print "turning"
    rospy.loginfo("COORDS[0]: " + str(coords[0]))
    while coords[0] > 40 or coords[0] < -40:
        if coords[0] > 0:
            rospy.loginfo("turn right")
            u.motor_left(0.22)
            u.motor_right(-0.22)
            time.sleep(1)
            u.motor_left(0.0)
            u.motor_right(0.0)
            if precached:
                coords = scan.check_precached(scan_img_left, scan_img_right)
            else:
                coords = scan.check_easy_sample(scan_img_left, scan_img_right)
        elif coords[0] <= 0:
            rospy.loginfo("turn left")
            u.motor_left(-0.22)
            u.motor_right(0.22)
            time.sleep(1)
            u.motor_right(0.0)
            u.motor_left(0.0)
            if precached:
                coords = scan.check_precached(scan_img_left, scan_img_right)
            else:
                coords = scan.check_easy_sample(scan_img_left, scan_img_right)
        while coords is None:
            if precached:
                coords = scan.check_precached(scan_img_left, scan_img_right)
            else:
                coords = scan.check_easy_sample(scan_img_left, scan_img_right)
Esempio n. 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 = scan.check_precached(scan_img_left, scan_img_right)
       if coords is None:
           coords = scan.check_easy_sample(scan_img_left, scan_img_right)
       else:
           precached = True
   
    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, precached)
Esempio n. 4
0
def test_scan_easy():
    time.sleep(1)
    while not rospy.is_shutdown():
        coords = scan.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)
Esempio n. 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_left', 2, '0.1', rospy.Time.now())
    u('motor_right', 2, '0.1', rospy.Time.now())
 
    while coords is None:
       coords = scan.check_precached(scan_img_left, scan_img_right)
       if coords is None:
           coords = scan.check_easy_sample(scan_img_left, scan_img_right)
       else:
           precached = True
   
    print "saw at thing" 
    # once sample is seen, stop and move arm back
    u('motor_left', 2, '0.0', rospy.Time.now())
    u('motor_right', 2, '0.0', rospy.Time.now())
    time.sleep(1)
    rospy.loginfo("coords: " + str(coords))

    u('arm_target', 2, 'X 0', rospy.Time.now())
    params = 'Y ' + str(u('arm_max', 2, 'Y', rospy.Time.now()))
    u('arm_target', 2, params, rospy.Time.now())
    while u('arm_should_be_moving', 2, 'X', rospy.Time.now()) or u('arm_should_be_moving', 2, 'Y', rospy.Time.now()): pass
    
    return (coords, precached)
Esempio n. 6
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_left', 2, '0.1', rospy.Time.now())
    u('motor_right', 2, '0.1', rospy.Time.now())

    while coords is None:
        coords = scan.check_precached(scan_img_left, scan_img_right)
        if coords is None:
            coords = scan.check_easy_sample(scan_img_left, scan_img_right)
        else:
            precached = True

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

    u('arm_target', 2, 'X 0', rospy.Time.now())
    params = 'Y ' + str(u('arm_max', 2, 'Y', rospy.Time.now()))
    u('arm_target', 2, params, rospy.Time.now())
    while u('arm_should_be_moving', 2, 'X', rospy.Time.now()) or u(
            'arm_should_be_moving', 2, 'Y', rospy.Time.now()):
        pass

    return (coords, precached)
Esempio n. 7
0
def turn_to_sample(u, coords, precached):
    print "turning"
    rospy.loginfo("COORDS[0]: " + str(coords[0]))
    time_turn = 1.2
    while coords[0] > 40 or coords[0] < -40:
       if coords[0] > 0:
          rospy.loginfo("turn right")
          u('motor_left', 2, '0.22', rospy.Time.now())
          u('motor_right', 2, '-0.22', rospy.Time.now())
          time.sleep(time_turn)
          u('motor_left', 2, '0.0', rospy.Time.now())
          u('motor_right', 2, '0.0', rospy.Time.now())
          if precached:
              coords = scan.check_precached(scan_img_left, scan_img_right)
          else:
              coords = scan.check_easy_sample(scan_img_left, scan_img_right)
       elif coords[0] <= 0:
          rospy.loginfo("turn left")
          u('motor_left', 2, '-0.22', rospy.Time.now())
          u('motor_right', 2, '0.22', rospy.Time.now())
          time.sleep(time_turn)
          u('motor_left', 2, '0.0', rospy.Time.now())
          u('motor_right', 2, '0.0', rospy.Time.now())
          if precached:
              coords = scan.check_precached(scan_img_left, scan_img_right)
          else:
              coords = scan.check_easy_sample(scan_img_left, scan_img_right)
       # go forward a bit after each turn
       u('motor_left', 2, '0.1', rospy.Time.now())
       u('motor_right', 2, '0.1', rospy.Time.now())
       time.sleep(2)
       u('motor_left', 2, '0.0', rospy.Time.now())
       u('motor_right', 2, '0.0', rospy.Time.now())

       while coords is None:
          if precached:
              coords = scan.check_precached(scan_img_left, scan_img_right)
          else:
              coords = scan.check_easy_sample(scan_img_left, scan_img_right)
       if time_turn > 0.4:
          time_turn -= 0.1
Esempio n. 8
0
def turn_to_sample(u, coords, precached):
    print "turning"
    rospy.loginfo("COORDS[0]: " + str(coords[0]))
    time_turn = 1.2
    while coords[0] > 40 or coords[0] < -40:
        if coords[0] > 0:
            rospy.loginfo("turn right")
            u('motor_left', 2, '0.22', rospy.Time.now())
            u('motor_right', 2, '-0.22', rospy.Time.now())
            time.sleep(time_turn)
            u('motor_left', 2, '0.0', rospy.Time.now())
            u('motor_right', 2, '0.0', rospy.Time.now())
            if precached:
                coords = scan.check_precached(scan_img_left, scan_img_right)
            else:
                coords = scan.check_easy_sample(scan_img_left, scan_img_right)
        elif coords[0] <= 0:
            rospy.loginfo("turn left")
            u('motor_left', 2, '-0.22', rospy.Time.now())
            u('motor_right', 2, '0.22', rospy.Time.now())
            time.sleep(time_turn)
            u('motor_left', 2, '0.0', rospy.Time.now())
            u('motor_right', 2, '0.0', rospy.Time.now())
            if precached:
                coords = scan.check_precached(scan_img_left, scan_img_right)
            else:
                coords = scan.check_easy_sample(scan_img_left, scan_img_right)
        # go forward a bit after each turn
        u('motor_left', 2, '0.1', rospy.Time.now())
        u('motor_right', 2, '0.1', rospy.Time.now())
        time.sleep(2)
        u('motor_left', 2, '0.0', rospy.Time.now())
        u('motor_right', 2, '0.0', rospy.Time.now())

        while coords is None:
            if precached:
                coords = scan.check_precached(scan_img_left, scan_img_right)
            else:
                coords = scan.check_easy_sample(scan_img_left, scan_img_right)
        if time_turn > 0.4:
            time_turn -= 0.1