Merge changes I1132152d,I2ee8fa9d,I9c11b56d,I04de0d01,If4fc6249
* changes:
Make Aimer use correct turret zero convention
Add basic shooting-on-the-fly implementation
Handle turret wrapping more intelligently in Aimer
Add targets for the turret aiming to shoot at
Perform basic turret auto-aiming
diff --git a/frc971/constants.h b/frc971/constants.h
index debfb55..7c2121d 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -97,9 +97,9 @@
double lower;
double upper;
- double middle() const { return (lower_hard + upper_hard) / 2.0; }
+ constexpr double middle() const { return (lower_hard + upper_hard) / 2.0; }
- double range() const { return upper_hard - lower_hard; }
+ constexpr double range() const { return upper_hard - lower_hard; }
};
} // namespace constants
diff --git a/frc971/control_loops/pose.h b/frc971/control_loops/pose.h
index 20672cf..ab8afd0 100644
--- a/frc971/control_loops/pose.h
+++ b/frc971/control_loops/pose.h
@@ -42,6 +42,8 @@
// position of (10, -5, 0) and a yaw of pi / 2, that suggests the robot is
// facing straight to the left from the driver's perspective and is placed 10m
// from the driver's station wall and 5m to the right of the center of the wall.
+// For 2020, we move the origin to be the center of the field and make positive
+// x always point towards the red alliance driver stations.
//
// Furthermore, Poses can be chained such that a Pose can be placed relative to
// another Pose; the other Pose can dynamically update, thus allowing us to,
@@ -134,7 +136,7 @@
// If new_base == nullptr, provides a Pose referenced to the global frame.
// Note that the lifetime of new_base should be greater than the lifetime of
// the returned object (unless new_base == nullptr).
- TypedPose Rebase(const TypedPose<Scalar> *new_base) const;
+ [[nodiscard]] TypedPose Rebase(const TypedPose<Scalar> *new_base) const;
// Convert this pose to the heading/distance/skew numbers that we
// traditionally use for EKF corrections.
diff --git a/y2020/control_loops/superstructure/BUILD b/y2020/control_loops/superstructure/BUILD
index d81b6b5..4534404 100644
--- a/y2020/control_loops/superstructure/BUILD
+++ b/y2020/control_loops/superstructure/BUILD
@@ -62,8 +62,11 @@
":superstructure_status_fbs",
"//aos/controls:control_loop",
"//aos/events:event_loop",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//y2020:constants",
"//y2020/control_loops/superstructure/shooter",
+ "//y2020/control_loops/superstructure/turret:aiming",
],
)
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index 5dfd2d1..9ea9eb2 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -16,7 +16,11 @@
hood_(constants::GetValues().hood),
intake_joint_(constants::GetValues().intake),
turret_(constants::GetValues().turret.subsystem_params),
- shooter_() {
+ drivetrain_status_fetcher_(
+ event_loop->MakeFetcher<frc971::control_loops::drivetrain::Status>(
+ "/drivetrain")),
+ joystick_state_fetcher_(
+ event_loop->MakeFetcher<aos::JoystickState>("/aos")) {
event_loop->SetRuntimeRealtimePriority(30);
}
@@ -34,6 +38,22 @@
const aos::monotonic_clock::time_point position_timestamp =
event_loop()->context().monotonic_event_time;
+ if (drivetrain_status_fetcher_.Fetch()) {
+ aos::Alliance alliance = aos::Alliance::kInvalid;
+ if (joystick_state_fetcher_.Fetch()) {
+ alliance = joystick_state_fetcher_->alliance();
+ }
+ const turret::Aimer::WrapMode mode =
+ (unsafe_goal != nullptr && unsafe_goal->shooting())
+ ? turret::Aimer::WrapMode::kAvoidWrapping
+ : turret::Aimer::WrapMode::kAvoidEdges;
+ aimer_.Update(drivetrain_status_fetcher_.get(), alliance, mode,
+ turret::Aimer::ShotMode::kShootOnTheFly);
+ }
+
+ const flatbuffers::Offset<AimerStatus> aimer_status_offset =
+ aimer_.PopulateStatus(status->fbb());
+
OutputT output_struct;
flatbuffers::Offset<AbsoluteEncoderProfiledJointStatus> hood_status_offset =
@@ -73,10 +93,14 @@
output != nullptr ? &(output_struct.intake_joint_voltage) : nullptr,
status->fbb());
+ const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
+ *turret_goal = unsafe_goal != nullptr ? (unsafe_goal->turret_tracking()
+ ? aimer_.TurretGoal()
+ : unsafe_goal->turret())
+ : nullptr;
flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
turret_status_offset = turret_.Iterate(
- unsafe_goal != nullptr ? unsafe_goal->turret() : nullptr,
- position->turret(),
+ turret_goal, position->turret(),
output != nullptr ? &(output_struct.turret_voltage) : nullptr,
status->fbb());
@@ -134,6 +158,7 @@
status_builder.add_intake(intake_status_offset);
status_builder.add_turret(turret_status_offset);
status_builder.add_shooter(shooter_status_offset);
+ status_builder.add_aimer(aimer_status_offset);
status->Send(status_builder.Finish());
diff --git a/y2020/control_loops/superstructure/superstructure.h b/y2020/control_loops/superstructure/superstructure.h
index 1526610..1a9d401 100644
--- a/y2020/control_loops/superstructure/superstructure.h
+++ b/y2020/control_loops/superstructure/superstructure.h
@@ -3,6 +3,8 @@
#include "aos/controls/control_loop.h"
#include "aos/events/event_loop.h"
+#include "aos/robot_state/joystick_state_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "y2020/constants.h"
#include "y2020/control_loops/superstructure/shooter/shooter.h"
#include "y2020/control_loops/superstructure/superstructure_goal_generated.h"
@@ -10,6 +12,7 @@
#include "y2020/control_loops/superstructure/superstructure_position_generated.h"
#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
#include "y2020/control_loops/superstructure/climber.h"
+#include "y2020/control_loops/superstructure/turret/aiming.h"
namespace y2020 {
namespace control_loops {
@@ -53,6 +56,11 @@
AbsoluteEncoderSubsystem intake_joint_;
PotAndAbsoluteEncoderSubsystem turret_;
shooter::Shooter shooter_;
+ turret::Aimer aimer_;
+
+ aos::Fetcher<frc971::control_loops::drivetrain::Status>
+ drivetrain_status_fetcher_;
+ aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
Climber climber_;
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index 6e4e08b..b461ec6 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -36,6 +36,7 @@
CreateStaticZeroingSingleDOFProfiledSubsystemGoal;
using ::frc971::control_loops::PositionSensorSimulator;
using ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
+typedef ::frc971::control_loops::drivetrain::Status DrivetrainStatus;
typedef Superstructure::AbsoluteEncoderSubsystem AbsoluteEncoderSubsystem;
typedef Superstructure::PotAndAbsoluteEncoderSubsystem
PotAndAbsoluteEncoderSubsystem;
@@ -72,7 +73,6 @@
event_loop_->MakeFetcher<Status>("/superstructure")),
superstructure_output_fetcher_(
event_loop_->MakeFetcher<Output>("/superstructure")),
-
hood_plant_(new CappedTestPlant(hood::MakeHoodPlant())),
hood_encoder_(constants::GetValues()
.hood.zeroing_constants.one_revolution_distance),
@@ -398,6 +398,10 @@
test_event_loop_->MakeFetcher<Output>("/superstructure")),
superstructure_position_fetcher_(
test_event_loop_->MakeFetcher<Position>("/superstructure")),
+ drivetrain_status_sender_(
+ test_event_loop_->MakeSender<DrivetrainStatus>("/drivetrain")),
+ joystick_state_sender_(
+ test_event_loop_->MakeSender<aos::JoystickState>("/aos")),
superstructure_event_loop_(MakeEventLoop("superstructure", roborio_)),
superstructure_(superstructure_event_loop_.get()),
superstructure_plant_event_loop_(MakeEventLoop("plant", roborio_)),
@@ -506,6 +510,8 @@
::aos::Fetcher<Status> superstructure_status_fetcher_;
::aos::Fetcher<Output> superstructure_output_fetcher_;
::aos::Fetcher<Position> superstructure_position_fetcher_;
+ ::aos::Sender<DrivetrainStatus> drivetrain_status_sender_;
+ ::aos::Sender<aos::JoystickState> joystick_state_sender_;
// Create a control loop and simulation.
::std::unique_ptr<::aos::EventLoop> superstructure_event_loop_;
@@ -827,6 +833,79 @@
VerifyNearGoal();
}
+class SuperstructureAllianceTest
+ : public SuperstructureTest,
+ public ::testing::WithParamInterface<aos::Alliance> {};
+
+// Tests that the turret switches to auto-aiming when we set turret_tracking to
+// true.
+TEST_P(SuperstructureAllianceTest, TurretAutoAim) {
+ SetEnabled(true);
+ // Set a reasonable goal.
+ const frc971::control_loops::Pose target = turret::OuterPortPose(GetParam());
+
+ WaitUntilZeroed();
+
+ constexpr double kShotAngle = 1.0;
+ {
+ auto builder = joystick_state_sender_.MakeBuilder();
+
+ aos::JoystickState::Builder joystick_builder =
+ builder.MakeBuilder<aos::JoystickState>();
+
+ joystick_builder.add_alliance(GetParam());
+
+ ASSERT_TRUE(builder.Send(joystick_builder.Finish()));
+ }
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_turret_tracking(true);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ }
+
+ {
+ auto builder = drivetrain_status_sender_.MakeBuilder();
+
+ frc971::control_loops::drivetrain::LocalizerState::Builder
+ localizer_builder = builder.MakeBuilder<
+ frc971::control_loops::drivetrain::LocalizerState>();
+ localizer_builder.add_left_velocity(0.0);
+ localizer_builder.add_right_velocity(0.0);
+ const auto localizer_offset = localizer_builder.Finish();
+
+ DrivetrainStatus::Builder status_builder =
+ builder.MakeBuilder<DrivetrainStatus>();
+
+ // Set the robot up at kShotAngle off from the target, 1m away.
+ status_builder.add_x(target.abs_pos().x() + std::cos(kShotAngle));
+ status_builder.add_y(target.abs_pos().y() + std::sin(kShotAngle));
+ status_builder.add_theta(0.0);
+ status_builder.add_localizer(localizer_offset);
+
+ ASSERT_TRUE(builder.Send(status_builder.Finish()));
+ }
+
+ // Give it time to stabilize.
+ RunFor(chrono::seconds(1));
+
+ superstructure_status_fetcher_.Fetch();
+ EXPECT_FLOAT_EQ(kShotAngle,
+ superstructure_status_fetcher_->turret()->position());
+ EXPECT_FLOAT_EQ(kShotAngle,
+ superstructure_status_fetcher_->aimer()->turret_position());
+ EXPECT_FLOAT_EQ(0,
+ superstructure_status_fetcher_->aimer()->turret_velocity());
+}
+
+INSTANTIATE_TEST_CASE_P(ShootAnyAlliance, SuperstructureAllianceTest,
+ ::testing::Values(aos::Alliance::kRed,
+ aos::Alliance::kBlue,
+ aos::Alliance::kInvalid));
+
} // namespace testing
} // namespace superstructure
} // namespace control_loops
diff --git a/y2020/control_loops/superstructure/superstructure_status.fbs b/y2020/control_loops/superstructure/superstructure_status.fbs
index 0b12d33..81293d3 100644
--- a/y2020/control_loops/superstructure/superstructure_status.fbs
+++ b/y2020/control_loops/superstructure/superstructure_status.fbs
@@ -29,6 +29,15 @@
accelerator_right:FlywheelControllerStatus;
}
+table AimerStatus {
+ // The current goal angle for the turret auto-tracking, in radians.
+ turret_position:double;
+ // The current goal velocity for the turret, in radians / sec.
+ turret_velocity:double;
+ // Whether we are currently aiming for the inner port.
+ aiming_for_inner_port:bool;
+}
+
table Status {
// All subsystems know their location.
zeroed:bool;
@@ -46,6 +55,9 @@
// Status of the control_panel
control_panel:frc971.control_loops.RelativeEncoderProfiledJointStatus;
+
+ // Status of the vision auto-tracking.
+ aimer:AimerStatus;
}
root_type Status;
diff --git a/y2020/control_loops/superstructure/turret/BUILD b/y2020/control_loops/superstructure/turret/BUILD
index 894d418..010c6fd 100644
--- a/y2020/control_loops/superstructure/turret/BUILD
+++ b/y2020/control_loops/superstructure/turret/BUILD
@@ -30,3 +30,28 @@
"//frc971/control_loops:state_feedback_loop",
],
)
+
+cc_library(
+ name = "aiming",
+ srcs = ["aiming.cc"],
+ hdrs = ["aiming.h"],
+ deps = [
+ "//aos:flatbuffers",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:pose",
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+ "//y2020:constants",
+ "//y2020/control_loops/drivetrain:drivetrain_base",
+ "//y2020/control_loops/superstructure:superstructure_status_fbs",
+ ],
+)
+
+cc_test(
+ name = "aiming_test",
+ srcs = ["aiming_test.cc"],
+ deps = [
+ ":aiming",
+ "//aos/testing:googletest",
+ ],
+)
diff --git a/y2020/control_loops/superstructure/turret/aiming.cc b/y2020/control_loops/superstructure/turret/aiming.cc
new file mode 100644
index 0000000..360fd1a
--- /dev/null
+++ b/y2020/control_loops/superstructure/turret/aiming.cc
@@ -0,0 +1,252 @@
+#include "y2020/control_loops/superstructure/turret/aiming.h"
+
+#include "y2020/constants.h"
+#include "y2020/control_loops/drivetrain/drivetrain_base.h"
+
+namespace y2020 {
+namespace control_loops {
+namespace superstructure {
+namespace turret {
+
+using frc971::control_loops::Pose;
+
+// 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 {
+// The overall length and width of the field, in meters.
+constexpr double kFieldLength = 15.983;
+constexpr double kFieldWidth = 8.212;
+// Height of the center of the port(s) above the ground, in meters.
+constexpr double kPortHeight = 2.494;
+
+// Maximum shot angle at which we will attempt to make the shot into the inner
+// port, in radians. Zero would imply that we could only shoot if we were
+// exactly perpendicular to the target. Larger numbers allow us to aim at the
+// inner port more aggressively, at the risk of being more likely to miss the
+// outer port entirely.
+constexpr double kMaxInnerPortAngle = 20.0 * M_PI / 180.0;
+
+// Distance (in meters) from the edge of the field to the port.
+constexpr double kEdgeOfFieldToPort = 2.404;
+
+// The amount (in meters) that the inner port is set back from the outer port.
+constexpr double kInnerPortBackset = 0.743;
+
+// Average speed-over-ground of the ball on its way to the target. Our current
+// model assumes constant ball velocity regardless of shot distance.
+// TODO(james): Is this an appropriate model? For the outer port it should be
+// good enough that it doesn't really matter, but for the inner port it may be
+// more appropriate to do something more dynamic--however, it is not yet clear
+// how we would best estimate speed-over-ground given a hood angle + shooter
+// speed. Assuming a constant average speed over the course of the trajectory
+// should be reasonable, since all we are trying to do here is calculate an
+// overall time-of-flight (we don't actually care about the ball speed itself).
+constexpr double kBallSpeedOverGround = 15.0; // m/s
+
+// Minimum distance that we must be from the inner port in order to attempt the
+// shot--this is to account for the fact that if we are too close to the target,
+// then we won't have a clear shot on the inner port.
+constexpr double kMinimumInnerPortShotDistance = 4.0;
+
+// Amount of buffer, in radians, to leave to help avoid wrapping. I.e., any time
+// that we are in kAvoidEdges mode, we will keep ourselves at least
+// kAntiWrapBuffer radians away from the hardstops.
+constexpr double kAntiWrapBuffer = 0.2;
+
+// If the turret is at zero, then it will be at this angle relative to pointed
+// straight forwards on the robot.
+constexpr double kTurretZeroOffset = M_PI;
+
+constexpr double kTurretRange = constants::Values::kTurretRange().range();
+static_assert((kTurretRange - 2.0 * kAntiWrapBuffer) > 2.0 * M_PI,
+ "kAntiWrap buffer should be small enough that we still have 360 "
+ "degrees of range.");
+
+Pose ReverseSideOfField(Pose target) {
+ *target.mutable_pos() *= -1;
+ target.set_theta(aos::math::NormalizeAngle(target.rel_theta() + M_PI));
+ return target;
+}
+
+flatbuffers::DetachedBuffer MakePrefilledGoal() {
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.ForceDefaults(true);
+ Aimer::Goal::Builder builder(fbb);
+ builder.add_unsafe_goal(0);
+ builder.add_goal_velocity(0);
+ builder.add_ignore_profile(true);
+ 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) {
+ const Pose target({kFieldLength / 2 + kInnerPortBackset,
+ -kFieldWidth / 2.0 + kEdgeOfFieldToPort, kPortHeight},
+ 0.0);
+ if (alliance == aos::Alliance::kRed) {
+ return ReverseSideOfField(target);
+ }
+ return target;
+}
+
+Pose OuterPortPose(aos::Alliance alliance) {
+ Pose target(
+ {kFieldLength / 2, -kFieldWidth / 2.0 + kEdgeOfFieldToPort, kPortHeight},
+ 0.0);
+ if (alliance == aos::Alliance::kRed) {
+ return ReverseSideOfField(target);
+ }
+ return target;
+}
+
+Aimer::Aimer() : goal_(MakePrefilledGoal()) {}
+
+void Aimer::Update(const Status *status, aos::Alliance alliance,
+ WrapMode wrap_mode, ShotMode shot_mode) {
+ const Pose robot_pose({status->x(), status->y(), 0}, status->theta());
+ const Pose inner_port = InnerPortPose(alliance);
+ const Pose outer_port = OuterPortPose(alliance);
+ const Pose robot_pose_from_inner_port = robot_pose.Rebase(&inner_port);
+
+ // TODO(james): This code should probably just be in the localizer and have
+ // xdot/ydot get populated in the status message directly... that way we don't
+ // keep duplicating this math.
+ // Also, this doesn't currently take into account the lateral velocity of the
+ // robot. All of this would be helped by just doing this work in the Localizer
+ // itself.
+ const Eigen::Vector2d linear_angular =
+ drivetrain::GetDrivetrainConfig().Tlr_to_la() *
+ Eigen::Vector2d(status->localizer()->left_velocity(),
+ status->localizer()->right_velocity());
+ const double xdot = linear_angular(0) * std::cos(status->theta());
+ const double ydot = linear_angular(0) * std::sin(status->theta());
+
+ const double inner_port_angle = robot_pose_from_inner_port.heading();
+ const double inner_port_distance = robot_pose_from_inner_port.xy_norm();
+ aiming_for_inner_port_ =
+ (std::abs(inner_port_angle) < kMaxInnerPortAngle) &&
+ (inner_port_distance > kMinimumInnerPortShotDistance);
+
+ // 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;
+ 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 double heading_to_goal = virtual_goal.heading();
+ CHECK(status->has_localizer());
+ 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;
+
+ // 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_y * xdot - rel_x * ydot) / 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_goal_velocity(dheading_dt);
+}
+
+flatbuffers::Offset<AimerStatus> Aimer::PopulateStatus(
+ flatbuffers::FlatBufferBuilder *fbb) const {
+ AimerStatus::Builder builder(*fbb);
+ builder.add_turret_position(goal_.message().unsafe_goal());
+ builder.add_turret_velocity(goal_.message().goal_velocity());
+ builder.add_aiming_for_inner_port(aiming_for_inner_port_);
+ return builder.Finish();
+}
+
+} // namespace turret
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2020
diff --git a/y2020/control_loops/superstructure/turret/aiming.h b/y2020/control_loops/superstructure/turret/aiming.h
new file mode 100644
index 0000000..3b3071e
--- /dev/null
+++ b/y2020/control_loops/superstructure/turret/aiming.h
@@ -0,0 +1,74 @@
+#ifndef y2020_CONTROL_LOOPS_SUPERSTRUCTURE_TURRET_AIMING_H_
+#define y2020_CONTROL_LOOPS_SUPERSTRUCTURE_TURRET_AIMING_H_
+
+#include "aos/flatbuffers.h"
+#include "aos/robot_state/joystick_state_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
+#include "frc971/control_loops/pose.h"
+#include "frc971/control_loops/profiled_subsystem_generated.h"
+#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
+
+namespace y2020 {
+namespace control_loops {
+namespace superstructure {
+namespace turret {
+
+// Returns the port that we want to score on given our current alliance. The yaw
+// of the port will be such that the positive x axis points out the back of the
+// target.
+frc971::control_loops::Pose InnerPortPose(aos::Alliance alliance);
+frc971::control_loops::Pose OuterPortPose(aos::Alliance alliance);
+
+// This class manages taking in drivetrain status messages and generating turret
+// goals so that it gets aimed at the goal.
+class Aimer {
+ public:
+ typedef frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
+ Goal;
+ typedef frc971::control_loops::drivetrain::Status Status;
+ // Mode to run the aimer in, to control how we manage wrapping the turret
+ // angle.
+ enum class WrapMode {
+ // Keep the turret as far away from the edges of the range of motion as
+ // reasonable, to minimize the odds that we will hit the hardstops once we
+ // start shooting.
+ kAvoidEdges,
+ // Do everything reasonable to avoid having to wrap the shooter--set this
+ // while shooting so that we don't randomly spin the shooter 360 while
+ // shooting.
+ 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,
+ };
+
+ Aimer();
+
+ void Update(const Status *status, aos::Alliance alliance, WrapMode wrap_mode,
+ ShotMode shot_mode);
+
+ const Goal *TurretGoal() const { return &goal_.message(); }
+
+ // Returns the distance to the goal, in meters.
+ double DistanceToGoal() const { return distance_; }
+
+ flatbuffers::Offset<AimerStatus> PopulateStatus(
+ flatbuffers::FlatBufferBuilder *fbb) const;
+
+ private:
+ aos::FlatbufferDetachedBuffer<Goal> goal_;
+ bool aiming_for_inner_port_ = false;
+ double distance_ = 0.0;
+};
+
+} // namespace turret
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2020
+#endif // y2020_CONTROL_LOOPS_SUPERSTRUCTURE_TURRET_AIMING_H_
diff --git a/y2020/control_loops/superstructure/turret/aiming_test.cc b/y2020/control_loops/superstructure/turret/aiming_test.cc
new file mode 100644
index 0000000..cb95b56
--- /dev/null
+++ b/y2020/control_loops/superstructure/turret/aiming_test.cc
@@ -0,0 +1,249 @@
+#include "y2020/control_loops/superstructure/turret/aiming.h"
+
+#include "frc971/control_loops/pose.h"
+#include "gtest/gtest.h"
+#include "y2020/constants.h"
+#include "y2020/control_loops/drivetrain/drivetrain_base.h"
+
+namespace y2020 {
+namespace control_loops {
+namespace superstructure {
+namespace turret {
+namespace testing {
+
+using frc971::control_loops::Pose;
+
+class AimerTest : public ::testing::Test {
+ public:
+ typedef Aimer::Goal Goal;
+ typedef Aimer::Status Status;
+ struct StatusData {
+ double x;
+ double y;
+ double theta;
+ double linear;
+ double angular;
+ };
+ aos::FlatbufferDetachedBuffer<Status> MakeStatus(const StatusData &data) {
+ flatbuffers::FlatBufferBuilder fbb;
+ frc971::control_loops::drivetrain::LocalizerState::Builder state_builder(
+ fbb);
+ state_builder.add_left_velocity(
+ data.linear -
+ data.angular * drivetrain::GetDrivetrainConfig().robot_radius);
+ state_builder.add_right_velocity(
+ data.linear +
+ data.angular * drivetrain::GetDrivetrainConfig().robot_radius);
+ const auto state_offset = state_builder.Finish();
+ Status::Builder builder(fbb);
+ builder.add_x(data.x);
+ builder.add_y(data.y);
+ builder.add_theta(data.theta);
+ builder.add_localizer(state_offset);
+ fbb.Finish(builder.Finish());
+ return fbb.Release();
+ }
+
+ const Goal *Update(
+ const StatusData &data, aos::Alliance alliance = aos::Alliance::kBlue,
+ Aimer::WrapMode wrap_mode = Aimer::WrapMode::kAvoidEdges,
+ Aimer::ShotMode shot_mode = Aimer::ShotMode::kShootOnTheFly) {
+ const auto buffer = MakeStatus(data);
+ aimer_.Update(&buffer.message(), alliance, wrap_mode, shot_mode);
+ const Goal *goal = aimer_.TurretGoal();
+ EXPECT_TRUE(goal->ignore_profile());
+ return goal;
+ }
+
+ protected:
+ Aimer aimer_;
+};
+
+TEST_F(AimerTest, StandingStill) {
+ const Pose target = OuterPortPose(aos::Alliance::kBlue);
+ const Goal *goal = Update({.x = target.abs_pos().x() + 1.0,
+ .y = target.abs_pos().y() + 0.0,
+ .theta = 0.0,
+ .linear = 0.0,
+ .angular = 0.0});
+ EXPECT_EQ(0.0, goal->unsafe_goal());
+ EXPECT_EQ(0.0, goal->goal_velocity());
+ EXPECT_EQ(1.0, aimer_.DistanceToGoal());
+ goal = Update({.x = target.abs_pos().x() + 1.0,
+ .y = target.abs_pos().y() + 0.0,
+ .theta = 1.0,
+ .linear = 0.0,
+ .angular = 0.0});
+ EXPECT_EQ(-1.0, goal->unsafe_goal());
+ EXPECT_EQ(0.0, goal->goal_velocity());
+ goal = Update({.x = target.abs_pos().x() + 1.0,
+ .y = target.abs_pos().y() + 0.0,
+ .theta = -1.0,
+ .linear = 0.0,
+ .angular = 0.0});
+ EXPECT_EQ(1.0, aos::math::NormalizeAngle(goal->unsafe_goal()));
+ EXPECT_EQ(0.0, goal->goal_velocity());
+ EXPECT_EQ(1.0, aimer_.DistanceToGoal());
+ // Test that we handle the case that where we are right on top of the target.
+ goal = Update({.x = target.abs_pos().x() + 0.0,
+ .y = target.abs_pos().y() + 0.0,
+ .theta = 0.0,
+ .linear = 0.0,
+ .angular = 0.0});
+ EXPECT_EQ(M_PI, goal->unsafe_goal());
+ EXPECT_EQ(0.0, goal->goal_velocity());
+ EXPECT_EQ(0.0, aimer_.DistanceToGoal());
+}
+
+TEST_F(AimerTest, SpinningRobot) {
+ const Pose target = OuterPortPose(aos::Alliance::kBlue);
+ const Goal *goal = Update({.x = target.abs_pos().x() + 1.0,
+ .y = target.abs_pos().y() + 0.0,
+ .theta = 0.0,
+ .linear = 0.0,
+ .angular = 1.0});
+ EXPECT_EQ(0.0, goal->unsafe_goal());
+ EXPECT_FLOAT_EQ(-1.0, goal->goal_velocity());
+}
+
+// Tests that when we drive straight away from the target we don't have to spin
+// the turret.
+TEST_F(AimerTest, DrivingAwayFromTarget) {
+ const Pose target = OuterPortPose(aos::Alliance::kBlue);
+ // To keep the test simple, disable shooting on the fly so that the
+ // goal distance comes out in an easy to calculate number.
+ const Goal *goal = Update({.x = target.abs_pos().x() + 1.0,
+ .y = target.abs_pos().y() + 0.0,
+ .theta = 0.0,
+ .linear = 1.0,
+ .angular = 0.0},
+ aos::Alliance::kBlue, Aimer::WrapMode::kAvoidEdges,
+ Aimer::ShotMode::kStatic);
+ EXPECT_EQ(0.0, goal->unsafe_goal());
+ EXPECT_FLOAT_EQ(0.0, goal->goal_velocity());
+ EXPECT_EQ(1.0, aimer_.DistanceToGoal());
+ // Next, try with shooting-on-the-fly enabled--because we are driving straight
+ // towards the target, only the goal distance should be impacted.
+ goal = Update({.x = target.abs_pos().x() + 1.0,
+ .y = target.abs_pos().y() + 0.0,
+ .theta = 0.0,
+ .linear = 1.0,
+ .angular = 0.0},
+ aos::Alliance::kBlue, Aimer::WrapMode::kAvoidEdges,
+ Aimer::ShotMode::kShootOnTheFly);
+ EXPECT_EQ(0.0, goal->unsafe_goal());
+ EXPECT_FLOAT_EQ(0.0, goal->goal_velocity());
+ EXPECT_LT(1.0001, aimer_.DistanceToGoal());
+ EXPECT_GT(1.1, aimer_.DistanceToGoal());
+}
+
+// Tests that when we drive perpendicular to the target, we do have to spin.
+TEST_F(AimerTest, DrivingLateralToTarget) {
+ const Pose target = OuterPortPose(aos::Alliance::kBlue);
+ // To keep the test simple, disable shooting on the fly so that the
+ // goal_velocity comes out in an easy to calculate number.
+ const Goal *goal = Update({.x = target.abs_pos().x() + 0.0,
+ .y = target.abs_pos().y() + 1.0,
+ .theta = 0.0,
+ .linear = 1.0,
+ .angular = 0.0},
+ aos::Alliance::kBlue, Aimer::WrapMode::kAvoidEdges,
+ Aimer::ShotMode::kStatic);
+ EXPECT_EQ(M_PI_2, goal->unsafe_goal());
+ EXPECT_FLOAT_EQ(-1.0, goal->goal_velocity());
+ EXPECT_EQ(1.0, aimer_.DistanceToGoal());
+ // Next, test with shooting-on-the-fly enabled, The goal numbers should all be
+ // slightly offset due to the robot velocity.
+ goal = Update({.x = target.abs_pos().x() + 0.0,
+ .y = target.abs_pos().y() + 1.0,
+ .theta = 0.0,
+ .linear = 1.0,
+ .angular = 0.0},
+ aos::Alliance::kBlue, Aimer::WrapMode::kAvoidEdges,
+ Aimer::ShotMode::kShootOnTheFly);
+ // Confirm that the turret heading goal is less then -pi / 2, but not by too
+ // much.
+ EXPECT_GT(M_PI_2 - 0.001, goal->unsafe_goal());
+ EXPECT_LT(M_PI_2 - 0.1, goal->unsafe_goal());
+ // Similarly, the turret velocity goal should be a bit greater than -1.0,
+ // since the turret is no longer at exactly a right angle.
+ EXPECT_LT(-1.0, goal->goal_velocity());
+ EXPECT_GT(-0.95, goal->goal_velocity());
+ // And the distance to the goal should be a bit greater than 1.0.
+ EXPECT_LT(1.0001, aimer_.DistanceToGoal());
+ EXPECT_GT(1.1, aimer_.DistanceToGoal());
+}
+
+// Confirms that we will indeed shoot at the inner port when we have a good shot
+// angle on it.
+TEST_F(AimerTest, InnerPort) {
+ const Pose target = InnerPortPose(aos::Alliance::kRed);
+ const Goal *goal = Update({.x = target.abs_pos().x() + 1.0,
+ .y = target.abs_pos().y() + 0.0,
+ .theta = 0.0,
+ .linear = 0.0,
+ .angular = 0.0},
+ aos::Alliance::kRed);
+ EXPECT_EQ(0.0, goal->unsafe_goal());
+ EXPECT_EQ(0.0, goal->goal_velocity());
+}
+
+// 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.
+TEST_F(AimerTest, WrapWhenOutOfRange) {
+ // Start ourselves needing a turret angle of M_PI.
+ const Pose target = OuterPortPose(aos::Alliance::kBlue);
+ StatusData status{.x = target.abs_pos().x() + 1.0,
+ .y = target.abs_pos().y() + 0.0,
+ .theta = 0.0,
+ .linear = 0.0,
+ .angular = 0.0};
+ const Goal *goal = Update(status);
+ EXPECT_EQ(0.0, goal->unsafe_goal());
+ EXPECT_EQ(0.0, goal->goal_velocity());
+ // Move the robot a small amount--we should go past pi and not wrap yet.
+ status.theta = -0.1;
+ goal = Update(status);
+ EXPECT_FLOAT_EQ(0.1, goal->unsafe_goal());
+ EXPECT_EQ(0.0, goal->goal_velocity());
+ // Move the robot so that, if we had no hard-stops, we would go past it.
+ status.theta = -2.0;
+ goal = Update(status);
+ EXPECT_FLOAT_EQ(2.0, goal->unsafe_goal());
+ EXPECT_EQ(0.0, goal->goal_velocity());
+}
+
+// Confirms that the avoid edges turret mode doesn't let us get all the way to
+// the turret hard-stops but that the avoid wrapping mode does.
+TEST_F(AimerTest, WrappingModes) {
+ // Start ourselves needing a turret angle of M_PI.
+ const Pose target = OuterPortPose(aos::Alliance::kBlue);
+ StatusData status{.x = target.abs_pos().x() - 1.0,
+ .y = target.abs_pos().y() + 0.0,
+ .theta = 0.0,
+ .linear = 0.0,
+ .angular = 0.0};
+ const Goal *goal =
+ Update(status, aos::Alliance::kBlue, Aimer::WrapMode::kAvoidWrapping);
+ EXPECT_EQ(M_PI, goal->unsafe_goal());
+ EXPECT_EQ(0.0, goal->goal_velocity());
+ constexpr double kUpperLimit = constants::Values::kTurretRange().upper;
+ // Move the robot to the upper limit with AvoidWrapping set--we should be at
+ // the upper limit and not wrapped.
+ status.theta = goal->unsafe_goal() - kUpperLimit;
+ goal = Update(status, aos::Alliance::kBlue, Aimer::WrapMode::kAvoidWrapping);
+ EXPECT_FLOAT_EQ(kUpperLimit, goal->unsafe_goal());
+ EXPECT_EQ(0.0, goal->goal_velocity());
+ // Enter kAvoidEdges mode--we should wrap around.
+ goal = Update(status, aos::Alliance::kBlue, Aimer::WrapMode::kAvoidEdges);
+ // confirm that this test is actually testing something...
+ ASSERT_NE(aos::math::NormalizeAngle(kUpperLimit), kUpperLimit);
+ EXPECT_FLOAT_EQ(aos::math::NormalizeAngle(kUpperLimit), goal->unsafe_goal());
+ EXPECT_EQ(0.0, goal->goal_velocity());
+}
+
+} // namespace testing
+} // namespace turret
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2020