def func(deg, mhz):
ang = deg * pi / 180
wavevector_unit = [sin(ang), 0, cos(ang)]
ray = Ray([0, 0, 0], wavevector_unit, self.op_wavelength_vac)
acoustics = Acoustics(mhz * 1e6, ac_power)
self.aod.propagate_ray([ray], [acoustics], self.order)
return ray.resc / (ray.energy + 1e-6)
def func(angle_out):
mhz = angle_out / 1e6 * 613 / self.op_wavelength_vac + 39
ang = deg * pi / 180
wavevector_unit = [sin(ang), 0, cos(ang)]
ray = Ray([0, 0, 0], wavevector_unit, self.op_wavelength_vac)
acoustics = Acoustics(mhz * 1e6, ac_power)
self.aod.propagate_ray([ray], [acoustics], self.order)
return ray.energy
def func(deg, deg_trans):
ang = deg * pi / 180
ang_trans = deg_trans * pi / 180
wavevector_unit = [ang, ang_trans, sqrt(1 - ang**2 - ang_trans**2)]
ray = Ray([0, 0, 0], wavevector_unit, self.op_wavelength_vac)
acoustics = Acoustics(freq, ac_power)
self.aod.propagate_ray([ray], [acoustics], self.order)
return ray.energy
def func(deg, mhz):
ang = deg * pi / 180
wavevector_unit = [sin(ang), 0, cos(ang)]
ray = Ray([0, 0, 0], wavevector_unit, self.op_wavelength_vac)
acoustics = Acoustics(mhz * 1e6, ac_power)
(mismatch, _, _) = diffract_by_wavevector_triangle(
self.aod, [ray.wavevector_unit], [ray.wavevector_vac_mag],
[acoustics], self.order, (0, 1))
return abs(mismatch)
def func(ac_power):
deg_range = linspace(1.9, 3.1, 40)
rad_range = deg_range * pi / 180
rays = [
Ray([0, 0, 0], [sin(ang), 0, cos(ang)], self.op_wavelength_vac)
for ang in rad_range
]
acoustics = Acoustics(ac_mhz * 1e6, ac_power)
self.aod.propagate_ray(rays, [acoustics] * len(rays), self.order)
return max([r.energy for r in rays])
def func(mhz):
deg_range = array([deg])
rad_range = deg_range * pi / 180
rays = [
Ray([0, 0, 0], [sin(ang), 0, cos(ang)], wavelen)
for ang in rad_range
]
acoustics = Acoustics(mhz * 1e6, ac_power)
self.aod.propagate_ray(rays, [acoustics] * len(rays),
self.order)
return max([r.energy for r in rays])
def func(mhz):
deg_range = linspace(0.9, 3, 70)
rad_range = deg_range * pi / 180
rays = [
Ray([0, 0, 0], [sin(ang), 0, cos(ang)], self.op_wavelength_vac)
for ang in rad_range
]
acoustics = Acoustics(mhz * 1e6, ac_power)
self.aod.propagate_ray(rays, [acoustics] * len(rays), self.order)
idx = argmax([r.energy for r in rays])
return rays[idx].resc
def func(deg, mhz):
ang = deg * pi / 180
optical_rot = pi / 180 * 0
wavevector_unit = [
cos(optical_rot) * sin(ang),
sin(optical_rot),
cos(optical_rot) * cos(ang)
]
ray = Ray([0, 0, 0], wavevector_unit, self.op_wavelength_vac)
acoustics = Acoustics(mhz * 1e6, ac_power)
self.aod.propagate_ray([ray], [acoustics], self.order)
return ray.energy
def func(deg, deg_trans):
ang = deg * pi / 180
ang_trans = deg_trans * pi / 180
wavevector_unit = [
sin(ang),
sin(ang_trans),
sqrt(1 - sin(ang)**2 - sin(ang_trans)**2)
]
ray = Ray([0, 0, 0], wavevector_unit, self.op_wavelength_vac)
acoustics = Acoustics(35e6, ac_power)
self.aod.propagate_ray([ray], [acoustics], self.order)
return arcsin(ray.wavevector_unit[0]) * 180 / pi
def main(cmd=None):
if (cmd is None):
cmd = argv[1]
if (cmd == 'competition'):
# Load ADC Stuff
acoustics = Acoustics()
acoustics.preset(0)
val = locate_pinger.main(acoustics.adc, dearm=False) # degrees right
# Dearm ADC outside of locate_pinger.py.
acoustics.adc.unready()
# return value to the user
return val
elif (cmd == 'competition-cont'):
# Load ADC Stuff
acoustics = Acoustics()
acoustics.preset(0)
# Locate pinger
while True:
# Exits on CTRL-C
try:
locate_pinger.main(acoustics.adc, dearm=False)
except KeyboardInterrupt:
print("Quitting program")
break
# Dearm ADC outside of locate_pinger.py.
acoustics.adc.unready()
elif (cmd == 'plt'):
# Load plotting stuff
plt = load_matplotlib()
# Load ADC Stuff
acoustics = Acoustics()
acoustics.preset(100)
acoustics.adc.adc_status()
quickplot.main(acoustics.adc, plt)
def init(self):
'''runs at start of mission '''
#self.process_manager.start_process(entities.PathEntity, "path", "forward", debug=True)
self.acoustics = Acoustics()
self.acoustics.connect(PORT_NAME)
# enable logger
self.acoustics.start_logger('acoustics1_{}'.format(time.time()))
# pinger object variable
self.ac_data = None
# pinger tracking variables
self.last_yaw_reading = None
self.last_pitch_reading = None
self.epoch = None
self.ping_age = None
self.listening = True
# state variables
self.ping_stale = True
self.action = 'init'
self.ping_used = False
# Flush stout so that system logging file can update with information
sys.stdout.flush()
# ######################
### Global Constants ###
#########################
def define_commlink():
uart.enable_uart()
print("Opening Port %s." % PORT_NAME)
return s.Serial(PORT_NAME, 9600, timeout=1)
PORT_NAME = "/dev/ttyO5"
acoustics = Acoustics() # Acoustics Control Object
# load global config settings
config = acoustics.pass_config_module()
pAC = define_commlink() # Acoustics communication port
log = acoustics.logger
data_dict = ('')
# ######################
### Other definitions ##
#######################
def init_acoustics():
class AcousticsMission1(MissionBase):
def init(self):
'''runs at start of mission '''
#self.process_manager.start_process(entities.PathEntity, "path", "forward", debug=True)
self.acoustics = Acoustics()
self.acoustics.connect(PORT_NAME)
# enable logger
self.acoustics.start_logger('acoustics1_{}'.format(time.time()))
# pinger object variable
self.ac_data = None
# pinger tracking variables
self.last_yaw_reading = None
self.last_pitch_reading = None
self.epoch = None
self.ping_age = None
self.listening = True
# state variables
self.ping_stale = True
self.action = 'init'
self.ping_used = False
#['init','drive_to_pinger','no_ping','over_pinger']
def stop(self):
sw3.nav.do(sw3.Forward(0))
self.finish_mission()
def change_heading(self, forward, rel_yaw):
if abs(rel_yaw) > YAW_TOLERANCE:
sw3.nav.do(sw3.CompoundRoutine(
sw3.RelativeYaw(-rel_yaw),
sw3.Forward(forward)
)
)
def listen_for_ping(self):
self.ac_data = self.acoustics.get_data()
if self.ac_data['error'] == 0:
# check if ping received was pretty old
self.ping_age = self.ac_data['data']['epoch']
self.epoch = time.time() - self.ping_age
#print "ping_age is {} (epoch = {})".format(self.ping_age, self.epoch)
if self.ping_age > ACOUSTICS_SAMPLING_INTERVAL:
print "hi"
print "data is stale. will try again later"
self.ping_stale = True
self.ping_used = False # new ping. It likely hasn't been used yet.
return None
else:
self.ping_stale = False
# grab pinger information
self.last_yaw_reading = self.ac_data['data']['heading']['ab']
self.last_pitch_reading = -self.ac_data['data']['heading']['cd']
if self.ping_age < 2:
print "PING!!! yaw={}, pitch={}".format(self.last_yaw_reading,
self.last_pitch_reading)
return self.ac_data
else:
print "ERROR received"
# if nothing happend, report no ping
return None
def step(self, vision_data):
#if not vision_data:
# return
print "action = {}".format(self.action)
if self.listening:
self.listen_for_ping()
if self.action == 'init':
if self.ping_stale:
return
else:
self.change_heading( 0.0, self.last_yaw_reading)
self.action = 'drive_to_pinger'
# mark ping as used
self.ping_used = False
elif self.action == 'drive_to_pinger':
if self.ping_stale or self.ping_used:
return
else:
self.change_heading( 0.0, self.last_yaw_reading)
self.ping_used = True
