def on_run(self,
meters_per_revolution=1.3,
deadband=0.2,
spin_ratio=1,
relative_depth_range=0.0,
heading_change_scale=None,
optimize_heading=False,
min_spin_radius=None):
radius = self.calc_radius(self.theta, meters_per_revolution)
target = self.calc_position(self.theta, meters_per_revolution,
relative_depth_range)
delta = target - _sub_position()
# fake_target = target + (delta * 10)
fake_target = target
GoToPosition(fake_target[0], fake_target[1], depth=fake_target[2])()
desired_heading = (spin_ratio * self.theta) + 90
if heading_change_scale is not None:
desired_heading *= min(1.0, radius) * heading_change_scale
if optimize_heading:
desired_heading = math.degrees(math.atan2(delta[1], delta[0]))
if min_spin_radius is not None:
if (not abs_heading_sub_degrees(shm.kalman.heading.get(),
desired_heading) < 10
or not radius > min_spin_radius
) and not self.started_heading:
desired_heading = shm.navigation_desires.heading.get()
else:
self.started_heading = True
Heading(desired_heading % 360)()
if np.linalg.norm(_sub_position() - target) < deadband:
self.theta += 10
def find_closest_angle(self, target, *angles, post=False):
if len(angles) == 0: return
# print(angles)
closest = min(angles, key=lambda x: abs_heading_sub_degrees(target, x))
print(closest)
if post:
shm.bins_garlic.angle_offset.set(heading_sub_degrees(target, closest))
return closest
def within_deadband(a, b, deadband, use_mod_error):
"""Check if two values are close enough, even on a circle.
Args:
a (float): The first value to compare.
b (float): The second value to compare.
deadband (float): The acceptable deadband for the deadband. This is exclusive.
use_mod_error (bool): If True, the calculation is performed using modular arithmetic respecting the mod
argument. Otherwise, the calculation is performed ignoring the mod argument.
use_mod_error (float): If use_mod_error is True, the calculation is performed mod 360.
Returns:
(bool): A boolean that represents if the two values are within the deadband of each other.
"""
if use_mod_error:
return abs_heading_sub_degrees(a, b) < deadband
else:
return abs(a - b) < deadband
def within_deadband(a, b, deadband, use_mod_error):
"""Check if two values are close enough, even on a circle.
Args:
a (float): The first value to compare.
b (float): The second value to compare.
deadband (float): The acceptable deadband for the deadband. This is exclusive.
use_mod_error (bool): If True, the calculation is performed using modular arithmetic respecting the mod
argument. Otherwise, the calculation is performed ignoring the mod argument.
use_mod_error (float): If use_mod_error is True, the calculation is performed mod 360.
Returns:
(bool): A boolean that represents if the two values are within the deadband of each other.
"""
if use_mod_error:
return abs_heading_sub_degrees(a, b) < deadband
else:
return abs(a - b) < deadband
def on_run(self, meters_per_revolution = 2.0, deadband = 0.2, spin_ratio = 5, relative_depth_range = 0.5, heading_change_scale = None, optimize_heading = False, min_spin_radius=None):
radius = self.calc_radius(self.theta, meters_per_revolution)
target = self.calc_position(self.theta, meters_per_revolution, relative_depth_range)
GoToPosition(target[0], target[1], depth = target[2])()
desired_heading = (spin_ratio * self.theta)
if heading_change_scale is not None:
desired_heading *= min(1.0, radius) * heading_change_scale
if optimize_heading:
delta = target - aslam.sub.position()
desired_heading = math.degrees(math.atan2(delta[1], delta[0]))
if min_spin_radius is not None:
if (not abs_heading_sub_degrees(shm.kalman.heading.get(), desired_heading) < 10 or not radius > min_spin_radius) and not self.started_heading:
desired_heading = shm.kalman.heading.get()
else:
self.started_heading = True
Heading(desired_heading % 360)()
if np.linalg.norm(aslam.sub.position() - target) < deadband:
self.theta += 10
outputs.heading_cumulative = outputs.heading
shm.kalman.set(outputs)
## Read Inputs
#Data relative to the sub
x_vel = -1*x_vel_in.get()
x_acc = 0 # x_acc_in.get()
y_vel = -1*y_vel_in.get()
y_acc = 0 # y_acc_in.get()
# When the DVL is tracking the surface the y velocity is reversed.
# This is not ideal... what happens when it is not exactly inverted?
if dvl_velocity and \
bool(abs_heading_sub_degrees(outputs.roll, 180) < 90) ^ \
bool(abs_heading_sub_degrees(outputs.pitch, 180) < 90):
y_vel = -y_vel
#depth = depth_in.get() - depth_offset.get()
depth = depth_in.get() - 8.64
#depth = 2.5 - shm.dvl.savg_altitude.get()
# Compensate for gravitational acceleration
grav_x = sin( radians(outputs.pitch) )*9.8 # XXX: CHRIS DOES NOT LIKE (small angle approx??)
grav_y = -sin( radians(outputs.roll) )*9.8
gx4_grav_y = np.tan(radians(outputs.pitch))*np.sqrt(shm.gx4.accelx.get()**2 + shm.gx4.accelz.get()**2)
gx4_grav_x = -1*np.tan(radians(outputs.roll))*shm.gx4.accelz.get()
him_grav_y = np.tan(radians(outputs.pitch))*np.sqrt(shm.him.x_accel.get()**2 + shm.him.z_accel.get()**2)
him_grav_x = -1*np.tan(radians(outputs.roll))*shm.him.z_accel.get()
x_acc = x_acc - grav_x
y_acc = y_acc - grav_y