Ejemplo n.º 1
0
def main():
    def menu():
        #print what options you have
        print "\n*** OPTIONS ***"
        print "0) Quit           5) Right Kick       10) Walk Left         15) Stand Up from Sit      20) R to S       25)Goalie"
        print "1) Stiffness ON   6) Walk Foward      11) Walk Right        16) Find Red Ball L to R   21) R to L       26) Map"
        print "2) Stiffness OFF  7) Walk Backwards   12) stand up on back  17) Find Red Ball L to S   22) Stop Tracker 27) Kalman Filter"
        print "3) Goalie Pose    8) Turn Left        13) stand up on front 18) Find Red Ball L to L   23) Dive Left    28) Goalie intercepts clockwise"
        print "4) Left Kick      9) Turn Right       14) Sit Down          19) Find Red Ball R to L   24) Dive Right   29) Goalie intercepts counterclockwise"
        print "type what you want him to say in quotations"
        return input ("\nChoose your option(s) as list: ")

    # NOW THE PROGRAM REALLY STARTS, AS CODE IS RUN
    loop = 1
    choice = 0
    while loop == 1:
        choice = menu()
        k=0
        while k < len(choice):
            if choice[k] == 0:
                loop = 0
                break
            elif choice[k] == 1:
                config.stiffnessOn()
            elif choice[k] == 2:
                config.stiffnessOff()
            elif choice[k] == 3:
                goalie.goaliePose()
            elif choice[k] == 4:
                kick.kickLeftFoot()
            elif choice[k] == 5:
                kick.kickRightFoot()
            elif choice[k] == 6:
                walk.ultimateWalkTo(1, 00, 00)
            elif choice[k] == 7:
                walk.ultimateWalkTo(-1, 00, 00)
            elif choice[k] == 8:
                walk.ultimateWalkTo(0,0,2)
            elif choice[k] == 9:
                walk.ultimateWalkTo(0,0,-2)
            elif choice[k] == 10:
                walk.ultimateWalkTo(0, -1, 0)
            elif choice[k] == 11:
                walk.ultimateWalkTo(0, 1, 0)
            elif choice[k] == 12:
                stand.standUp()
            elif choice[k] == 13:
                stand.standUpOnFront()
            elif choice[k] == 14:
                stand.sitDown()
            elif choice[k] == 15:
                stand.standFromSit()
            elif choice[k] == 16:
                track.findRedBallAndKick(1)
            elif choice[k] == 17:
                track.findRedBallAndKick(2)
            elif choice[k] == 18:
                track.findRedBallAndKick(3)
            elif choice[k] == 19:
                track.findRedBallAndKick(4)
            elif choice[k] == 20:
                track.findRedBallAndKick(5)
            elif choice[k] == 21:
                track.findRedBallAndKick(6)
            elif choice[k] == 22:
                track.stopTracker()
            elif choice[k] == 23:
                dive.diveLeft()
            elif choice[k] == 24:
                dive.diveRight()
            elif choice[k] == 25:
                goalie.goalie()
            elif choice[k] == 26:
                mapping.map()
            elif choice[k] == 27:
                kalmanthreaded.kalman().start()
            elif choice[k] == 28:
                goalie.goalieImproved(1)
            elif choice[k] == 29:
                goalie.goalieImproved(0)
            elif choice[k] == 30:
                kalmanthreaded.stop()
            elif isinstance(choice[k], basestring): #this checks to see if the input is a string.
                texttospeechProxy.say(choice[k])    #if it is a string it will make the nao speak it.
            else:
                #this else statement is used to try out new files that you dont want in the comand menu.
                #just type out what you want it to run.
                #example [kalmanthreaded.get('xposition')]
                choice[k]
            k = k+1
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