Remove mallocs from 2020 aimer.
Change-Id: Ic447442984b4edafb07e86b46627684718c43968
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2020/control_loops/superstructure/turret/aiming.cc b/y2020/control_loops/superstructure/turret/aiming.cc
index 4976efa..0b2c5f3 100644
--- a/y2020/control_loops/superstructure/turret/aiming.cc
+++ b/y2020/control_loops/superstructure/turret/aiming.cc
@@ -9,9 +9,9 @@
namespace turret {
using frc971::control_loops::Pose;
-using frc971::control_loops::aiming::TurretGoal;
-using frc971::control_loops::aiming::ShotConfig;
using frc971::control_loops::aiming::RobotState;
+using frc971::control_loops::aiming::ShotConfig;
+using frc971::control_loops::aiming::TurretGoal;
// Shooting-on-the-fly concept:
// The current way that we manage shooting-on-the fly endeavors to be reasonably
@@ -129,7 +129,9 @@
return target;
}
-Aimer::Aimer() : goal_(MakePrefilledGoal()) {}
+Aimer::Aimer()
+ : goal_(MakePrefilledGoal()),
+ Tlr_to_la_(drivetrain::GetDrivetrainConfig().Tlr_to_la()) {}
void Aimer::Update(const Status *status, aos::Alliance alliance,
WrapMode wrap_mode, ShotMode shot_mode) {
@@ -145,9 +147,8 @@
// 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());
+ 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());
diff --git a/y2020/control_loops/superstructure/turret/aiming.h b/y2020/control_loops/superstructure/turret/aiming.h
index 217085c..f38113d 100644
--- a/y2020/control_loops/superstructure/turret/aiming.h
+++ b/y2020/control_loops/superstructure/turret/aiming.h
@@ -64,6 +64,8 @@
// Real-world distance to the target.
double target_distance_ = 0.0; // meters
double inner_port_angle_ = 0.0; // radians
+
+ Eigen::Matrix<double, 2, 2> Tlr_to_la_;
};
} // namespace turret
diff --git a/y2020/control_loops/superstructure/turret/aiming_test.cc b/y2020/control_loops/superstructure/turret/aiming_test.cc
index a01f47d..fe71e31 100644
--- a/y2020/control_loops/superstructure/turret/aiming_test.cc
+++ b/y2020/control_loops/superstructure/turret/aiming_test.cc
@@ -12,12 +12,24 @@
namespace turret {
namespace testing {
+class TeamNumberEnvironment : public ::testing::Environment {
+ public:
+ ~TeamNumberEnvironment() override {}
+
+ // Override this to define how to set up the environment.
+ void SetUp() override { aos::network::OverrideTeamNumber(971); }
+
+ // Override this to define how to tear down the environment.
+ void TearDown() override {}
+};
+
+::testing::Environment *const team_number_env =
+ ::testing::AddGlobalTestEnvironment(new TeamNumberEnvironment);
+
using frc971::control_loops::Pose;
class AimerTest : public ::testing::Test {
public:
- AimerTest() { aos::network::OverrideTeamNumber(971); }
-
typedef Aimer::Goal Goal;
typedef Aimer::Status Status;
struct StatusData {