Handle turret wrapping more intelligently in Aimer
This now has actually reasonable logic for handling the turret's full
range of motion. Note that I did have to up the turret range constants a
bit to make it work, since they had just been set to +/-PI previously.
Change-Id: I9c11b56d720cf4aa5064a7f0b9cc5609205b7df7
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/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index e4742d1..8fb2ca1 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -43,7 +43,11 @@
if (joystick_state_fetcher_.Fetch()) {
alliance = joystick_state_fetcher_->alliance();
}
- aimer_.Update(drivetrain_status_fetcher_.get(), alliance);
+ const turret::Aimer::Mode mode =
+ (unsafe_goal != nullptr && unsafe_goal->shooting())
+ ? turret::Aimer::Mode::kAvoidWrapping
+ : turret::Aimer::Mode::kAvoidEdges;
+ aimer_.Update(drivetrain_status_fetcher_.get(), alliance, mode);
}
const flatbuffers::Offset<AimerStatus> aimer_status_offset =
diff --git a/y2020/control_loops/superstructure/turret/BUILD b/y2020/control_loops/superstructure/turret/BUILD
index 8403e7c..010c6fd 100644
--- a/y2020/control_loops/superstructure/turret/BUILD
+++ b/y2020/control_loops/superstructure/turret/BUILD
@@ -41,6 +41,7 @@
"//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",
],
diff --git a/y2020/control_loops/superstructure/turret/aiming.cc b/y2020/control_loops/superstructure/turret/aiming.cc
index d35bec9..97331f3 100644
--- a/y2020/control_loops/superstructure/turret/aiming.cc
+++ b/y2020/control_loops/superstructure/turret/aiming.cc
@@ -1,5 +1,6 @@
#include "y2020/control_loops/superstructure/turret/aiming.h"
+#include "y2020/constants.h"
#include "y2020/control_loops/drivetrain/drivetrain_base.h"
namespace y2020 {
@@ -29,11 +30,22 @@
// 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;
+// 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;
+
+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));
@@ -74,10 +86,7 @@
Aimer::Aimer() : goal_(MakePrefilledGoal()) {}
-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.
+void Aimer::Update(const Status *status, aos::Alliance alliance, Mode mode) {
const Pose robot_pose({status->x(), status->y(), 0}, status->theta());
const Pose inner_port = InnerPortPose(alliance);
const Pose outer_port = OuterPortPose(alliance);
@@ -119,7 +128,24 @@
// 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);
+ double range = kTurretRange;
+ if (mode == Mode::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 - 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);
}
diff --git a/y2020/control_loops/superstructure/turret/aiming.h b/y2020/control_loops/superstructure/turret/aiming.h
index 675eea3..5fbbdc6 100644
--- a/y2020/control_loops/superstructure/turret/aiming.h
+++ b/y2020/control_loops/superstructure/turret/aiming.h
@@ -26,8 +26,20 @@
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 Mode {
+ // 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,
+ };
Aimer();
- void Update(const Status *status, aos::Alliance alliance);
+ void Update(const Status *status, aos::Alliance alliance, Mode mode);
const Goal *TurretGoal() const { return &goal_.message(); }
// Returns the distance to the goal, in meters.
double DistanceToGoal() const { return distance_; }
diff --git a/y2020/control_loops/superstructure/turret/aiming_test.cc b/y2020/control_loops/superstructure/turret/aiming_test.cc
index b16b181..9bccfdd 100644
--- a/y2020/control_loops/superstructure/turret/aiming_test.cc
+++ b/y2020/control_loops/superstructure/turret/aiming_test.cc
@@ -2,6 +2,7 @@
#include "frc971/control_loops/pose.h"
#include "gtest/gtest.h"
+#include "y2020/constants.h"
#include "y2020/control_loops/drivetrain/drivetrain_base.h"
namespace y2020 {
@@ -44,9 +45,10 @@
}
const Goal *Update(const StatusData &data,
- aos::Alliance alliance = aos::Alliance::kBlue) {
+ aos::Alliance alliance = aos::Alliance::kBlue,
+ Aimer::Mode mode = Aimer::Mode::kAvoidEdges) {
const auto buffer = MakeStatus(data);
- aimer_.Update(&buffer.message(), alliance);
+ aimer_.Update(&buffer.message(), alliance, mode);
const Goal *goal = aimer_.TurretGoal();
EXPECT_TRUE(goal->ignore_profile());
return goal;
@@ -77,7 +79,7 @@
.theta = -1.0,
.linear = 0.0,
.angular = 0.0});
- EXPECT_EQ(-M_PI + 1.0, goal->unsafe_goal());
+ EXPECT_EQ(-M_PI + 1.0, aos::math::NormalizeAngle(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 = target.abs_pos().x() + 0.0,
@@ -139,6 +141,60 @@
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(M_PI, 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(M_PI + 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(-M_PI + 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::Mode::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::Mode::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::Mode::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