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/frc971/control_loops/pose.h b/frc971/control_loops/pose.h
index 20672cf..6b6628d 100644
--- a/frc971/control_loops/pose.h
+++ b/frc971/control_loops/pose.h
@@ -134,7 +134,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 9a6c3cd..d1731e1 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -16,7 +16,9 @@
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")) {
event_loop->SetRuntimeRealtimePriority(30);
}
@@ -34,6 +36,13 @@
const aos::monotonic_clock::time_point position_timestamp =
event_loop()->context().monotonic_event_time;
+ if (drivetrain_status_fetcher_.Fetch()) {
+ aimer_.Update(drivetrain_status_fetcher_.get());
+ }
+
+ const flatbuffers::Offset<AimerStatus> aimer_status_offset =
+ aimer_.PopulateStatus(status->fbb());
+
OutputT output_struct;
flatbuffers::Offset<AbsoluteEncoderProfiledJointStatus> hood_status_offset =
@@ -49,10 +58,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());
@@ -92,6 +105,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 d8ce917..f66842b 100644
--- a/y2020/control_loops/superstructure/superstructure.h
+++ b/y2020/control_loops/superstructure/superstructure.h
@@ -3,6 +3,7 @@
#include "aos/controls/control_loop.h"
#include "aos/events/event_loop.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 +11,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 {
@@ -45,6 +47,10 @@
AbsoluteEncoderSubsystem intake_joint_;
PotAndAbsoluteEncoderSubsystem turret_;
shooter::Shooter shooter_;
+ turret::Aimer aimer_;
+
+ aos::Fetcher<frc971::control_loops::drivetrain::Status>
+ drivetrain_status_fetcher_;
Climber climber_;
DISALLOW_COPY_AND_ASSIGN(Superstructure);
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index f68db08..0300295 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),
@@ -386,6 +386,8 @@
test_event_loop_->MakeFetcher<Output>("/superstructure")),
superstructure_position_fetcher_(
test_event_loop_->MakeFetcher<Position>("/superstructure")),
+ drivetrain_status_sender_(
+ test_event_loop_->MakeSender<DrivetrainStatus>("/drivetrain")),
superstructure_event_loop_(MakeEventLoop("superstructure", roborio_)),
superstructure_(superstructure_event_loop_.get()),
superstructure_plant_event_loop_(MakeEventLoop("plant", roborio_)),
@@ -494,6 +496,7 @@
::aos::Fetcher<Status> superstructure_status_fetcher_;
::aos::Fetcher<Output> superstructure_output_fetcher_;
::aos::Fetcher<Position> superstructure_position_fetcher_;
+ ::aos::Sender<DrivetrainStatus> drivetrain_status_sender_;
// Create a control loop and simulation.
::std::unique_ptr<::aos::EventLoop> superstructure_event_loop_;
@@ -814,6 +817,55 @@
VerifyNearGoal();
}
+// Tests that the turret switches to auto-aiming when we set turret_tracking to
+// true.
+TEST_F(SuperstructureTest, TurretAutoAim) {
+ SetEnabled(true);
+ // Set a reasonable goal.
+
+ WaitUntilZeroed();
+ {
+ 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>();
+
+ status_builder.add_x(0.0);
+ status_builder.add_y(1.0);
+ 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(-M_PI_2,
+ superstructure_status_fetcher_->turret()->position());
+ EXPECT_FLOAT_EQ(-M_PI_2,
+ superstructure_status_fetcher_->aimer()->turret_position());
+ EXPECT_FLOAT_EQ(0,
+ superstructure_status_fetcher_->aimer()->turret_velocity());
+}
+
} // 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 e8f7637..61208da 100644
--- a/y2020/control_loops/superstructure/superstructure_status.fbs
+++ b/y2020/control_loops/superstructure/superstructure_status.fbs
@@ -26,6 +26,13 @@
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;
+}
+
table Status {
// All subsystems know their location.
zeroed:bool;
@@ -43,6 +50,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..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