def __init__(self, sim, config): self.sim = sim self.config = config # Note: this is passed in, and is located at sim.pod.forces.aero in the config file # @see http://www.softschools.com/formulas/physics/air_resistance_formula/85/ self.air_resistance_k = Units.SI( self.config.air_density) * self.config.drag_coefficient * Units.SI( self.config.drag_area) / 2
def __init__(self, sim, config): PollingSensor.__init__(self, sim, config) self.logger = logging.getLogger("Accelerometer") self.logger.info("Initializing Accelerometer {}".format(self.config.id)) self.data = namedtuple('AccelerometerData', ['t_usec', 'raw_x', 'raw_y', 'raw_z', 'real_x', 'real_y', 'real_z']) # Quantities for converting to raw values raw_min = self.config.raw_min raw_max = self.config.raw_max real_min = Units.SI(self.config.real_min) real_max = Units.SI(self.config.real_max) self.sensor_input_range = (real_min, real_max) self.sensor_output_range = (raw_min, raw_max)
def __init__(self, sim, config): self.sim = sim self.config = config self.name = "F_lateral_stability" self.step_listeners = [] self.data = namedtuple(self.name, ['x', 'y', 'z']) self.damping_coefficient = Units.SI(self.config.damping_coefficient)
def __init__(self, sim, config): PollingSensor.__init__(self, sim, config) self.logger = logging.getLogger("LaserOptoSensor") self.logger.info("Initializing Laser Opto Sensor {}".format( self.config.id)) # Get model info (min/max, raw values, etc.) self.model = self.sim.config.sensors.models[self.config.model] # Set ranges for mapping real to raw and vice versa self.real_range = [ Units.SI(self.model.real_min), Units.SI(self.model.real_max) ] self.raw_range = [self.model.raw_min, self.model.raw_max] # Get the height offset for this sensor? self.he_height_offset = Units.SI(self.config.he_height_offset) self.measurement_offset = self.he_height_offset - Units.SI( self.model.internal_offset) # Data types self.data = namedtuple('LaserOptoSensorData', ('t_usec', 'height'))
def __init__(self, sim, config): Sensor.__init__(self, sim, config) self.logger = logging.getLogger("PollingSensor") # Configuration # Grab the fixed timestep from the sim. If we wanted to use a variable timestep we would need to do the next calculations in the lerp function self.sampling_rate = Units.SI(self.config.sampling_rate) # Hz # @see self._add_noise() and numpy.random.normal self.noise_center = self.config.noise.center or 0.0 self.noise_scale = self.config.noise.scale or 0.0 # Volatile #self.buffer = RingBuffer(config.buffer_size, config.dtype) # @todo: How to specify dtype in configuration? There should be a string rep of dtype I think self.buffer = None # Note: we depend on our listeners to handle any buffering/saving/sending of sensor values. Buffer is cleard after each step. self.sample_times = None self.next_start = 0.0 self.step_lerp_pcts = None # Set during step
def create_step_samples(self, dt_usec): # Pod acceleration (x) pod_start_accel = self.sim.pod.last_acceleration pod_end_accel = self.sim.pod.acceleration # Create the samples (lerp between the times) sample_data = self._lerp(pod_start_accel, pod_end_accel) sample_times = sample_times = self._get_sample_times(dt_usec) # @todo: apply a rotation matrix to simulate the actual # NOTE: Accel is mounted such that it is rotated 90 degrees in the horizontal plane such that: # +y accel = +x pod reference frame, +x accel = -y pod reference frame # ^ This is the format for the data. # @todo: move this to config somehow (tbd) # Map real values to sample values samples = [] for i, t in enumerate(sample_times): real_x = 0 real_y = sample_data[i] real_z = 9.81 # Accel due to gravity # @todo: Apply a rotation matrix? # Map xyz = np.array((real_x, real_y, real_z)) # Add some noise (in G's) xyz += np.random.normal(self.noise_center, Units.SI(self.noise_scale), 3) xyz = np.interp(xyz, self.sensor_input_range, self.sensor_output_range) xyz = xyz.astype(int) samples.append(self.data(t, xyz[0], xyz[1], xyz[2], real_x, real_y, real_z)) #samples += self._get_gaussian_noise(samples, self.noise_center, self.noise_scale) return samples
def __init__(self, sim, config): self.sim = sim self.config = config # Physical # NOTE: @see diagram in http://confluence.rloop.org/display/SD/2.+Determine+Pod+Kinematics # For the track/tube reference frame, x=0 at the start of the track, and x = length at the end. # For the pod, x=0 is ahead and below the pod, with x+ being toward the read of the pod. self.length = Units.SI( config.length ) # meters -- Length from pod start position to end of track # Reference for the reflective stripes self.reflective_strip_width = Units.SI( config.reflective_strip_width) # meters (4 inches) self.reflective_pattern_interval = Units.SI( config.reflective_pattern_interval) # meters (100 feet) self.reflective_pattern_spacing = Units.SI( config.reflective_pattern_spacing ) # meters -- distance between stripes in a pattern self.track_gap_width = Units.SI( config.track_gap_width ) # Width of the gap in the subtrack in meters (probably worst scenario, more realistic 1-2 mm) self.track_gap_interval = Units.SI( config.track_gap_interval ) # Interval between the gaps in the subtrack in meters (Length of the aluminium plate) # Initialize self.reflective_strips = None # Will be a numpy array after initialization self._init_reflective_strips(config.enable_strip_patterns) self.reflective_strip_ends = self.reflective_strips + self.reflective_strip_width # Pre-calc the ends for use in LaserContrastSensor self.track_gaps = [] self._init_track_gaps()
def __init__(self, sim, config): self.sim = sim self.config = config self.logger = logging.getLogger("Brake") # Limit Switches # Switch activation: We only want to call the callback when the switch is tripped, not repeatedly self.retract_sw_activated = False self.extend_sw_activated = False # Volatile #self.gap = Units.SI(self.config.initial_gap) # @todo: make this work # Rail Gap # Screw pos is the main value from which we calculate the gap and MLP values. [0, 75000]um fully retracted to fully extended (maps to brake gap) # Note: screw position is updated by the callback from the FCU self.gap = Units.SI(self.config.gap.initial_gap) #print "Gap after conversion is {}".format(self.gap) #exit() self.retracted_gap = Units.SI(self.config.gap.retracted_gap) self.extended_gap = Units.SI(self.config.gap.extended_gap) self.gap_range = [self.retracted_gap, self.extended_gap] # Screw self.screw_limit_sw_retract = Units.SI( self.config.lead_screw.limit_sw_retract) self.screw_limit_sw_extend = Units.SI( self.config.lead_screw.limit_sw_extend) self.screw_range = [ self.screw_limit_sw_retract, self.screw_limit_sw_extend ] # Calculate initial screw position from the initial gap (note: during processing it's the other way around) self.screw_pos = np.interp(self.gap, self.gap_range, self.screw_range) # Linear Position Sensor # @todo: check this to make sure the min/max are in the correct order -- should be retracted->extended self.mlp_range = [self.config.mlp.raw_min, self.config.mlp.raw_max] # Calculate raw MLP value from the screw position print("{}, {}, {}".format(self.screw_pos, self.screw_range, self.mlp_range)) self.mlp_raw = np.interp(self.screw_pos, self.screw_range, self.mlp_range) # Negator self.negator_torque = Units.SI(self.config.negator.torque) # TESTING ONLY self._gap_target = self.gap # Initialize to current value so we don't move yet self._gap_close_time = Units.SI(self.config.gap_close_min_time) self._gap_close_dist = self.retracted_gap - self.extended_gap self._gap_close_speed = self._gap_close_dist / self._gap_close_time # meters/second -- this is just a guess -- .007 m/s = closing 21mm in 3s #self.logger.debug("Brake gap close speed: {} m/s".format(self._gap_close_speed)) # /TESTING self.normal_force = 0.0 # N -- normal against the rail; +normal is away from the rail self.drag_force = 0.0 # N -- drag on the pod; -drag is toward the back of the pod self.last_normal_force = 0.0 self.last_drag_force = 0.0 # Lead Screw # revs per cm=2.5 There are 4 mm per single lead so 2.5 turns move the carriage 1 cm # Formulas: http://www.nookindustries.com/LinearLibraryItem/Ballscrew_Torque_Calculations self.screw_pitch = Units.SI(self.config.lead_screw.pitch) self.drive_efficiency = self.config.lead_screw.drive_efficiency self.backdrive_efficiency = self.config.lead_screw.backdrive_efficiency # Lead Screw Precalculated Values self._drive_torque_multiplier = self.screw_pitch / ( self.drive_efficiency * 2 * 3.14) self._backdrive_torque_multiplier = ( self.screw_pitch * self.backdrive_efficiency) / (2 * 3.14)
def __init__(self, sim, config): self.logger = logging.getLogger("Pod") self.logger.info("Initializing pod") self.sim = sim self.config = config self.name = 'pod_actual' self.step_listeners = [] self.step_forces = OrderedDict( ) # This will be filled in during each step # Pod physical properties # *** NOTE: all distances are in the track/tube reference frame, origin is pod location reference (centerpoint of fwd crossbar). +x is forward, +y is left, +z is up. # @see http://confluence.rloop.org/display/SD/2.+Determine+Pod+Kinematics self.mass = Units.SI(self.config.mass) self.pusher_plate_offset = Units.SI( self.config.physical.pusher_plate_offset) self.pusher_pin_travel = Units.SI( self.config.physical.pusher_pin_travel) # Forces that can act on the pod (note: these are cleared at the end of each step) self.net_force = np.array( (0.0, 0.0, 0.0) ) # Newtons; (x, y, z). +x pushes the pod forward, +z force lifts the pod, y is not currently used. # Initialize actual physical values (volatile variables). All refer to action in the x dimension only. self.acceleration = Units.SI( self.config.acceleration) or 0.0 # meters per second ^2 self.velocity = Units.SI( self.config.velocity) or 0.0 # meters per second self.position = Units.SI( self.config.position ) or 0.0 # meters. Position relative to the track; start position is 0m # @todo: this is just a sketch, for use with the hover engine calculations. Maybe switch accel, velocity, and position to coordinates? hmmmm... self.z_acceleration = 0.0 self.z_velocity = 0.0 self.he_height = Units.SI( self.config.landing_gear.initial_height ) # This should be gotten from the starting height of the lift mechanism self._initial_he_height = self.he_height # @todo: Remove this -- it's only used as a temporary block from going through the floor until the landing gear is implemented self.elapsed_time_usec = 0 # Values from the previous step (for lerping and whatnot) self.last_acceleration = 0.0 self.last_velocity = 0.0 self.last_position = 0.0 self.last_he_height = 0.0 # Handle forces that act on the pod """ self.forces = [] self.forces.append( AeroForce(self.sim, self.config.forces.aero) ) self.forces.append( BrakeForce(self.sim, self.config.forces.brakes) ) self.forces.append( GimbalForce(self.sim, self.config.forces.gimbals) ) self.forces.append( HoverEngineForce(self.sim, self.config.forces.hover_engines) ) self.forces.append( LateralStabilityForce(self.sim, self.config.forces.lateral_stability) ) self.forces.append( LandingGearForce(self.sim, self.config.forces.landing_gear) ) """ self.force_exerters = OrderedDict() self.force_exerters['aero'] = AeroForce(self.sim, self.config.forces.aero) self.force_exerters['brakes'] = BrakeForce(self.sim, self.config.forces.brakes) self.force_exerters['gimbals'] = GimbalForce( self.sim, self.config.forces.gimbals) self.force_exerters['hover_engines'] = HoverEngineForce( self.sim, self.config.forces.hover_engines) self.force_exerters['lateral_stability'] = LateralStabilityForce( self.sim, self.config.forces.lateral_stability) self.force_exerters['landing_gear'] = LandingGearForce( self.sim, self.config.forces.landing_gear) # Forces applied during the step (for recording purposes) # @todo: Maybe initialize these to ForceExerter.data(0,0,0)? self.step_forces = OrderedDict() self.step_forces['aero'] = ForceExerter.data(0, 0, 0) self.step_forces['brakes'] = ForceExerter.data(0, 0, 0) self.step_forces['gimbals'] = ForceExerter.data(0, 0, 0) self.step_forces['hover_engines'] = ForceExerter.data(0, 0, 0) self.step_forces['lateral_stability'] = ForceExerter.data(0, 0, 0) self.step_forces['landing_gear'] = ForceExerter.data(0, 0, 0) # Pre-calculated values # HE Drag """ Drag for a single hover engine (keep in mind that we have 8): Constants: w: Comes from solving 3d maxwell's equations (@whiplash) and using other fancy math -- provided by @whiplash -- not dependent on velocity, just depends on magnetic characteristics mu_0: Absolute permeability of the vacuum (constant) -- permeability constant, magnetic constant, etc. -- permeability of free space m: Magnetic dipole strength (constant, but depends on how magnets are arranged) h: thickness of conducting material (rail in this case) rho: density of conducting material (aluminum in this case) Variables: z_0: Distance between magnet and conducting surface v: Velocity Calculated: w = 2 * rho / (mu_0 * h) F_lift / F_drag = v / w F_lift = ((3 * mu_0 * m**2) / (32 * 3.14159 * z_0**4)) * (1 - w * (v**2 + w**2)**(-1/2)) Values (from @whiplash): mu_0: 4 pi x10e-7 rho: 2.7 g/cubic centimeter h: 0.5 in m: (@whiplash will need to calculate this -- it's a vector quantity, need to draw a magnetic circuit) """ # Brakes #self.brakes = [] #for brake_config in self.config.brakes: # self.brakes.append(Brake(self.sim, brake_config)) self.brakes = Brakes(self.sim, self.config.brakes) """ Sketch:
def __init__(self, sim, config): ForceExerter.__init__(self, sim, config) self.name = 'F_lateral_stability' self.damping_coefficient = Units.SI(self.config.damping_coefficient)
def __init__(self, sim, config): ForceExerter.__init__(self, sim, config) self.name = 'F_aero' # @see http://www.softschools.com/formulas/physics/air_resistance_formula/85/ self.air_resistance_k = Units.SI(self.config.air_density) * self.config.drag_coefficient * Units.SI(self.config.drag_area) / 2