Perform basic turret auto-aiming
Implements a basic aimer which will shoot balls straight at the origin.
This still leaves a variety of things to be done:
-Actually shooting at the correct coordinates.
-Choosing between whether to shoot at the 2-point goal or 3-point goal.
-Managing turret wrap/redundancy intelligently.
-Shooting on the fly.
Change-Id: If4fc6249951faa5300411a0ca7d29da9e11dbb15
diff --git a/y2020/control_loops/superstructure/turret/BUILD b/y2020/control_loops/superstructure/turret/BUILD
index 894d418..8403e7c 100644
--- a/y2020/control_loops/superstructure/turret/BUILD
+++ b/y2020/control_loops/superstructure/turret/BUILD
@@ -30,3 +30,27 @@
"//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/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..0a22700
--- /dev/null
+++ b/y2020/control_loops/superstructure/turret/aiming.cc
@@ -0,0 +1,82 @@
+#include "y2020/control_loops/superstructure/turret/aiming.h"
+
+#include "frc971/control_loops/pose.h"
+#include "y2020/control_loops/drivetrain/drivetrain_base.h"
+
+namespace y2020 {
+namespace control_loops {
+namespace superstructure {
+namespace turret {
+
+using frc971::control_loops::Pose;
+
+namespace {
+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();
+}
+} // namespace
+
+Aimer::Aimer() : goal_(MakePrefilledGoal()) {}
+
+void Aimer::Update(const Status *status) {
+ // For now, just do enough to keep the turret pointed straight towards (0, 0).
+ // Don't worry about properly handling shooting on the fly--just try to keep
+ // the turret pointed straight towards one target.
+ // This also doesn't do anything intelligent with wrapping--it just produces a
+ // result in the range (-pi, pi] rather than taking advantage of the turret's
+ // full range.
+ Pose goal({0, 0, 0}, 0);
+ const Pose robot_pose({status->x(), status->y(), 0}, status->theta());
+ goal = goal.Rebase(&robot_pose);
+ const double heading_to_goal = goal.heading();
+ CHECK(status->has_localizer());
+ // 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());
+ // X and Y dot are negated because we are interested in the derivative of
+ // (target_pos - robot_pos).
+ const double xdot = -linear_angular(0) * std::cos(status->theta());
+ const double ydot = -linear_angular(0) * std::sin(status->theta());
+ const double rel_x = goal.rel_pos().x();
+ const double rel_y = 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.
+ const double atan_diff = (squared_norm < 1e-3)
+ ? 0.0
+ : (rel_x * ydot - rel_y * 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);
+
+ goal_.mutable_message()->mutate_unsafe_goal(heading_to_goal);
+ 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());
+ 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..c9f3873
--- /dev/null
+++ b/y2020/control_loops/superstructure/turret/aiming.h
@@ -0,0 +1,36 @@
+#ifndef y2020_CONTROL_LOOPS_SUPERSTRUCTURE_TURRET_AIMING_H_
+#define y2020_CONTROL_LOOPS_SUPERSTRUCTURE_TURRET_AIMING_H_
+
+#include "aos/flatbuffers.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.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 {
+
+// 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;
+ Aimer();
+ void Update(const Status *status);
+ const Goal *TurretGoal() const { return &goal_.message(); }
+
+ flatbuffers::Offset<AimerStatus> PopulateStatus(
+ flatbuffers::FlatBufferBuilder *fbb) const;
+
+ private:
+ aos::FlatbufferDetachedBuffer<Goal> goal_;
+};
+
+} // 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..487479f
--- /dev/null
+++ b/y2020/control_loops/superstructure/turret/aiming_test.cc
@@ -0,0 +1,99 @@
+#include "y2020/control_loops/superstructure/turret/aiming.h"
+
+#include "gtest/gtest.h"
+#include "y2020/control_loops/drivetrain/drivetrain_base.h"
+
+namespace y2020 {
+namespace control_loops {
+namespace superstructure {
+namespace turret {
+namespace testing {
+
+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) {
+ const auto buffer = MakeStatus(data);
+ aimer_.Update(&buffer.message());
+ const Goal *goal = aimer_.TurretGoal();
+ EXPECT_TRUE(goal->ignore_profile());
+ return goal;
+ }
+
+ protected:
+ Aimer aimer_;
+};
+
+TEST_F(AimerTest, StandingStill) {
+ const Goal *goal = Update(
+ {.x = 1.0, .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());
+ goal =
+ Update({.x = 1.0, .y = 0.0, .theta = 1.0, .linear = 0.0, .angular = 0.0});
+ EXPECT_EQ(M_PI - 1.0, goal->unsafe_goal());
+ EXPECT_EQ(0.0, goal->goal_velocity());
+ // Test that we handle the case that where we are right on top of the target.
+ goal =
+ Update({.x = 0.0, .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());
+}
+
+TEST_F(AimerTest, SpinningRobot) {
+ const Goal *goal = Update(
+ {.x = 1.0, .y = 0.0, .theta = 0.0, .linear = 0.0, .angular = 1.0});
+ EXPECT_EQ(M_PI, 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 Goal *goal = Update(
+ {.x = 1.0, .y = 0.0, .theta = 0.0, .linear = 1.0, .angular = 0.0});
+ EXPECT_EQ(M_PI, goal->unsafe_goal());
+ EXPECT_FLOAT_EQ(0.0, goal->goal_velocity());
+}
+
+// Tests that when we drive perpendicular to the target, we do have to spin.
+TEST_F(AimerTest, DrivingLateralToTarget) {
+ const Goal *goal = Update(
+ {.x = 0.0, .y = 1.0, .theta = 0.0, .linear = 1.0, .angular = 0.0});
+ EXPECT_EQ(-M_PI_2, goal->unsafe_goal());
+ EXPECT_FLOAT_EQ(-1.0, goal->goal_velocity());
+}
+
+} // namespace testing
+} // namespace turret
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2020