def findRedBall(trackstop=threading.Event()): #added the trackstop so that if you want to use this
#in a thread you can stop it.
#This code finds the red ball.
# Set stiffnes ON
config.stiffnessOn()
#Make sure top camera is active
camera.topCamera()
# Start looking for red ball to track.
redballtracker.stopTracker()
redballtracker.startTracker()
redballtracker.setWholeBodyOn(True)
print "Tracking red ball"
#Store initial red ball positon to variable
initialredballposition = redballtracker.getPosition()
print "Initial Position: ", initialredballposition
count = 0
times = 0
headpitchangle = 1.433
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
times += 1
elif redballtracker.isNewData() == False and count == 1:
motionProxy.setAngles(['HeadYaw', 'HeadPitch'], [0.5, headpitchangle], 0.07)
time.sleep(3)
count = 0
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
if trackstop.is_set():
initialredballposition=[-1000,-1000,-1000]
print "stopped at tracking"
print "found red ball"
redballposition=redballtracker.getPosition()
redballtracker.stopTracker()
return redballposition
def alignGoalie():
config.stiffnessOn()
config.poseInit()
#Make sure top camera is active
camera.topCamera()
# Start looking for red ball to track.
redballtracker.stopTracker()
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
foundredballposition = redballtracker.getPosition()
print "Found red ball position: ", foundredballposition
redballtracker.stopTracker()
walk.ultimatewalkto(0, foundredballposition[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
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
