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
