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 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