def aim(self):
"""Get location, aim turret, accelerate wheels and advance dart."""
# We need to be up to speed before reading the dart velocity
self.gun.spin_up()
time.sleep(1.0)
x_pos, y_pos, theta = self.localize()
if not self.validate_pose(x_pos, y_pos, theta):
self.logger.error("What do I do now?!")
# dart_velocity should be roughtly 5-14 m/s
dart_velocity = self.gun.dart_velocity
self.logger.debug(
"Getting firing solution using dart_vel: %0.2f", dart_velocity)
pitch, yaw = targeting.getFiringSolution(
x_pos, y_pos, theta, dart_velocity)
yaw += 90
self.logger.info(
"Aiming turret to (pitch: {}, yaw: {})".format(pitch, yaw))
self.turret.pitch = pitch
self.turret.yaw = yaw
# Allow turrent time to move
# TODO: make config param so we can zero during testing
time.sleep(0.2)
# Call CV to find left-right offset,
# calc server adjustment based on dist to target
# opencv stuff => image offset
self.logger.warning("OpenCV turret repositioning not implemented")
targeting.getTargetDistance(x_pos, y_pos)
def test_getTargetDistance(self):
distance = targeting.getTargetDistance(.3572, 0.9144)
self.assertAlmostEqual(distance, .9422, places=4)
