def trackObject(self):
"""
Method to actually perform the tracking.
Should only be called explicitly from state
methods in TrackingStates.py
"""
#if self.firstFrame():
# self.brain.motion.stopHeadMoves()
(changeX,changeY) = (0.,0.)
# Find the target's angular distance from the center of the screen
# if we have an object, track that
if self.target and \
self.target.on:
changeX = self.target.angleX
changeY = self.target.angleY #the pitch is pos = down
else:
# by default, the change is none
#self.printf( "No object")
return
motionAngles = self.brain.sensors.motionAngles
curPitch = motionAngles[MotionConstants.HeadPitch]
curYaw = motionAngles[MotionConstants.HeadYaw]
maxChange = 13.0
#Warning- no gain is applied currently!
safeChangeX = MyMath.clip(changeX,-maxChange,maxChange )
safeChangeY = MyMath.clip(changeY,-maxChange,maxChange )
newYaw = curYaw + safeChangeX/3
newPitch = curPitch - safeChangeY/3
newYaw = MyMath.clip(newYaw,-80.,80.)
maxSpeed = 2.0
headMove = motion.SetHeadCommand(newYaw,newPitch,
maxSpeed, maxSpeed)
self.brain.motion.setHead(headMove)
def run (self):
'''
Overload the FSA version of run, so we can exectue our own updates
'''
#Each frame we need to update where we are going
#and calculate some facts about how far away we are
#unnecessary?
self.lastDestX,self.lastDestY,self.lastDestH = self.destX,self.destY,self.destH
self.bearingToDest = MyMath.getRelativeBearing(self.brain.my.x,
self.brain.my.y,
self.brain.my.h,
self.destX,
self.destY)
self.distToDest = MyMath.dist(self.brain.my.x,
self.brain.my.y,
self.destX,
self.destY)
self.bearingToDestHeading = self.destH - self.brain.my.h
FSA.FSA.run(self)
def getShootingPos(self,dist):
''' Returns where to position from ball location.
Puts the robot in line with ball and goal center.'''
destX,destY,destH = 0,0,0
m = ((OPP_GOALBOX_TOP_Y-self.brain.ball.y)/(NogginConstants.MIDFIELD_X-self.brain.ball.x))
destX = self.brain.ball.x - math.sqrt(dist**2/(1+m**2))
destY = m*(destX-self.brain.ball.x)+self.brain.ball.y
if m == 0:
destH = 0
else:
destH = MyMath.getRelativeHeading(destX,destY,
self.brain.ball.x,
self.brain.ball.y)
return destX,destY,destH
def tracking(tracker):
''' state askes it's parent (the tracker) for an object or angles to track
while the object is on screen, or angles are passed, we track it.
Otherwise, we continually write the current values into motion via setHeads.
If a sweet move is begun while we are tracking, the current setup is to let
the sweet move conclude and then resume tracking afterward.'''
if tracker.firstFrame():
#supersede anything in the motion queue:
tracker.brain.motion.stopHeadMoves()
if DEBUG: tracker.printf("stopping head moves, first frame",'cyan')
(changeX,changeY) = (0.,0.)
#Find the target's angular distance from the center of the screen
#if we have an object, track that
if tracker.target != None:
if tracker.target.on:
changeX = tracker.target.angleX
changeY = tracker.target.angleY #the pitch is pos = down
else:
if DEBUG : tracker.printf("Missing object this frame",'cyan')
if tracker.target == tracker.brain.ball and \
tracker.target.framesOff > TRACKER_FRAMES_OFF_REFIND_THRESH:
return tracker.goLater(tracker.lastDiffState)
return tracker.stay()
#otherwise, if we have angles, track by those
elif tracker.angleX != None and tracker.angleY !=None:
changeX = tracker.angleX
changeY = tracker.angleY #the pitch is pos = down
else:
#by default, the change is none
tracker.player.printf( "No object")
return tracker.stay()
def getGain(angleToCover):
''' choose our gain by default on how far we need to go'''
#should this be based on distance?
angleToCover = abs(angleToCover)
if angleToCover > 8:
return 0.3*tracker.gain
if angleToCover > 4:
return .2*tracker.gain
if angleToCover > .5:
return .1*tracker.gain
return 0.
xGain = getGain(changeX)
yGain = getGain(changeY)
curPitch = tracker.brain.sensors.motionAngles[MotionConstants.HeadPitch]
curYaw = tracker.brain.sensors.motionAngles[MotionConstants.HeadYaw]
if DEBUG:
print "curHead (%g,%g), gain (%g,%g) change (%g,%g)" %\
(curYaw,curPitch,xGain,yGain,changeX,changeY)
maxChange = 13.0
#Warning- no gain is applied currently!
safeChangeX = MyMath.clip(changeX,-maxChange,maxChange )
safeChangeY = MyMath.clip(changeY,-maxChange,maxChange )
newYaw = curYaw + safeChangeX/3
newPitch = curPitch - safeChangeY/3
if newYaw < 0:
newYaw = max(newYaw,-80.0)
else:
newYaw = min(newYaw,80.0)
if DEBUG:
print "target: x %g, y%g" % (newYaw,newPitch)
maxSpeed = 2.0
#motion.stopHeadMoves()
#headMove = motion.HeadJointCommand(.15,(newYaw,newPitch),1)
headMove = motion.SetHeadCommand(newYaw,newPitch,
maxSpeed, maxSpeed)
tracker.brain.motion.setHead(headMove)
return tracker.stay()
