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