Add targets for the turret aiming to shoot at
This adds a basic aiming algorithm that aims at the 3-point shot when we
think it can make it, and a 2-point shot otherwise.
Note: This also formalizes the fact that we are using the origin that is
defined by sift.fbs this year.
Change-Id: I04de0d01e2596bfa75226c6ac00af1d1d08d487f
diff --git a/frc971/control_loops/pose.h b/frc971/control_loops/pose.h
index 6b6628d..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,
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index d1731e1..e4742d1 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -18,7 +18,9 @@
turret_(constants::GetValues().turret.subsystem_params),
drivetrain_status_fetcher_(
event_loop->MakeFetcher<frc971::control_loops::drivetrain::Status>(
- "/drivetrain")) {
+ "/drivetrain")),
+ joystick_state_fetcher_(
+ event_loop->MakeFetcher<aos::JoystickState>("/aos")) {
event_loop->SetRuntimeRealtimePriority(30);
}
@@ -37,7 +39,11 @@
event_loop()->context().monotonic_event_time;
if (drivetrain_status_fetcher_.Fetch()) {
- aimer_.Update(drivetrain_status_fetcher_.get());
+ aos::Alliance alliance = aos::Alliance::kInvalid;
+ if (joystick_state_fetcher_.Fetch()) {
+ alliance = joystick_state_fetcher_->alliance();
+ }
+ aimer_.Update(drivetrain_status_fetcher_.get(), alliance);
}
const flatbuffers::Offset<AimerStatus> aimer_status_offset =
diff --git a/y2020/control_loops/superstructure/superstructure.h b/y2020/control_loops/superstructure/superstructure.h
index f66842b..cea7c6a 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 "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"
@@ -51,6 +52,7 @@
aos::Fetcher<frc971::control_loops::drivetrain::Status>
drivetrain_status_fetcher_;
+ aos::Fetcher<aos::JoystickState> joystick_state_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 0300295..641f430 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -388,6 +388,8 @@
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_)),
@@ -497,6 +499,7 @@
::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_;
@@ -817,13 +820,30 @@
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_F(SuperstructureTest, TurretAutoAim) {
+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();
@@ -833,6 +853,7 @@
ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
+
{
auto builder = drivetrain_status_sender_.MakeBuilder();
@@ -846,8 +867,9 @@
DrivetrainStatus::Builder status_builder =
builder.MakeBuilder<DrivetrainStatus>();
- status_builder.add_x(0.0);
- status_builder.add_y(1.0);
+ // 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);
@@ -858,14 +880,19 @@
RunFor(chrono::seconds(1));
superstructure_status_fetcher_.Fetch();
- EXPECT_FLOAT_EQ(-M_PI_2,
+ EXPECT_FLOAT_EQ(kShotAngle - M_PI,
superstructure_status_fetcher_->turret()->position());
- EXPECT_FLOAT_EQ(-M_PI_2,
+ EXPECT_FLOAT_EQ(kShotAngle - M_PI,
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 61208da..5ae287c 100644
--- a/y2020/control_loops/superstructure/superstructure_status.fbs
+++ b/y2020/control_loops/superstructure/superstructure_status.fbs
@@ -31,6 +31,8 @@
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 {
diff --git a/y2020/control_loops/superstructure/turret/aiming.cc b/y2020/control_loops/superstructure/turret/aiming.cc
index 0a22700..d35bec9 100644
--- a/y2020/control_loops/superstructure/turret/aiming.cc
+++ b/y2020/control_loops/superstructure/turret/aiming.cc
@@ -1,6 +1,5 @@
#include "y2020/control_loops/superstructure/turret/aiming.h"
-#include "frc971/control_loops/pose.h"
#include "y2020/control_loops/drivetrain/drivetrain_base.h"
namespace y2020 {
@@ -11,6 +10,36 @@
using frc971::control_loops::Pose;
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;
+
+// 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;
+
+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);
@@ -23,20 +52,46 @@
}
} // 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) {
- // 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
+void Aimer::Update(const Status *status, aos::Alliance alliance) {
+ // This 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 Pose inner_port = InnerPortPose(alliance);
+ const Pose outer_port = OuterPortPose(alliance);
+ const Pose robot_pose_from_inner_port = robot_pose.Rebase(&inner_port);
+ 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);
+ const Pose goal =
+ (aiming_for_inner_port_ ? inner_port : outer_port).Rebase(&robot_pose);
const double heading_to_goal = goal.heading();
CHECK(status->has_localizer());
+ distance_ = goal.xy_norm();
// 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.
@@ -73,6 +128,7 @@
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();
}
diff --git a/y2020/control_loops/superstructure/turret/aiming.h b/y2020/control_loops/superstructure/turret/aiming.h
index c9f3873..675eea3 100644
--- a/y2020/control_loops/superstructure/turret/aiming.h
+++ b/y2020/control_loops/superstructure/turret/aiming.h
@@ -2,7 +2,9 @@
#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"
@@ -11,6 +13,12 @@
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 {
@@ -19,14 +27,18 @@
Goal;
typedef frc971::control_loops::drivetrain::Status Status;
Aimer();
- void Update(const Status *status);
+ void Update(const Status *status, aos::Alliance alliance);
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
diff --git a/y2020/control_loops/superstructure/turret/aiming_test.cc b/y2020/control_loops/superstructure/turret/aiming_test.cc
index 487479f..b16b181 100644
--- a/y2020/control_loops/superstructure/turret/aiming_test.cc
+++ b/y2020/control_loops/superstructure/turret/aiming_test.cc
@@ -1,5 +1,6 @@
#include "y2020/control_loops/superstructure/turret/aiming.h"
+#include "frc971/control_loops/pose.h"
#include "gtest/gtest.h"
#include "y2020/control_loops/drivetrain/drivetrain_base.h"
@@ -9,6 +10,8 @@
namespace turret {
namespace testing {
+using frc971::control_loops::Pose;
+
class AimerTest : public ::testing::Test {
public:
typedef Aimer::Goal Goal;
@@ -40,9 +43,10 @@
return fbb.Release();
}
- const Goal *Update(const StatusData &data) {
+ const Goal *Update(const StatusData &data,
+ aos::Alliance alliance = aos::Alliance::kBlue) {
const auto buffer = MakeStatus(data);
- aimer_.Update(&buffer.message());
+ aimer_.Update(&buffer.message(), alliance);
const Goal *goal = aimer_.TurretGoal();
EXPECT_TRUE(goal->ignore_profile());
return goal;
@@ -53,24 +57,45 @@
};
TEST_F(AimerTest, StandingStill) {
- const Goal *goal = Update(
- {.x = 1.0, .y = 0.0, .theta = 0.0, .linear = 0.0, .angular = 0.0});
+ 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(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});
+ 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(M_PI - 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(-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});
+ 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(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});
+ 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(M_PI, goal->unsafe_goal());
EXPECT_FLOAT_EQ(-1.0, goal->goal_velocity());
}
@@ -78,20 +103,42 @@
// 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});
+ 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 = 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});
+ const Pose target = OuterPortPose(aos::Alliance::kBlue);
+ 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});
EXPECT_EQ(-M_PI_2, goal->unsafe_goal());
EXPECT_FLOAT_EQ(-1.0, goal->goal_velocity());
}
+// 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(M_PI, goal->unsafe_goal());
+ EXPECT_EQ(0.0, goal->goal_velocity());
+}
+
} // namespace testing
} // namespace turret
} // namespace superstructure