Writing the tests for the superstructure
Signed-off-by: Sabina Leaver <100027607@mvla.net>
Change-Id: I76633fc1dec29e6ddb65a341fa00be33521203f4
Signed-off-by: Sabina Leaver <100027607@mvla.net>
diff --git a/y2021_bot3/control_loops/superstructure/BUILD b/y2021_bot3/control_loops/superstructure/BUILD
index 404555f..77cdf45 100644
--- a/y2021_bot3/control_loops/superstructure/BUILD
+++ b/y2021_bot3/control_loops/superstructure/BUILD
@@ -76,3 +76,29 @@
"//aos/events:shm_event_loop",
],
)
+
+cc_test(
+ name = "superstructure_lib_test",
+ srcs = [
+ "superstructure_lib_test.cc",
+ ],
+ data = [
+ "//y2021_bot3:config",
+ ],
+ deps = [
+ ":superstructure_goal_fbs",
+ ":superstructure_lib",
+ ":superstructure_output_fbs",
+ ":superstructure_position_fbs",
+ ":superstructure_status_fbs",
+ "//aos:math",
+ "//aos/events/logging:log_writer",
+ "//aos/testing:googletest",
+ "//aos/time",
+ "//frc971/control_loops:capped_test_plant",
+ "//frc971/control_loops:control_loop_test",
+ "//frc971/control_loops:position_sensor_sim",
+ "//frc971/control_loops:team_number_test_environment",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+ ],
+)
\ No newline at end of file
diff --git a/y2021_bot3/control_loops/superstructure/superstructure_lib_test.cc b/y2021_bot3/control_loops/superstructure/superstructure_lib_test.cc
new file mode 100644
index 0000000..cd031c3
--- /dev/null
+++ b/y2021_bot3/control_loops/superstructure/superstructure_lib_test.cc
@@ -0,0 +1,158 @@
+#include <chrono>
+#include <memory>
+
+#include "aos/events/logging/log_writer.h"
+#include "frc971/control_loops/capped_test_plant.h"
+#include "frc971/control_loops/control_loop_test.h"
+#include "frc971/control_loops/position_sensor_sim.h"
+#include "frc971/control_loops/team_number_test_environment.h"
+#include "gtest/gtest.h"
+#include "y2021_bot3/control_loops/superstructure/superstructure.h"
+
+namespace y2021_bot3 {
+namespace control_loops {
+namespace superstructure {
+namespace testing {
+
+class SuperstructureTest : public ::frc971::testing::ControlLoopTest {
+ public:
+ SuperstructureTest()
+ : ::frc971::testing::ControlLoopTest(
+ aos::configuration::ReadConfig("y2021_bot3/config.json"),
+ std::chrono::microseconds(5050)),
+ superstructure_event_loop(MakeEventLoop("Superstructure")),
+ superstructure_(superstructure_event_loop.get()),
+ test_event_loop_(MakeEventLoop("test")),
+ superstructure_goal_fetcher_(
+ test_event_loop_->MakeFetcher<Goal>("/superstructure")),
+ superstructure_goal_sender_(
+ test_event_loop_->MakeSender<Goal>("/superstructure")),
+ superstructure_status_fetcher_(
+ test_event_loop_->MakeFetcher<Status>("/superstructure")),
+ superstructure_output_fetcher_(
+ test_event_loop_->MakeFetcher<Output>("/superstructure")),
+ superstructure_position_fetcher_(
+ test_event_loop_->MakeFetcher<Position>("/superstructure")),
+ superstructure_position_sender_(
+ test_event_loop_->MakeSender<Position>("/superstructure")) {
+ set_team_id(frc971::control_loops::testing::kTeamNumber);
+ SetEnabled(true);
+
+ phased_loop_handle_ = test_event_loop_->AddPhasedLoop(
+ [this](int) { SendPositionMessage(); }, dt());
+ }
+
+ void VerifyResults(double intake_voltage, double outtake_voltage,
+ double intake_speed, double outtake_speed) {
+ superstructure_output_fetcher_.Fetch();
+ superstructure_status_fetcher_.Fetch();
+ ASSERT_TRUE(superstructure_output_fetcher_.get() != nullptr);
+ ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
+ EXPECT_EQ(superstructure_output_fetcher_->intake_volts(), intake_voltage);
+ EXPECT_EQ(superstructure_output_fetcher_->outtake_volts(), outtake_voltage);
+ EXPECT_EQ(superstructure_status_fetcher_->intake_speed(), intake_speed);
+ EXPECT_EQ(superstructure_status_fetcher_->outtake_speed(), outtake_speed);
+ EXPECT_EQ(superstructure_status_fetcher_->estopped(), false);
+ EXPECT_EQ(superstructure_status_fetcher_->zeroed(), true);
+ }
+
+ void SendPositionMessage() {
+ auto builder = superstructure_position_sender_.MakeBuilder();
+ Position::Builder position_builder = builder.MakeBuilder<Position>();
+ builder.Send(position_builder.Finish());
+ }
+
+ ::std::unique_ptr<::aos::EventLoop> superstructure_event_loop;
+ ::y2021_bot3::control_loops::superstructure::Superstructure superstructure_;
+ ::std::unique_ptr<::aos::EventLoop> test_event_loop_;
+ ::aos::PhasedLoopHandler *phased_loop_handle_ = nullptr;
+ ::aos::Fetcher<Goal> superstructure_goal_fetcher_;
+ ::aos::Sender<Goal> superstructure_goal_sender_;
+ ::aos::Fetcher<Status> superstructure_status_fetcher_;
+ ::aos::Fetcher<Output> superstructure_output_fetcher_;
+ ::aos::Fetcher<Position> superstructure_position_fetcher_;
+ ::aos::Sender<Position> superstructure_position_sender_;
+};
+
+// Tests running the intake and outtake separately
+TEST_F(SuperstructureTest, RunIntakeOrOuttake) {
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake_speed(10.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ SendPositionMessage();
+ RunFor(dt() * 2);
+ VerifyResults(10.0, 0.0, 10.0, 0.0);
+ }
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_outtake_speed(10.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ RunFor(dt() * 2);
+ VerifyResults(0.0, 10.0, 0.0, 10.0);
+ }
+}
+
+// Tests running both the intake and the outtake simultaneously
+TEST_F(SuperstructureTest, RunIntakeAndOuttake) {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake_speed(10.0);
+ goal_builder.add_outtake_speed(5.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ RunFor(dt() * 2);
+ VerifyResults(10.0, 5.0, 10.0, 5.0);
+}
+
+// Tests for an invalid voltage (over 12 or under -12) to check that it defaults
+// to 12 or -12
+TEST_F(SuperstructureTest, InvalidVoltage) {
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake_speed(20.0);
+ goal_builder.add_outtake_speed(15.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ RunFor(dt() * 2);
+ VerifyResults(12.0, 12.0, 20.0, 15.0);
+ }
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake_speed(-20.0);
+ goal_builder.add_outtake_speed(-15.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ RunFor(dt() * 2);
+ VerifyResults(-12.0, -12.0, -20.0, -15.0);
+ }
+}
+
+// Tests that there is no output when the goal is null
+TEST_F(SuperstructureTest, GoalIsNull) {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ RunFor(dt() * 2);
+ VerifyResults(0.0, 0.0, 0.0, 0.0);
+}
+
+// Tests that the robot behaves properly when disabled
+TEST_F(SuperstructureTest, Disabled) {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake_speed(6.0);
+ goal_builder.add_outtake_speed(5.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ SetEnabled(false);
+ RunFor(dt() * 2);
+ VerifyResults(0.0, 0.0, 6.0, 5.0);
+}
+
+} // namespace testing
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2021_bot3
\ No newline at end of file