def __init__(self, world, foot, historyLength): self._fsrs = [] self._historyLength = historyLength self.dataRunningMedian = running_median(self._historyLength) self.dataRunningMedian.next() self.lastRead = None for location in [x+y for x in ['Front', 'Rear'] for y in ['Left', 'Right']]: var = 'Device/SubDeviceList/%sFoot/FSR/%s/Sensor/Value' % (foot, location) self._fsrs.append( Sensors.Sensor(world, var) )
def __init__(self, name, world, real_length, world_x=None, world_y=None): """ real_length - [cm] real world largest diameter of object. memory, motion - proxies to ALMemory and ALMotion respectively. world_x, world_y - [cm] locations in world coordinate frame. World coordinate frame (reminder) origin is in our team goal center, x axis is towards opposite goal, y axis is to the left (complete a right handed coordinate system). """ super(Locatable, self).__init__(name) self._world = world # cached proxies self._memory = world._memory self._motion = world._motion # longest arc across the object, i.e. a diagonal. self._real_length = real_length # This is the world x coordinate. Our x axis is to the right of our goal # center, and our y is towards the enemy gate^H^H^Hoal. The enemy goal is up. # (screw Ender) # default to estimating everything is in center of field. self.world_x = world_x if world_x is not None else field.MIDFIELD_X self.world_y = world_y if world_y is not None else field.MIDFIELD_Y # TODO - is't wise? self.world_heading = 0.0 # default to pointing towards target goal self.history = RingBuffer(Locatable.HISTORY_SIZE) # stores history for last # This is the player body frame relative bearing. radians. self.bearing = 0.0 self.elevation = 0.0 # This is the player body frame relative distance. centimeters. self.dist = 0.0 self.update_time = 0.0 # time of current update # previous non zero values self.last_bearing = 0.0 self.last_dist = 0.0 self.last_elevation = 0.0 self.last_update_time = 0.0 # time of previous update # Body coordinate system: x is to the right (body is the torso, and generally # the forward walk direction is along the y axis). self.body_x = 0.0 self.body_y = 0.0 # upper barrier on speed, used to remove outliers. cm/sec self.upper_v_limit = 400.0 self.seen = False self.recently_seen = False # Was the object seen within MISSING_FRAMES_MINIMUM self.centered = False # whether the distance from the center is smaller then XXX self.centered_at_pitch_limit = False # centered on X axis, on pitch low limit (looking most upwardly) self.missingFramesCounter = MISSING_FRAMES_MINIMUM # start as "not seen for inifinity". This is close enough. # smoothed variables self.distSmoothed = 0.0 self.distRunningMedian = running_median(3) # TODO: Change to ballEKF/ballLoc? self.distRunningMedian.next() # Vision variables defaults self.centerX = None self.centerY = None self.normalized2_centerX = None self.normalized2_centerY = None self.x = None self.y = None # centered - a copy of some of the values that keeps # the current searcher values for this target. self.centered_self = CenteredLocatable(self)
def __init__(self, world, var, historyLength): super(Sensors.SmoothedSensor, self).__init__(world, var) self._historyLength = historyLength self.dataRunningMedian = running_median(self._historyLength) self.dataRunningMedian.next() self.lastRead = None