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()