Merge "Factor out generic aiming code from 2020 aimer"
diff --git a/frc971/control_loops/aiming/BUILD b/frc971/control_loops/aiming/BUILD
new file mode 100644
index 0000000..f779b8e
--- /dev/null
+++ b/frc971/control_loops/aiming/BUILD
@@ -0,0 +1,22 @@
+cc_library(
+ name = "aiming",
+ srcs = ["aiming.cc"],
+ hdrs = ["aiming.h"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos/logging",
+ "//frc971:constants",
+ "//frc971/control_loops:pose",
+ ],
+)
+
+cc_test(
+ name = "aiming_test",
+ srcs = ["aiming_test.cc"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":aiming",
+ "//aos/testing:googletest",
+ ],
+)
diff --git a/frc971/control_loops/aiming/aiming.cc b/frc971/control_loops/aiming/aiming.cc
new file mode 100644
index 0000000..8229e13
--- /dev/null
+++ b/frc971/control_loops/aiming/aiming.cc
@@ -0,0 +1,132 @@
+#include "frc971/control_loops/aiming/aiming.h"
+
+#include "glog/logging.h"
+
+namespace frc971::control_loops::aiming {
+
+// Shooting-on-the-fly concept:
+// The current way that we manage shooting-on-the fly endeavors to be reasonably
+// simple, until we get a chance to see how the actual dynamics play out.
+// Essentially, we assume that the robot's velocity will represent a constant
+// offset to the ball's velocity over the entire trajectory to the goal and
+// then offset the target that we are pointing at based on that.
+// Let us assume that, if the robot shoots while not moving, regardless of shot
+// distance, the ball's average speed-over-ground to the target will be a
+// constant s_shot (this implies that if the robot is driving straight towards
+// the target, the actual ball speed-over-ground will be greater than s_shot).
+// We will define things in the robot's coordinate frame. We will be shooting
+// at a target that is at position (target_x, target_y) in the robot frame. The
+// robot is travelling at (v_robot_x, v_robot_y). In order to shoot the ball,
+// we need to generate some virtual target (virtual_x, virtual_y) that we will
+// shoot at as if we were standing still. The total time-of-flight to that
+// target will be t_shot = norm2(virtual_x, virtual_y) / s_shot.
+// we will have virtual_x + v_robot_x * t_shot = target_x, and the same
+// for y. This gives us three equations and three unknowns (virtual_x,
+// virtual_y, and t_shot), and given appropriate assumptions, can be solved
+// analytically. However, doing so is obnoxious and given appropriate functions
+// for t_shot may not be feasible. As such, instead of actually solving the
+// equation analytically, we will use an iterative solution where we maintain
+// a current virtual target estimate. We start with this estimate as if the
+// robot is stationary. We then use this estimate to calculate t_shot, and
+// calculate the next value for the virtual target.
+
+namespace {
+// This implements the iteration in the described shooting-on-the-fly algorithm.
+// robot_pose: Current robot pose.
+// robot_velocity: Current robot velocity, in the absolute field frame.
+// target_pose: Absolute goal Pose.
+// current_virtual_pose: Current estimate of where we want to shoot at.
+// ball_speed_over_ground: Approximate ground speed of the ball that we are
+// shooting.
+Pose IterateVirtualGoal(const Pose &robot_pose,
+ const Eigen::Vector3d &robot_velocity,
+ const Pose &target_pose,
+ const Pose ¤t_virtual_pose,
+ double ball_speed_over_ground) {
+ const double air_time = current_virtual_pose.Rebase(&robot_pose).xy_norm() /
+ ball_speed_over_ground;
+ const Eigen::Vector3d virtual_target =
+ target_pose.abs_pos() - air_time * robot_velocity;
+ return Pose(virtual_target, target_pose.abs_theta());
+}
+} // namespace
+
+TurretGoal AimerGoal(const ShotConfig &config, const RobotState &state) {
+ TurretGoal result;
+ // This code manages compensating the goal turret heading for the robot's
+ // current velocity, to allow for shooting on-the-fly.
+ // This works by solving for the correct turret angle numerically, since while
+ // we technically could do it analytically, doing so would both make it hard
+ // to make small changes (since it would force us to redo the math) and be
+ // error-prone since it'd be easy to make typos or other minor math errors.
+ Pose virtual_goal;
+ {
+ result.target_distance = config.goal.Rebase(&state.pose).xy_norm();
+ virtual_goal = config.goal;
+ if (config.mode == ShotMode::kShootOnTheFly) {
+ for (int ii = 0; ii < 3; ++ii) {
+ virtual_goal = IterateVirtualGoal(
+ state.pose, {state.velocity(0), state.velocity(1), 0}, config.goal,
+ virtual_goal, config.ball_speed_over_ground);
+ }
+ VLOG(1) << "Shooting-on-the-fly target position: "
+ << virtual_goal.abs_pos().transpose();
+ }
+ virtual_goal = virtual_goal.Rebase(&state.pose);
+ }
+
+ const double heading_to_goal = virtual_goal.heading();
+ result.virtual_shot_distance = virtual_goal.xy_norm();
+
+ // The following code all works to calculate what the rate of turn of the
+ // turret should be. The code only accounts for the rate of turn if we are
+ // aiming at a static target, which should be close enough to correct that it
+ // doesn't matter that it fails to account for the
+ // shooting-on-the-fly compensation.
+ const double rel_x = virtual_goal.rel_pos().x();
+ const double rel_y = virtual_goal.rel_pos().y();
+ const double squared_norm = rel_x * rel_x + rel_y * rel_y;
+ // rel_xdot and rel_ydot are the derivatives (with respect to time) of rel_x
+ // and rel_y. Since these are in the robot's coordinate frame, and since we
+ // are ignoring lateral velocity for this exercise, rel_ydot is zero, and
+ // rel_xdot is just the inverse of the robot's velocity.
+ // Note that rel_x and rel_y are in the robot frame.
+ const double rel_xdot = -Eigen::Vector2d(std::cos(state.pose.rel_theta()),
+ std::sin(state.pose.rel_theta()))
+ .dot(state.velocity);
+ const double rel_ydot = 0.0;
+
+ // If squared_norm gets to be too close to zero, just zero out the relevant
+ // term to prevent NaNs. Note that this doesn't address the chattering that
+ // would likely occur if we were to get excessively close to the target.
+ // Note that x and y terms are swapped relative to what you would normally see
+ // in the derivative of atan because xdot and ydot are the derivatives of
+ // robot_pos and we are working with the atan of (target_pos - robot_pos).
+ const double atan_diff =
+ (squared_norm < 1e-3) ? 0.0 : (rel_x * rel_ydot - rel_y * rel_xdot) /
+ squared_norm;
+ // heading = atan2(relative_y, relative_x) - robot_theta
+ // dheading / dt =
+ // (rel_x * rel_y' - rel_y * rel_x') / (rel_x^2 + rel_y^2) - dtheta / dt
+ const double dheading_dt = atan_diff - state.yaw_rate;
+
+ const double range =
+ config.turret_range.range() - config.anti_wrap_buffer * 2.0;
+ // Calculate a goal turret heading such that it is within +/- pi of the
+ // current position (i.e., a goal that would minimize the amount the turret
+ // would have to travel).
+ // We then check if this goal would bring us out of range of the valid angles,
+ // and if it would, we reset to be within +/- pi of zero.
+ double turret_heading =
+ state.last_turret_goal +
+ aos::math::NormalizeAngle(heading_to_goal - config.turret_zero_offset -
+ state.last_turret_goal);
+ if (std::abs(turret_heading - config.turret_range.middle()) > range / 2.0) {
+ turret_heading = aos::math::NormalizeAngle(turret_heading);
+ }
+ result.position = turret_heading;
+ result.velocity = dheading_dt;
+ return result;
+}
+
+} // namespace frc971::control_loops::aiming
diff --git a/frc971/control_loops/aiming/aiming.h b/frc971/control_loops/aiming/aiming.h
new file mode 100644
index 0000000..47fd06a
--- /dev/null
+++ b/frc971/control_loops/aiming/aiming.h
@@ -0,0 +1,62 @@
+#ifndef FRC971_CONTROL_LOOPS_AIMING_AIMING_H_
+#define FRC971_CONTROL_LOOPS_AIMING_AIMING_H_
+#include "frc971/constants.h"
+#include "frc971/control_loops/pose.h"
+
+// This library provides utilities associated with attempting to aim balls into
+// a goal.
+
+namespace frc971::control_loops::aiming {
+
+// Control modes for managing how we manage shooting on the fly.
+enum class ShotMode {
+ // Don't do any shooting-on-the-fly compensation--just point straight at the
+ // target. Primarily used in tests.
+ kStatic,
+ // Do do shooting-on-the-fly compensation.
+ kShootOnTheFly,
+};
+
+struct TurretGoal {
+ // Goal position (in radians) for the turret.
+ double position = 0.0;
+ // Goal velocity (in radians / sec) for the turret.
+ double velocity = 0.0;
+ // Physical distance from the robot's origin to the target we are shooting at,
+ // in meters.
+ double target_distance = 0.0;
+ // Shot distance to use when shooting on the fly (e.g., if driving towards the
+ // target, we will aim for a shorter shot than the actual physical distance),
+ // in meters.
+ double virtual_shot_distance = 0.0;
+};
+
+struct RobotState {
+ // Pose of the robot, in the field frame.
+ Pose pose;
+ // X/Y components of the robot velocity, in m/s.
+ Eigen::Vector2d velocity;
+ // Yaw rate of the robot, in rad / sec.
+ double yaw_rate;
+ // Last turret goal that we produced.
+ double last_turret_goal;
+};
+
+struct ShotConfig {
+ // Pose of the goal, in the field frame.
+ Pose goal;
+ ShotMode mode;
+ const constants::Range turret_range;
+ // We assume that the ball being shot has an ~constant speed over the ground,
+ // to allow us to estimate shooting-on-the fly values.
+ double ball_speed_over_ground;
+ // Amount of buffer to add on each side of the range to prevent wrapping/to
+ // prevent getting too close to the hard stops.
+ double anti_wrap_buffer;
+ // Offset from zero in the robot frame to zero for the turret.
+ double turret_zero_offset;
+};
+
+TurretGoal AimerGoal(const ShotConfig &config, const RobotState &state);
+}
+#endif // FRC971_CONTROL_LOOPS_AIMING_AIMING_H_
diff --git a/frc971/control_loops/aiming/aiming_test.cc b/frc971/control_loops/aiming/aiming_test.cc
new file mode 100644
index 0000000..c1f7367
--- /dev/null
+++ b/frc971/control_loops/aiming/aiming_test.cc
@@ -0,0 +1,160 @@
+#include "frc971/control_loops/aiming/aiming.h"
+
+#include "frc971/control_loops/pose.h"
+#include "gtest/gtest.h"
+#include "frc971/constants.h"
+
+namespace frc971::control_loops::aiming::testing {
+
+TEST(AimerTest, StandingStill) {
+ const Pose target({0.0, 0.0, 0.0}, 0.0);
+ Pose robot_pose({1.0, 0.0, 0.0}, 0.0);
+ const constants::Range range{-4.5, 4.5, -4.0, 4.0};
+ const double kBallSpeed = 10.0;
+ // Robot is ahead of target, should have to turret to 180 deg to shoot.
+ TurretGoal goal = AimerGoal(
+ ShotConfig{target, ShotMode::kShootOnTheFly, range, kBallSpeed, 0.0, 0.0},
+ RobotState{robot_pose, {0.0, 0.0}, 0.0, 0.0});
+ EXPECT_FLOAT_EQ(M_PI, goal.position);
+ EXPECT_FLOAT_EQ(0.0, goal.velocity);
+ EXPECT_FLOAT_EQ(1.0, goal.virtual_shot_distance);
+ EXPECT_FLOAT_EQ(1.0, goal.target_distance);
+
+ // If there is a turret offset, it should get compensated out.
+ goal = AimerGoal(ShotConfig{target, ShotMode::kShootOnTheFly, range,
+ kBallSpeed, 0.0, M_PI},
+ RobotState{robot_pose, {0.0, 0.0}, 0.0, 0.0});
+ EXPECT_FLOAT_EQ(0.0, goal.position);
+
+ robot_pose = Pose({-1.0, 0.0, 0.0}, 1.0);
+ goal = AimerGoal(
+ ShotConfig{target, ShotMode::kShootOnTheFly, range, kBallSpeed, 0.0, 0.0},
+ RobotState{robot_pose, {0.0, 0.0}, 0.0, 0.0});
+ EXPECT_FLOAT_EQ(-1.0, goal.position);
+ EXPECT_FLOAT_EQ(0.0, goal.velocity);
+ EXPECT_FLOAT_EQ(1.0, goal.virtual_shot_distance);
+ EXPECT_FLOAT_EQ(1.0, goal.target_distance);
+
+ // Test that we handle the case that where we are right on top of the target.
+ robot_pose = Pose({0.0, 0.0, 0.0}, 0.0);
+ goal = AimerGoal(
+ ShotConfig{target, ShotMode::kShootOnTheFly, range, kBallSpeed, 0.0, 0.0},
+ RobotState{robot_pose, {0.0, 0.0}, 0.0, 0.0});
+ EXPECT_FLOAT_EQ(0.0, goal.position);
+ EXPECT_FLOAT_EQ(0.0, goal.velocity);
+ EXPECT_FLOAT_EQ(0.0, goal.virtual_shot_distance);
+ EXPECT_FLOAT_EQ(0.0, goal.target_distance);
+}
+
+// Test that spinning in place results in correct velocity goals.
+TEST(AimerTest, SpinningRobot) {
+ const Pose target({0.0, 0.0, 0.0}, 0.0);
+ Pose robot_pose({-1.0, 0.0, 0.0}, 0.0);
+ const constants::Range range{-4.5, 4.5, -4.0, 4.0};
+ const double kBallSpeed = 10.0;
+ TurretGoal goal = AimerGoal(
+ ShotConfig{target, ShotMode::kShootOnTheFly, range, kBallSpeed, 0.0, 0.0},
+ RobotState{robot_pose, {0.0, 0.0}, 971.0, 0.0});
+ EXPECT_FLOAT_EQ(0.0, goal.position);
+ EXPECT_FLOAT_EQ(-971.0, goal.velocity);
+}
+
+// Tests that when we drive straight away from the target we don't have to spin
+// the turret.
+TEST(AimerTest, DrivingAwayFromTarget) {
+ const Pose target({0.0, 0.0, 0.0}, 0.0);
+ Pose robot_pose({-1.0, 0.0, 0.0}, 0.0);
+ const constants::Range range{-4.5, 4.5, -4.0, 4.0};
+ const double kBallSpeed = 10.0;
+ TurretGoal goal = AimerGoal(
+ ShotConfig{target, ShotMode::kStatic, range, kBallSpeed, 0.0, 0.0},
+ RobotState{robot_pose, {-1.0, 0.0}, 0.0, 0.0});
+ EXPECT_FLOAT_EQ(0.0, goal.position);
+ EXPECT_FLOAT_EQ(0.0, goal.velocity);
+ EXPECT_FLOAT_EQ(1.0, goal.virtual_shot_distance);
+ EXPECT_FLOAT_EQ(1.0, goal.target_distance);
+ // Next, try with shooting-on-the-fly enabled--because we are driving straight
+ // away from the target, only the goal distance should be impacted.
+ goal = AimerGoal(
+ ShotConfig{target, ShotMode::kShootOnTheFly, range, kBallSpeed, 0.0, 0.0},
+ RobotState{robot_pose, {-1.0, 0.0}, 0.0, 0.0});
+ EXPECT_FLOAT_EQ(0.0, goal.position);
+ EXPECT_FLOAT_EQ(0.0, goal.velocity);
+ EXPECT_FLOAT_EQ(1.111, goal.virtual_shot_distance);
+ EXPECT_FLOAT_EQ(1.0, goal.target_distance);
+}
+
+// Tests that when we drive perpendicular to the target, we do have to spin.
+TEST(AimerTest, DrivingLateralToTarget) {
+ const Pose target({0.0, 0.0, 0.0}, 0.0);
+ Pose robot_pose({0.0, -1.0, 0.0}, 0.0);
+ const constants::Range range{-4.5, 4.5, -4.0, 4.0};
+ const double kBallSpeed = 10.0;
+ TurretGoal goal = AimerGoal(
+ ShotConfig{target, ShotMode::kStatic, range, kBallSpeed, 0.0, 0.0},
+ RobotState{robot_pose, {1.0, 0.0}, 0.0, 0.0});
+ EXPECT_FLOAT_EQ(M_PI_2, goal.position);
+ EXPECT_FLOAT_EQ(1.0, goal.velocity);
+ EXPECT_FLOAT_EQ(1.0, goal.virtual_shot_distance);
+ EXPECT_FLOAT_EQ(1.0, goal.target_distance);
+ // Next, test with shooting-on-the-fly enabled, The goal numbers should all be
+ // slightly offset due to the robot velocity.
+ goal = AimerGoal(
+ ShotConfig{target, ShotMode::kShootOnTheFly, range, kBallSpeed, 0.0, 0.0},
+ RobotState{robot_pose, {1.0, 0.0}, 0.0, 0.0});
+ // Confirm that the turret heading goal is a bit more than pi / 2, but not by
+ // too much.
+ EXPECT_LT(M_PI_2 + 0.01, goal.position);
+ EXPECT_GT(M_PI_2 + 0.5, goal.position);
+ // Similarly, the turret velocity goal should be a bit less than 1.0,
+ // since the turret is no longer at exactly a right angle.
+ EXPECT_LT(0.9, goal.velocity);
+ EXPECT_GT(0.999, goal.velocity);
+ // And the distance to the goal should be a bit greater than 1.0.
+ EXPECT_LT(1.00001, goal.virtual_shot_distance);
+ EXPECT_GT(1.1, goal.virtual_shot_distance);
+ EXPECT_FLOAT_EQ(1.0, goal.target_distance);
+}
+
+// Confirms that when we move the turret heading so that it would be entirely
+// out of the normal range of motion that we send a valid (in-range) goal.
+// I.e., test that we have some hysteresis, but that it doesn't take us
+// out-of-range.
+TEST(AimerTest, WrapWhenOutOfRange) {
+ // Start ourselves needing a turret angle of 0.0.
+ const Pose target({0.0, 0.0, 0.0}, 0.0);
+ Pose robot_pose({-1.0, 0.0, 0.0}, 0.0);
+ const constants::Range range{-5.5, 5.5, -5.0, 5.0};
+ const double kBallSpeed = 10.0;
+ TurretGoal goal = AimerGoal(
+ ShotConfig{target, ShotMode::kShootOnTheFly, range, kBallSpeed, 0.0, 0.0},
+ RobotState{robot_pose, {0.0, 0.0}, 0.0, 0.0});
+ EXPECT_FLOAT_EQ(0.0, goal.position);
+ EXPECT_FLOAT_EQ(0.0, goal.velocity);
+
+ // Rotate a bit...
+ robot_pose = Pose({-1.0, 0.0, 0.0}, 2.0);
+ goal = AimerGoal(
+ ShotConfig{target, ShotMode::kShootOnTheFly, range, kBallSpeed, 0.0, 0.0},
+ RobotState{robot_pose, {0.0, 0.0}, 0.0, goal.position});
+ EXPECT_FLOAT_EQ(-2.0, goal.position);
+ EXPECT_FLOAT_EQ(0.0, goal.velocity);
+
+ // Rotate to the soft stop.
+ robot_pose = Pose({-1.0, 0.0, 0.0}, 4.0);
+ goal = AimerGoal(
+ ShotConfig{target, ShotMode::kShootOnTheFly, range, kBallSpeed, 0.0, 0.0},
+ RobotState{robot_pose, {0.0, 0.0}, 0.0, goal.position});
+ EXPECT_FLOAT_EQ(-4.0, goal.position);
+ EXPECT_FLOAT_EQ(0.0, goal.velocity);
+
+ // Rotate past the hard stop.
+ robot_pose = Pose({-1.0, 0.0, 0.0}, 0.0);
+ goal = AimerGoal(
+ ShotConfig{target, ShotMode::kShootOnTheFly, range, kBallSpeed, 0.0, 0.0},
+ RobotState{robot_pose, {0.0, 0.0}, 0.0, goal.position});
+ EXPECT_FLOAT_EQ(0.0, goal.position);
+ EXPECT_FLOAT_EQ(0.0, goal.velocity);
+}
+
+} // namespace frc971::control_loops::aiming::testing
diff --git a/y2020/control_loops/superstructure/turret/BUILD b/y2020/control_loops/superstructure/turret/BUILD
index 3761868..703c01e 100644
--- a/y2020/control_loops/superstructure/turret/BUILD
+++ b/y2020/control_loops/superstructure/turret/BUILD
@@ -43,6 +43,7 @@
"//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:pose",
"//frc971/control_loops:profiled_subsystem_fbs",
+ "//frc971/control_loops/aiming",
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//y2020:constants",
"//y2020/control_loops/drivetrain:drivetrain_base",
diff --git a/y2020/control_loops/superstructure/turret/aiming.cc b/y2020/control_loops/superstructure/turret/aiming.cc
index c8f8f6e..4976efa 100644
--- a/y2020/control_loops/superstructure/turret/aiming.cc
+++ b/y2020/control_loops/superstructure/turret/aiming.cc
@@ -9,6 +9,9 @@
namespace turret {
using frc971::control_loops::Pose;
+using frc971::control_loops::aiming::TurretGoal;
+using frc971::control_loops::aiming::ShotConfig;
+using frc971::control_loops::aiming::RobotState;
// Shooting-on-the-fly concept:
// The current way that we manage shooting-on-the fly endeavors to be reasonably
@@ -104,22 +107,6 @@
fbb.Finish(builder.Finish());
return fbb.Release();
}
-
-// This implements the iteration in the described shooting-on-the-fly algorithm.
-// robot_pose: Current robot pose.
-// robot_velocity: Current robot velocity, in the absolute field frame.
-// target_pose: Absolute goal Pose.
-// current_virtual_pose: Current estimate of where we want to shoot at.
-Pose IterateVirtualGoal(const Pose &robot_pose,
- const Eigen::Vector3d &robot_velocity,
- const Pose &target_pose,
- const Pose ¤t_virtual_pose) {
- const double air_time =
- current_virtual_pose.Rebase(&robot_pose).xy_norm() / kBallSpeedOverGround;
- const Eigen::Vector3d virtual_target =
- target_pose.abs_pos() - air_time * robot_velocity;
- return Pose(virtual_target, target_pose.abs_theta());
-}
} // namespace
Pose InnerPortPose(aos::Alliance alliance) {
@@ -176,83 +163,25 @@
aiming_for_inner_port_ =
(std::abs(inner_port_angle_) < max_inner_port_angle) &&
(inner_port_distance > min_inner_port_distance);
+ const Pose goal = aiming_for_inner_port_ ? inner_port : outer_port;
- // This code manages compensating the goal turret heading for the robot's
- // current velocity, to allow for shooting on-the-fly.
- // This works by solving for the correct turret angle numerically, since while
- // we technically could do it analytically, doing so would both make it hard
- // to make small changes (since it would force us to redo the math) and be
- // error-prone since it'd be easy to make typos or other minor math errors.
- Pose virtual_goal;
- {
- const Pose goal = aiming_for_inner_port_ ? inner_port : outer_port;
- target_distance_ = goal.Rebase(&robot_pose).xy_norm();
- virtual_goal = goal;
- if (shot_mode == ShotMode::kShootOnTheFly) {
- for (int ii = 0; ii < 3; ++ii) {
- virtual_goal =
- IterateVirtualGoal(robot_pose, {xdot, ydot, 0}, goal, virtual_goal);
- }
- VLOG(1) << "Shooting-on-the-fly target position: "
- << virtual_goal.abs_pos().transpose();
- }
- virtual_goal = virtual_goal.Rebase(&robot_pose);
- }
+ const struct TurretGoal turret_goal =
+ frc971::control_loops::aiming::AimerGoal(
+ ShotConfig{goal, shot_mode, constants::Values::kTurretRange(),
+ kBallSpeedOverGround,
+ wrap_mode == WrapMode::kAvoidEdges ? kAntiWrapBuffer : 0.0,
+ kTurretZeroOffset},
+ RobotState{robot_pose,
+ {xdot, ydot},
+ linear_angular(1),
+ goal_.message().unsafe_goal()});
- const double heading_to_goal = virtual_goal.heading();
- CHECK(status->has_localizer());
- shot_distance_ = virtual_goal.xy_norm();
+ target_distance_ = turret_goal.target_distance;
+ shot_distance_ = turret_goal.virtual_shot_distance;
- // The following code all works to calculate what the rate of turn of the
- // turret should be. The code only accounts for the rate of turn if we are
- // aiming at a static target, which should be close enough to correct that it
- // doesn't matter that it fails to account for the
- // shooting-on-the-fly compensation.
- const double rel_x = virtual_goal.rel_pos().x();
- const double rel_y = virtual_goal.rel_pos().y();
- const double squared_norm = rel_x * rel_x + rel_y * rel_y;
- // rel_xdot and rel_ydot are the derivatives (with respect to time) of rel_x
- // and rel_y. Since these are in the robot's coordinate frame, and since we
- // are ignoring lateral velocity for this exercise, rel_ydot is zero, and
- // rel_xdot is just the inverse of the robot's velocity.
- const double rel_xdot = -linear_angular(0);
- const double rel_ydot = 0.0;
-
- // If squared_norm gets to be too close to zero, just zero out the relevant
- // term to prevent NaNs. Note that this doesn't address the chattering that
- // would likely occur if we were to get excessively close to the target.
- // Note that x and y terms are swapped relative to what you would normally see
- // in the derivative of atan because xdot and ydot are the derivatives of
- // robot_pos and we are working with the atan of (target_pos - robot_pos).
- const double atan_diff =
- (squared_norm < 1e-3) ? 0.0 : (rel_x * rel_ydot - rel_y * rel_xdot) /
- squared_norm;
- // heading = atan2(relative_y, relative_x) - robot_theta
- // dheading / dt =
- // (rel_x * rel_y' - rel_y * rel_x') / (rel_x^2 + rel_y^2) - dtheta / dt
- const double dheading_dt = atan_diff - linear_angular(1);
-
- double range = kTurretRange;
- if (wrap_mode == WrapMode::kAvoidEdges) {
- range -= 2.0 * kAntiWrapBuffer;
- }
- // Calculate a goal turret heading such that it is within +/- pi of the
- // current position (i.e., a goal that would minimize the amount the turret
- // would have to travel).
- // We then check if this goal would bring us out of range of the valid angles,
- // and if it would, we reset to be within +/- pi of zero.
- double turret_heading =
- goal_.message().unsafe_goal() +
- aos::math::NormalizeAngle(heading_to_goal - kTurretZeroOffset -
- goal_.message().unsafe_goal());
- if (std::abs(turret_heading - constants::Values::kTurretRange().middle()) >
- range / 2.0) {
- turret_heading = aos::math::NormalizeAngle(turret_heading);
- }
-
- goal_.mutable_message()->mutate_unsafe_goal(turret_heading);
+ goal_.mutable_message()->mutate_unsafe_goal(turret_goal.position);
goal_.mutable_message()->mutate_goal_velocity(
- std::clamp(dheading_dt, -2.0, 2.0));
+ std::clamp(turret_goal.velocity, -2.0, 2.0));
}
flatbuffers::Offset<AimerStatus> Aimer::PopulateStatus(
diff --git a/y2020/control_loops/superstructure/turret/aiming.h b/y2020/control_loops/superstructure/turret/aiming.h
index ed00972..217085c 100644
--- a/y2020/control_loops/superstructure/turret/aiming.h
+++ b/y2020/control_loops/superstructure/turret/aiming.h
@@ -6,6 +6,7 @@
#include "frc971/control_loops/pose.h"
#include "frc971/control_loops/profiled_subsystem_generated.h"
#include "frc971/input/joystick_state_generated.h"
+#include "frc971/control_loops/aiming/aiming.h"
#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
namespace y2020 {
@@ -39,14 +40,7 @@
kAvoidWrapping,
};
- // Control modes for managing how we manage shooting on the fly.
- enum class ShotMode {
- // Don't do any shooting-on-the-fly compensation--just point straight at the
- // target. Primarily used in tests.
- kStatic,
- // Do do shooting-on-the-fly compensation.
- kShootOnTheFly,
- };
+ typedef frc971::control_loops::aiming::ShotMode ShotMode;
Aimer();