def kick(): f = .07 names = list() angles = list() times = list() names.append('RKneePitch') angles.append( [1.0769100189208984, 0.808459997177124, 0.5568840503692627, 0.3467259407043457, 0.556, 0.808, 1.076] ) times.append(factor([1,2,3,4,5,6,7], f)) names.append('RShoulderRoll') angles.append( [-0.5752918720245361, -1.30] ) times.append(factor([1.05,3],f)) motion.angleInterpolation(names, angles, times, True) config.poseInit(motion)
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 map(): config.stiffnessOn() config.poseInit() redballposition = track.findRedBall() mapLocation(localization.getNaoLocation(),(redballposition[0],redballposition[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