Ejemplo n.º 1
0
def goalieImproved(option):
    redballposition=track.findRedBall()
    print redballposition
    redballtracker.startTracker()
    if option == 1:
        walk.walkClockwise()
        while redballposition[1]<-.31:
            redballposition= redballtracker.getPosition()
            print "Walking!", redballposition[1]
        redballtracker.stopTracker()
        walk.stop()
    else:
        walk.walkCounterclockwise()
        while redballposition[1] > .31:
            redballposition= redballtracker.getPosition()
            print "counter", redballposition[1]
        redballtracker.stopTracker()
        walk.stop()
    goaliePose()
Ejemplo n.º 2
0
def findRedBallAndKick(option):
    # Set stiffnes ON
    config.stiffnessOn()

    #Make sure top camera is active
    camera.topCamera()

    # Start looking for red ball to track.
    redballtracker.startTracker()
    redballtracker.setWholeBodyOn(True)
    print "Tracking red ball"

    #Store initial red ball positon to variable
    initialredballposition = redballtracker.getPosition()
    print "Initial Position: ", initialredballposition
    cameras = 0 #Top camera
    count = 0
    counter = 0 #Used for Redballposition
    times = 0
    headpitchangle = 1.433
    while redballtracker.isActive(): # runs while loop as long as redballtracker is active
        while redballtracker.getPosition() == initialredballposition:  # Ball lost.
            if redballtracker.isNewData() == False and count == 0: # Ball still lost.true if a new Red Ball was detected since the last getPosition().
                print "Looking for red ball"
                motionProxy.setAngles(['HeadYaw', 'HeadPitch'], [-0.5, headpitchangle], 0.07)
                time.sleep(3)
                count = 1
                cameras = 0 # top camera
                times += 1
            elif redballtracker.isNewData() == False and count == 1:
                motionProxy.setAngles(['HeadYaw', 'HeadPitch'], [0.5, headpitchangle], 0.07)
                time.sleep(3)
                count = 0
                cameras = 1 # bottom camera
                times += 1
            # the next if condition is to look up slightly if he can't find the ball and then when it looks up 3 times
            # it decides turns left and starts all over again.
            if times > 1:
                number = float(times)
                if number % 2 == 1:
                    if headpitchangle < 0:
                        headpitchangle = 1.433
                        walk.ultimateWalkTo(0, 0, 2)
                    elif headpitchangle > .5:
                        headpitchangle = .2
                    else:
                        headpitchangle = headpitchangle - 0.2
                    print headpitchangle
                else:
                    pass
        # Ball found.
        # Store found red ball positon to variable.
        #foundredballposition and foundredballposition 2 are to make sure there are no
        #random jumps so the robot doesn't randomly pick a different thing to go to.
        if counter == 0:
            foundredballposition2 = redballtracker.getPosition()
        walk.ewalk()
        foundredballposition = foundredballposition2
        foundredballposition2 = redballtracker.getPosition()
        print "Found red ball position: ", foundredballposition2
        counter = 1
        if redballtracker.isNewData() == False and count > 1000 or foundredballposition[0]/foundredballposition2[0] > 2: #means it lost the ball
            walk.stop()                                                                                                  #start fresh
            initialredballposition = redballtracker.getPosition()
            count = 0
            counter = 0
        elif cameras == 0 and foundredballposition2[0] < .57 and foundredballposition[0]/foundredballposition2[0] <= 2:
            walk.stop()
            print "I made it"
            walk.ultimateWalkTo(0.28, foundredballposition2[1]/1.5, foundredballposition2[2]/2.1)
            redballtracker.stopTracker()
            print "I made it"
            if option == 1:
                walk.ultimateWalkTo(0, -.01, 0)
                kick.kickLeftFoot()
            elif option == 2:
                walk.ultimateWalkTo(0, -.075, 0)
                kick.kickLeftFoot()
            elif option == 3:
                walk.ultimateWalkTo(0, -.14, 0)
                kick.kickLeftFoot()
            if option == 4:
                walk.ultimateWalkTo(0, .01, 0)
                kick.kickRightFoot()
            elif option == 5:
                walk.ultimateWalkTo(0, .075, 0)
                kick.kickRightFoot()
            elif option == 6:
                walk.ultimateWalkTo(0, .14, 0)
                kick.kickRightFoot()
            config.poseInit()
        elif cameras == 1 and foundredballposition2[0] < .4 and foundredballposition[0]/foundredballposition2[0] <= 2:
            walk.stop()
            print "I made it, bottom camera"
            kick.kickRightFoot()
            redballtracker.stopTracker()
            config.poseInit()
        elif redballtracker.isNewData() == True and foundredballposition2[0] >= .4:
            count = 2

        elif redballtracker.isNewData() == False and count <= 1000:
            count += 1
            print count