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_getFiringSolution_bad_velocity(self):
with self.assertRaises(ValueError):
targeting.getFiringSolution(.3572, .9144, 0, .5)
def test_getFiringSolution(self):
vert_servo_angle, horiz_servo_angle = targeting.getFiringSolution(
.3572, .9144, 0, 7.4439)
self.assertAlmostEqual(horiz_servo_angle, 13.942, places=2)
self.assertAlmostEqual(vert_servo_angle, 63, places=0)