diff --git a/aos/BUILD b/aos/BUILD
index ac5c3e9..47de872 100644
--- a/aos/BUILD
+++ b/aos/BUILD
@@ -475,6 +475,7 @@
         "testdata/expected.json",
         "testdata/expected_multinode.json",
         "testdata/good_multinode.json",
+        "testdata/good_multinode_hostnames.json",
         "testdata/invalid_destination_node.json",
         "testdata/invalid_nodes.json",
         "testdata/invalid_source_node.json",
diff --git a/aos/configuration.cc b/aos/configuration.cc
index ee343c2..ac897bf 100644
--- a/aos/configuration.cc
+++ b/aos/configuration.cc
@@ -362,7 +362,7 @@
   // Check that if there is a node list, all the source nodes are filled out and
   // valid, and all the destination nodes are valid (and not the source).  This
   // is a basic consistency check.
-  if (result.message().has_nodes()) {
+  if (result.message().has_nodes() && config.message().has_channels()) {
     for (const Channel *c : *config.message().channels()) {
       CHECK(c->has_source_node()) << ": Channel " << FlatbufferToJson(c)
                                   << " is missing \"source_node\"";
@@ -597,9 +597,16 @@
 const Node *GetNodeFromHostname(const Configuration *config,
                                 std::string_view hostname) {
   for (const Node *node : *config->nodes()) {
-    if (node->hostname()->string_view() == hostname) {
+    if (node->has_hostname() && node->hostname()->string_view() == hostname) {
       return node;
     }
+    if (node->has_hostnames()) {
+      for (const auto &candidate : *node->hostnames()) {
+        if (candidate->string_view() == hostname) {
+          return node;
+        }
+      }
+    }
   }
   return nullptr;
 }
diff --git a/aos/configuration.fbs b/aos/configuration.fbs
index 317c100..576616e 100644
--- a/aos/configuration.fbs
+++ b/aos/configuration.fbs
@@ -126,6 +126,12 @@
   hostname:string;
   // Port to serve forwarded data from.
   port:ushort = 9971;
+
+  // An alternative to hostname which allows specifying multiple hostnames,
+  // any of which will match this node.
+  //
+  // Don't specify a hostname in multiple nodes in the same configuration.
+  hostnames:[string];
 }
 
 // Overall configuration datastructure for the pubsub.
diff --git a/aos/configuration_test.cc b/aos/configuration_test.cc
index 0c0874b..c36bd93 100644
--- a/aos/configuration_test.cc
+++ b/aos/configuration_test.cc
@@ -677,6 +677,46 @@
             GetNodeOrDie(&config.message(), config2.message().nodes()->Get(0)));
 }
 
+TEST_F(ConfigurationTest, GetNodeFromHostname) {
+  FlatbufferDetachedBuffer<Configuration> config =
+      ReadConfig("aos/testdata/good_multinode.json");
+  EXPECT_EQ("pi1",
+            CHECK_NOTNULL(GetNodeFromHostname(&config.message(), "raspberrypi"))
+                ->name()
+                ->string_view());
+  EXPECT_EQ("pi2", CHECK_NOTNULL(
+                       GetNodeFromHostname(&config.message(), "raspberrypi2"))
+                       ->name()
+                       ->string_view());
+  EXPECT_EQ(nullptr, GetNodeFromHostname(&config.message(), "raspberrypi3"));
+  EXPECT_EQ(nullptr, GetNodeFromHostname(&config.message(), "localhost"));
+  EXPECT_EQ(nullptr, GetNodeFromHostname(&config.message(), "3"));
+}
+
+TEST_F(ConfigurationTest, GetNodeFromHostnames) {
+  FlatbufferDetachedBuffer<Configuration> config =
+      ReadConfig("aos/testdata/good_multinode_hostnames.json");
+  EXPECT_EQ("pi1",
+            CHECK_NOTNULL(GetNodeFromHostname(&config.message(), "raspberrypi"))
+                ->name()
+                ->string_view());
+  EXPECT_EQ("pi2", CHECK_NOTNULL(
+                       GetNodeFromHostname(&config.message(), "raspberrypi2"))
+                       ->name()
+                       ->string_view());
+  EXPECT_EQ("pi2", CHECK_NOTNULL(
+                       GetNodeFromHostname(&config.message(), "raspberrypi3"))
+                       ->name()
+                       ->string_view());
+  EXPECT_EQ("pi2", CHECK_NOTNULL(
+                       GetNodeFromHostname(&config.message(), "other"))
+                       ->name()
+                       ->string_view());
+  EXPECT_EQ(nullptr, GetNodeFromHostname(&config.message(), "raspberrypi4"));
+  EXPECT_EQ(nullptr, GetNodeFromHostname(&config.message(), "localhost"));
+  EXPECT_EQ(nullptr, GetNodeFromHostname(&config.message(), "3"));
+}
+
 }  // namespace testing
 }  // namespace configuration
 }  // namespace aos
diff --git a/aos/network/team_number.cc b/aos/network/team_number.cc
index b031299..14fadab 100644
--- a/aos/network/team_number.cc
+++ b/aos/network/team_number.cc
@@ -35,6 +35,29 @@
   return std::nullopt;
 }
 
+std::optional<uint16_t> ParsePiTeamNumber(const std::string &hostname) {
+  if (hostname.substr(0, 3) != "pi-") {
+    return std::nullopt;
+  }
+  size_t first_separator = hostname.find('-');
+  if (first_separator == hostname.npos ||
+      first_separator >= hostname.size() - 2) {
+    return std::nullopt;
+  }
+  ++first_separator;
+  const size_t second_separator = hostname.find('-', first_separator);
+  if (second_separator == hostname.npos) {
+    return std::nullopt;
+  }
+  const std::string number_string =
+      hostname.substr(first_separator, second_separator - first_separator);
+  int number;
+  if (!util::StringToNumber(number_string, &number)) {
+    return std::nullopt;
+  }
+  return number;
+}
+
 }  // namespace team_number_internal
 
 namespace {
@@ -65,6 +88,13 @@
       return *result;
     }
   }
+  {
+    const auto result = team_number_internal::ParsePiTeamNumber(hostname);
+    if (result) {
+      LOG(INFO) << "Pi hostname team number is: " << *result;
+      return *result;
+    }
+  }
   LOG(FATAL) << "Failed to parse a team number from hostname: " << hostname;
 }
 
diff --git a/aos/network/team_number.h b/aos/network/team_number.h
index 6eb1335..618affb 100644
--- a/aos/network/team_number.h
+++ b/aos/network/team_number.h
@@ -28,6 +28,8 @@
 
 std::optional<uint16_t> ParseRoborioTeamNumber(const std::string &hostname);
 
+std::optional<uint16_t> ParsePiTeamNumber(const std::string &hostname);
+
 }  // namespace team_number_internal
 }  // namespace network
 }  // namespace aos
diff --git a/aos/network/team_number_test.cc b/aos/network/team_number_test.cc
index 03001de..c11f0a0 100644
--- a/aos/network/team_number_test.cc
+++ b/aos/network/team_number_test.cc
@@ -7,6 +7,7 @@
 namespace testing {
 
 using team_number_internal::ParseRoborioTeamNumber;
+using team_number_internal::ParsePiTeamNumber;
 
 TEST(TeamNumberTest, Parse2015TeamNumber) {
   EXPECT_EQ(971u, *ParseRoborioTeamNumber("roboRIO-971"));
@@ -29,6 +30,17 @@
   EXPECT_FALSE(ParseRoborioTeamNumber("roboRIO--FRC"));
 }
 
+TEST(TeamNumberTest, ParsePiTeamNumber) {
+  EXPECT_EQ(971u, *ParsePiTeamNumber("pi-971-1"));
+  EXPECT_EQ(8971u, *ParsePiTeamNumber("pi-8971-22"));
+  EXPECT_EQ(8971u, *ParsePiTeamNumber("pi-8971-"));
+
+  EXPECT_FALSE(ParseRoborioTeamNumber("pi"));
+  EXPECT_FALSE(ParseRoborioTeamNumber("pi-"));
+  EXPECT_FALSE(ParseRoborioTeamNumber("pi-971"));
+  EXPECT_FALSE(ParseRoborioTeamNumber("pi-971a-1"));
+}
+
 }  // namespace testing
 }  // namespace network
 }  // namespace aos
diff --git a/aos/testdata/good_multinode_hostnames.json b/aos/testdata/good_multinode_hostnames.json
new file mode 100644
index 0000000..b82f6f3
--- /dev/null
+++ b/aos/testdata/good_multinode_hostnames.json
@@ -0,0 +1,18 @@
+{
+  "nodes": [
+    {
+      "name": "pi1",
+      "hostnames": [
+        "raspberrypi"
+      ]
+    },
+    {
+      "name": "pi2",
+      "hostnames": [
+        "raspberrypi2",
+        "raspberrypi3",
+        "other"
+      ]
+    }
+  ]
+}
diff --git a/frc971/config/BUILD b/frc971/config/BUILD
index 0f4f7ba..098d1f8 100644
--- a/frc971/config/BUILD
+++ b/frc971/config/BUILD
@@ -8,6 +8,7 @@
     srcs = ["setup_roborio.sh"],
     data = [
         ":rio_robotCommand",
+        ":sctp.ko",
         "@arm_frc_linux_gnueabi_repo//:compiler_pieces",
     ],
     visibility = ["//visibility:public"],
diff --git a/frc971/config/sctp.ko b/frc971/config/sctp.ko
new file mode 100644
index 0000000..81e41ec
--- /dev/null
+++ b/frc971/config/sctp.ko
Binary files differ
diff --git a/frc971/config/setup_roborio.sh b/frc971/config/setup_roborio.sh
index 8c2be86..f288df8 100755
--- a/frc971/config/setup_roborio.sh
+++ b/frc971/config/setup_roborio.sh
@@ -34,6 +34,15 @@
   ssh "admin@${ROBOT_HOSTNAME}" ln -s /media/sda1/aos_log-current robot_code/aos_log-current
 fi
 
+if [[ "$(ssh admin@${ROBOT_HOSTNAME} uname -r)" != "4.14.87-rt49-cg-7.0.0f0-xilinx-zynq-189" ]]; then
+  echo "Target roboRIO has the wrong kernel"
+  exit 1
+fi
+
+ssh "admin@${ROBOT_HOSTNAME}" mkdir "/lib/modules/4.14.87-rt49-cg-7.0.0f0-xilinx-zynq-189/kernel/net/sctp/"
+scp frc971/config/sctp.ko "admin@${ROBOT_HOSTNAME}:/lib/modules/4.14.87-rt49-cg-7.0.0f0-xilinx-zynq-189/kernel/net/sctp/sctp.ko"
+ssh "admin@${ROBOT_HOSTNAME}" depmod
+
 # This fails if the code isn't running.
 ssh "admin@${ROBOT_HOSTNAME}" 'PATH="${PATH}":/usr/local/natinst/bin/ /usr/local/frc/bin/frcKillRobot.sh -r -t' || true
 
diff --git a/frc971/constants.h b/frc971/constants.h
index a52add2..debfb55 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -98,6 +98,8 @@
   double upper;
 
   double middle() const { return (lower_hard + upper_hard) / 2.0; }
+
+  double range() const { return upper_hard - lower_hard; }
 };
 
 }  // namespace constants
diff --git a/frc971/control_loops/python/angular_system.py b/frc971/control_loops/python/angular_system.py
index 36da41a..bd4447b 100755
--- a/frc971/control_loops/python/angular_system.py
+++ b/frc971/control_loops/python/angular_system.py
@@ -22,6 +22,13 @@
                  kalman_q_voltage,
                  kalman_r_position,
                  dt=0.00505):
+        """Constructs an AngularSystemParams object.
+
+        Args:
+          motor: Motor object with the motor constants.
+          G: float, Gear ratio.  Less than 1 means output moves slower than the
+              input.
+        """
         self.name = name
         self.motor = motor
         self.G = G
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 347e1cd..27a659a 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -688,3 +688,7 @@
                    (12.0 - self.resistance * self.free_current))
         # Torque constant
         self.Kt = self.stall_torque / self.stall_current
+        # Motor inertia in kg m^2
+        # Diameter of 1.9", weight of: 100 grams
+        # TODO(austin): Get a number from Scott Westbrook for the mass
+        self.motor_inertia = 0.1 * ((0.95 * 0.0254) ** 2.0)
diff --git a/y2020/BUILD b/y2020/BUILD
index 79dc651..9a233d4 100644
--- a/y2020/BUILD
+++ b/y2020/BUILD
@@ -33,10 +33,12 @@
         "//frc971/control_loops:pose",
         "//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
         "//y2020/control_loops/drivetrain:polydrivetrain_plants",
+        "//y2020/control_loops/superstructure/accelerator:accelerator_plants",
+        "//y2020/control_loops/superstructure/control_panel:control_panel_plants",
+        "//y2020/control_loops/superstructure/finisher:finisher_plants",
         "//y2020/control_loops/superstructure/hood:hood_plants",
         "//y2020/control_loops/superstructure/intake:intake_plants",
         "//y2020/control_loops/superstructure/turret:turret_plants",
-	"//y2020/control_loops/superstructure/control_panel:control_panel_plants",
         "@com_google_absl//absl/base",
     ],
 )
diff --git a/y2020/constants.cc b/y2020/constants.cc
index cc5c1fb..a35798f 100644
--- a/y2020/constants.cc
+++ b/y2020/constants.cc
@@ -92,6 +92,11 @@
   turret_params->zeroing_constants.moving_buffer_size = 20;
   turret_params->zeroing_constants.allowable_encoder_error = 0.9;
 
+  CHECK_LE(hood->range.range(),
+           hood->zeroing_constants.one_revolution_distance);
+  CHECK_LE(intake->range.range(),
+           intake->zeroing_constants.one_revolution_distance);
+
   switch (team) {
     // A set of constants for tests.
     case 1:
diff --git a/y2020/constants.h b/y2020/constants.h
index 16a3e8b..ed58777 100644
--- a/y2020/constants.h
+++ b/y2020/constants.h
@@ -10,7 +10,9 @@
 #include "frc971/control_loops/pose.h"
 #include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
 #include "y2020/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2020/control_loops/superstructure/accelerator/accelerator_plant.h"
 #include "y2020/control_loops/superstructure/control_panel/control_panel_plant.h"
+#include "y2020/control_loops/superstructure/finisher/finisher_plant.h"
 #include "y2020/control_loops/superstructure/hood/hood_plant.h"
 #include "y2020/control_loops/superstructure/intake/intake_plant.h"
 #include "y2020/control_loops/superstructure/turret/turret_plant.h"
@@ -25,10 +27,10 @@
   static constexpr double kDrivetrainEncoderCountsPerRevolution() {
     return kDrivetrainCyclesPerRevolution() * 4;
   }
-  static constexpr double kDrivetrainEncoderRatio() { return (24.0 / 52.0); }
+  static constexpr double kDrivetrainEncoderRatio() { return 1.0; }
   static constexpr double kMaxDrivetrainEncoderPulsesPerSecond() {
     return control_loops::drivetrain::kFreeSpeed / (2.0 * M_PI) *
-           control_loops::drivetrain::kHighOutputRatio /
+           control_loops::drivetrain::kHighGearRatio /
            constants::Values::kDrivetrainEncoderRatio() *
            kDrivetrainEncoderCountsPerRevolution();
   }
@@ -36,22 +38,20 @@
   // Hood
   static constexpr double kHoodEncoderCountsPerRevolution() { return 4096.0; }
 
-  // TODO(sabina): Update constants
-  static constexpr double kHoodEncoderRatio() { return 1.0; }
+  static constexpr double kHoodEncoderRatio() { return 8.0 / 72.0; }
 
   static constexpr double kMaxHoodEncoderPulsesPerSecond() {
-    return control_loops::superstructure::hood::kFreeSpeed *
+    return control_loops::superstructure::hood::kFreeSpeed / (2.0 * M_PI) *
            control_loops::superstructure::hood::kOutputRatio /
-           kHoodEncoderRatio() / (2.0 * M_PI) *
-           kHoodEncoderCountsPerRevolution();
+           kHoodEncoderRatio() * kHoodEncoderCountsPerRevolution();
   }
 
   static constexpr ::frc971::constants::Range kHoodRange() {
     return ::frc971::constants::Range{
-        0.00,  // Back Hard
-        0.79,  // Front Hard
-        0.14,  // Back Soft
-        0.78   // Front Soft
+        -0.01,  // Back Hard
+        0.65,   // Front Hard
+        0.0,    // Back Soft
+        0.64    // Front Soft
     };
   }
 
@@ -65,10 +65,9 @@
   static constexpr double kIntakeEncoderRatio() { return (16.0 / 32.0); }
 
   static constexpr double kMaxIntakeEncoderPulsesPerSecond() {
-    return control_loops::superstructure::intake::kFreeSpeed *
+    return control_loops::superstructure::intake::kFreeSpeed / (2.0 * M_PI) *
            control_loops::superstructure::intake::kOutputRatio /
-           kIntakeEncoderRatio() / (2.0 * M_PI) *
-           kIntakeEncoderCountsPerRevolution();
+           kIntakeEncoderRatio() * kIntakeEncoderCountsPerRevolution();
   }
 
   // TODO(sabina): update range
@@ -92,18 +91,16 @@
   static constexpr double kTurretEncoderCountsPerRevolution() { return 4096.0; }
 
   static constexpr double kTurretEncoderRatio() {
-    return 1.0;  // TODO (Kai): Get Gear Ratios when ready
+    return (26.0 / 150.0) * (130.0 / 40.0);
   }
 
   static constexpr double kMaxTurretEncoderPulsesPerSecond() {
-    return control_loops::superstructure::turret::kFreeSpeed *
+    return control_loops::superstructure::turret::kFreeSpeed / (2.0 * M_PI) *
            control_loops::superstructure::turret::kOutputRatio /
-           kTurretEncoderRatio() / (2.0 * M_PI) *
-           kTurretEncoderCountsPerRevolution();
+           kTurretEncoderRatio() * kTurretEncoderCountsPerRevolution();
   }
 
-  // TODO(austin): Figure out the actual constant here.
-  static constexpr double kTurretPotRatio() { return 1.0; }
+  static constexpr double kTurretPotRatio() { return (26.0 / 150.0); }
 
   static constexpr ::frc971::constants::Range kTurretRange() {
     return ::frc971::constants::Range{
@@ -135,23 +132,40 @@
   static constexpr double kControlPanelEncoderRatio() { return (56.0 / 28.0); }
 
   static constexpr double kMaxControlPanelEncoderPulsesPerSecond() {
-    return control_loops::superstructure::control_panel::kFreeSpeed *
+    return control_loops::superstructure::control_panel::kFreeSpeed /
+           (2.0 * M_PI) *
            control_loops::superstructure::control_panel::kOutputRatio /
-           kControlPanelEncoderRatio() / (2.0 * M_PI) *
+           kControlPanelEncoderRatio() *
            kControlPanelEncoderCountsPerRevolution();
   }
 
   // Shooter
-  // TODO: Get actual constants.
   static constexpr double kFinisherEncoderCountsPerRevolution() {
     return 4096.0;
   }
-  static constexpr double kFinisherEncoderRatio() { return 1.0; }
+  static constexpr double kFinisherEncoderRatio() { return 30.0 / 40.0; }
+
+  static constexpr double kMaxFinisherEncoderPulsesPerSecond() {
+    return control_loops::superstructure::finisher::kFreeSpeed / (2.0 * M_PI) *
+           control_loops::superstructure::finisher::kOutputRatio /
+           kFinisherEncoderRatio() * kFinisherEncoderCountsPerRevolution();
+  }
+
 
   static constexpr double kAcceleratorEncoderCountsPerRevolution() {
     return 4096.0;
   }
-  static constexpr double kAcceleratorEncoderRatio() { return 1.0; }
+  static constexpr double kAcceleratorEncoderRatio() {
+    return (1.2 * 1.2 * 1.2) * (30.0 / 40.0);
+  }
+
+  static constexpr double kMaxAcceleratorEncoderPulsesPerSecond() {
+    return control_loops::superstructure::accelerator::kFreeSpeed /
+           (2.0 * M_PI) *
+           control_loops::superstructure::accelerator::kOutputRatio /
+           kAcceleratorEncoderRatio() *
+           kAcceleratorEncoderCountsPerRevolution();
+  }
 
   // Climber
   static constexpr double kClimberSupplyCurrentLimit() { return 60.0; }
diff --git a/y2020/control_loops/python/accelerator.py b/y2020/control_loops/python/accelerator.py
index 257ed23..64ae12e 100644
--- a/y2020/control_loops/python/accelerator.py
+++ b/y2020/control_loops/python/accelerator.py
@@ -13,22 +13,32 @@
 
 gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
 
+# Inertia for a single 4" diameter, 1" wide neopreme wheel.
+J_wheel = 0.000319
+# Gear ratio between wheels (speed up!)
+G_per_wheel = 1.2
+# Gear ratio to the final wheel.
+G = (30.0 / 40.0) * numpy.power(G_per_wheel, 3.0)
+# Overall flywheel inertia.
+J = J_wheel * (
+    1.0 + numpy.power(G, -2.0) + numpy.power(G, -4.0) + numpy.power(G, -6.0))
+
+# The position and velocity are measured for the final wheel.
 kAccelerator = flywheel.FlywheelParams(
     name='Accelerator',
     motor=control_loop.Falcon(),
-    G=1.0,
-    J=0.006,
+    G=G,
+    J=J,
     q_pos=0.08,
     q_vel=4.00,
-    q_voltage=0.3,
+    q_voltage=0.4,
     r_pos=0.05,
-    controller_poles=[.87],
-    dt=0.00505)
+    controller_poles=[.80])
 
 
 def main(argv):
     if FLAGS.plot:
-        R = numpy.matrix([[0.0], [100.0], [0.0]])
+        R = numpy.matrix([[0.0], [500.0], [0.0]])
         flywheel.PlotSpinup(kAccelerator, goal=R, iterations=200)
         return 0
 
diff --git a/y2020/control_loops/python/control_panel.py b/y2020/control_loops/python/control_panel.py
index 206a39e..e02aa13 100644
--- a/y2020/control_loops/python/control_panel.py
+++ b/y2020/control_loops/python/control_panel.py
@@ -17,12 +17,11 @@
 except gflags.DuplicateFlagError:
     pass
 
-#TODO(sabina): update moment
 kControlPanel = angular_system.AngularSystemParams(
     name='ControlPanel',
     motor=control_loop.BAG(),
-    G=(1.0),
-    J=0.3,
+    G=1.0,
+    J=0.000009,
     q_pos=0.20,
     q_vel=5.0,
     kalman_q_pos=0.12,
diff --git a/y2020/control_loops/python/drivetrain.py b/y2020/control_loops/python/drivetrain.py
index 54745dd..95b9fcf 100644
--- a/y2020/control_loops/python/drivetrain.py
+++ b/y2020/control_loops/python/drivetrain.py
@@ -1,6 +1,7 @@
 #!/usr/bin/python
 
 from frc971.control_loops.python import drivetrain
+from frc971.control_loops.python import control_loop
 import sys
 
 import gflags
@@ -11,19 +12,20 @@
 gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
 
 kDrivetrain = drivetrain.DrivetrainParams(
-    J=1.5,
-    mass=38.5,
-    robot_radius=0.45 / 2.0,
-    wheel_radius=4.0 * 0.0254 / 2.0,
-    G=9.0 / 52.0,
+    J=6.0,
+    mass=68.0,
+    # TODO(austin): Measure radius a bit better.
+    robot_radius=0.7 / 2.0,
+    wheel_radius=6.0 * 0.0254 / 2.0,
+    motor_type=control_loop.Falcon(),
+    G=(8.0 / 70.0) * (17.0 / 24.0),
     q_pos=0.14,
     q_vel=1.30,
     efficiency=0.80,
     has_imu=True,
     force=True,
     kf_q_voltage=13.0,
-    controller_poles=[0.82, 0.82],
-    robot_cg_offset=0.0)
+    controller_poles=[0.82, 0.82])
 
 
 def main(argv):
@@ -38,5 +40,6 @@
         # Write the generated constants out to a file.
         drivetrain.WriteDrivetrain(argv[1:3], argv[3:5], 'y2020', kDrivetrain)
 
+
 if __name__ == '__main__':
     sys.exit(main(sys.argv))
diff --git a/y2020/control_loops/python/finisher.py b/y2020/control_loops/python/finisher.py
index 0b0fbb4..0d46bb9 100644
--- a/y2020/control_loops/python/finisher.py
+++ b/y2020/control_loops/python/finisher.py
@@ -13,22 +13,32 @@
 
 gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
 
+# Inertia for a single 4" diameter, 2" wide neopreme wheel.
+J_wheel = 0.000319 * 2.0
+# Gear ratio to the final wheel.
+# 40 tooth on the flywheel
+# 48 for the falcon.
+# 60 tooth on the outer wheel.
+G = 48.0 / 40.0
+# Overall flywheel inertia.
+J = J_wheel * (1.0 + (40.0 / 60.0)**2.0)
+
+# The position and velocity are measured for the final wheel.
 kFinisher = flywheel.FlywheelParams(
     name='Finisher',
     motor=control_loop.Falcon(),
-    G=1.0,
-    J=0.006,
+    G=G,
+    J=J,
     q_pos=0.08,
     q_vel=4.00,
-    q_voltage=0.3,
+    q_voltage=0.4,
     r_pos=0.05,
-    controller_poles=[.87],
-    dt=0.00505)
+    controller_poles=[.80])
 
 
 def main(argv):
     if FLAGS.plot:
-        R = numpy.matrix([[0.0], [100.0], [0.0]])
+        R = numpy.matrix([[0.0], [500.0], [0.0]])
         flywheel.PlotSpinup(params=kFinisher, goal=R, iterations=200)
         return 0
 
diff --git a/y2020/control_loops/python/flywheel.py b/y2020/control_loops/python/flywheel.py
index 9a6fc46..630d223 100755
--- a/y2020/control_loops/python/flywheel.py
+++ b/y2020/control_loops/python/flywheel.py
@@ -214,7 +214,7 @@
             offset.append(observer_flywheel.X_hat[2, 0])
 
         applied_U = U.copy()
-        if i > 30:
+        if i > 100:
             applied_U += 2
         flywheel.Update(applied_U)
 
@@ -273,10 +273,10 @@
     loop_writer = control_loop.ControlLoopWriter(
         name, flywheels, namespaces=namespace)
     loop_writer.AddConstant(
-        control_loop.Constant('kOutputRatio' + params.name, '%f',
+        control_loop.Constant('kOutputRatio', '%f',
                               flywheels[0].G))
     loop_writer.AddConstant(
-        control_loop.Constant('kFreeSpeed' + params.name, '%f',
+        control_loop.Constant('kFreeSpeed', '%f',
                               flywheels[0].motor.free_speed))
     loop_writer.Write(plant_files[0], plant_files[1])
 
diff --git a/y2020/control_loops/python/hood.py b/y2020/control_loops/python/hood.py
index 8c99940..b4d8c84 100644
--- a/y2020/control_loops/python/hood.py
+++ b/y2020/control_loops/python/hood.py
@@ -17,29 +17,33 @@
 except gflags.DuplicateFlagError:
     pass
 
-'''
-Hood is an angular subsystem due to the mounting of the encoder on the hood joint.
-We are currently electing to ignore potential non-linearity.
-'''
 
-#TODO: update constants
+# Hood is an angular subsystem due to the mounting of the encoder on the hood
+# joint.  We are currently electing to ignore potential non-linearity.
+
+range_of_travel_radians = (37.0 * numpy.pi / 180.0)
+# 0.083 inches/turn
+# 6.38 inches of travel
+turns_of_leadscrew_per_range_of_travel = 6.38 / 0.083
+
+radians_per_turn = range_of_travel_radians / turns_of_leadscrew_per_range_of_travel
+
 kHood = angular_system.AngularSystemParams(
     name='Hood',
     motor=control_loop.BAG(),
-    # meters / rad (used the displacement of the lead screw and the angle)
-    G=(0.1601 / 0.6457),
-    J=0.3,
-    q_pos=0.20,
-    q_vel=5.0,
+    G=radians_per_turn,
+    J=0.08389,
+    q_pos=0.55,
+    q_vel=14.0,
     kalman_q_pos=0.12,
     kalman_q_vel=2.0,
-    kalman_q_voltage=4.0,
+    kalman_q_voltage=2.5,
     kalman_r_position=0.05)
 
 
 def main(argv):
     if FLAGS.plot:
-        R = numpy.matrix([[numpy.pi / 2.0], [0.0]])
+        R = numpy.matrix([[numpy.pi / 4.0], [0.0]])
         angular_system.PlotKick(kHood, R)
         angular_system.PlotMotion(kHood, R)
 
diff --git a/y2020/control_loops/python/intake.py b/y2020/control_loops/python/intake.py
index f65679a..9e80244 100644
--- a/y2020/control_loops/python/intake.py
+++ b/y2020/control_loops/python/intake.py
@@ -17,14 +17,13 @@
 except gflags.DuplicateFlagError:
     pass
 
-#TODO: update constants
 kIntake = angular_system.AngularSystemParams(
     name='Intake',
     motor=control_loop.BAG(),
-    G=(1.0 / 1.0),
-    J=0.3,
-    q_pos=0.20,
-    q_vel=5.0,
+    G=(12.0 / 24.0) * (1.0 / 7.0) * (1.0 / 7.0) * (16.0 / 32.0),
+    J=3.0 * 0.139 * 0.139,
+    q_pos=0.40,
+    q_vel=20.0,
     kalman_q_pos=0.12,
     kalman_q_vel=2.0,
     kalman_q_voltage=4.0,
diff --git a/y2020/control_loops/python/turret.py b/y2020/control_loops/python/turret.py
index 58ea7d8..e276457 100644
--- a/y2020/control_loops/python/turret.py
+++ b/y2020/control_loops/python/turret.py
@@ -21,18 +21,21 @@
 kTurret = angular_system.AngularSystemParams(
     name='Turret',
     motor=control_loop.Vex775Pro(),
-    #TODO: Update Gear Ratios when they are ready
-    G=(6.0 / 60.0) * (20.0 / 100.0) * (24.0 / 84.0),
-    #TODO: Get number from Bryan (moment of inertia)
-    J=0.30,
-    q_pos=0.20,
-    q_vel=5.0,
+    G=(6.0 / 60.0) * (26.0 / 150.0),
+    J=0.11,
+    q_pos=0.40,
+    q_vel=7.0,
     kalman_q_pos=0.12,
     kalman_q_vel=2.0,
-    kalman_q_voltage=4.0,
+    kalman_q_voltage=3.0,
     kalman_r_position=0.05)
 
 def main(argv):
+    if FLAGS.plot:
+        R = numpy.matrix([[numpy.pi], [0.0]])
+        angular_system.PlotKick(kTurret, R)
+        angular_system.PlotMotion(kTurret, R)
+
     # Write the generated constants out to a file.
     if len(argv) != 5:
         glog.fatal(
diff --git a/y2020/control_loops/superstructure/BUILD b/y2020/control_loops/superstructure/BUILD
index 87ee689..d81b6b5 100644
--- a/y2020/control_loops/superstructure/BUILD
+++ b/y2020/control_loops/superstructure/BUILD
@@ -63,6 +63,7 @@
         "//aos/controls:control_loop",
         "//aos/events:event_loop",
         "//y2020:constants",
+        "//y2020/control_loops/superstructure/shooter",
     ],
 )
 
@@ -94,6 +95,7 @@
         ":superstructure_status_fbs",
         "//aos:math",
         "//aos/controls:control_loop_test",
+        "//aos/events/logging:logger",
         "//aos/testing:googletest",
         "//aos/time",
         "//frc971/control_loops:capped_test_plant",
@@ -102,6 +104,7 @@
         "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
         "//y2020/control_loops/superstructure/hood:hood_plants",
         "//y2020/control_loops/superstructure/intake:intake_plants",
+        "//y2020/control_loops/superstructure/shooter:shooter_plants",
     ],
 )
 
diff --git a/y2020/control_loops/superstructure/accelerator/BUILD b/y2020/control_loops/superstructure/accelerator/BUILD
index 2c680bf..30fca08 100644
--- a/y2020/control_loops/superstructure/accelerator/BUILD
+++ b/y2020/control_loops/superstructure/accelerator/BUILD
@@ -7,11 +7,28 @@
     outs = [
         "accelerator_plant.h",
         "accelerator_plant.cc",
-        "accelerator_integral_plant.h",
-        "accelerator_integral_plant.cc",
+        "integral_accelerator_plant.h",
+        "integral_accelerator_plant.cc",
     ],
     cmd = "$(location //y2020/control_loops/python:accelerator) $(OUTS)",
     tools = [
         "//y2020/control_loops/python:accelerator",
     ],
 )
+
+cc_library(
+    name = "accelerator_plants",
+    srcs = [
+        "accelerator_plant.cc",
+        "integral_accelerator_plant.cc",
+    ],
+    hdrs = [
+        "accelerator_plant.h",
+        "integral_accelerator_plant.h",
+    ],
+    visibility = ["//visibility:public"],
+    deps = [
+        "//frc971/control_loops:hybrid_state_feedback_loop",
+        "//frc971/control_loops:state_feedback_loop",
+    ],
+)
diff --git a/y2020/control_loops/superstructure/finisher/BUILD b/y2020/control_loops/superstructure/finisher/BUILD
index b9bfc4f..2009886 100644
--- a/y2020/control_loops/superstructure/finisher/BUILD
+++ b/y2020/control_loops/superstructure/finisher/BUILD
@@ -7,11 +7,28 @@
     outs = [
         "finisher_plant.h",
         "finisher_plant.cc",
-        "finisher_integral_plant.h",
-        "finisher_integral_plant.cc",
+        "integral_finisher_plant.h",
+        "integral_finisher_plant.cc",
     ],
     cmd = "$(location //y2020/control_loops/python:finisher) $(OUTS)",
     tools = [
         "//y2020/control_loops/python:finisher",
     ],
 )
+
+cc_library(
+    name = "finisher_plants",
+    srcs = [
+        "finisher_plant.cc",
+        "integral_finisher_plant.cc",
+    ],
+    hdrs = [
+        "finisher_plant.h",
+        "integral_finisher_plant.h",
+    ],
+    visibility = ["//visibility:public"],
+    deps = [
+        "//frc971/control_loops:hybrid_state_feedback_loop",
+        "//frc971/control_loops:state_feedback_loop",
+    ],
+)
diff --git a/y2020/control_loops/superstructure/shooter/BUILD b/y2020/control_loops/superstructure/shooter/BUILD
index d63231b..fe8a29a 100644
--- a/y2020/control_loops/superstructure/shooter/BUILD
+++ b/y2020/control_loops/superstructure/shooter/BUILD
@@ -4,18 +4,6 @@
 
 cc_library(
     name = "shooter_plants",
-    srcs = [
-        "//y2020/control_loops/superstructure/accelerator:accelerator_integral_plant.cc",
-        "//y2020/control_loops/superstructure/accelerator:accelerator_plant.cc",
-        "//y2020/control_loops/superstructure/finisher:finisher_integral_plant.cc",
-        "//y2020/control_loops/superstructure/finisher:finisher_plant.cc",
-    ],
-    hdrs = [
-        "//y2020/control_loops/superstructure/accelerator:accelerator_integral_plant.h",
-        "//y2020/control_loops/superstructure/accelerator:accelerator_plant.h",
-        "//y2020/control_loops/superstructure/finisher:finisher_integral_plant.h",
-        "//y2020/control_loops/superstructure/finisher:finisher_plant.h",
-    ],
     deps = [
         "//frc971/control_loops:state_feedback_loop",
     ],
@@ -49,10 +37,11 @@
         "flywheel_controller.h",
     ],
     deps = [
-        ":shooter_plants",
         "//aos/controls:control_loop",
         "//frc971/control_loops:profiled_subsystem",
         "//y2020/control_loops/superstructure:superstructure_goal_fbs",
         "//y2020/control_loops/superstructure:superstructure_status_fbs",
+        "//y2020/control_loops/superstructure/accelerator:accelerator_plants",
+        "//y2020/control_loops/superstructure/finisher:finisher_plants",
     ],
 )
diff --git a/y2020/control_loops/superstructure/shooter/flywheel_controller.cc b/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
index 0f920a4..77fb769 100644
--- a/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
+++ b/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
@@ -13,7 +13,8 @@
 
 FlywheelController::FlywheelController(StateFeedbackLoop<3, 1, 1> &&loop)
     : loop_(new StateFeedbackLoop<3, 1, 1>(std::move(loop))) {
-  history_.fill(0);
+  history_.fill(std::pair<double, ::aos::monotonic_clock::time_point>(
+      0, ::aos::monotonic_clock::epoch()));
   Y_.setZero();
 }
 
@@ -22,12 +23,16 @@
   last_goal_ = angular_velocity_goal;
 }
 
-void FlywheelController::set_position(double current_position) {
+void FlywheelController::set_position(
+    double current_position,
+    const aos::monotonic_clock::time_point position_timestamp) {
   // Update position in the model.
   Y_ << current_position;
 
   // Add the position to the history.
-  history_[history_position_] = current_position;
+  history_[history_position_] =
+      std::pair<double, ::aos::monotonic_clock::time_point>(current_position,
+                                                            position_timestamp);
   history_position_ = (history_position_ + 1) % kHistoryLength;
 }
 
@@ -50,15 +55,20 @@
   const int oldest_history_position =
       ((history_position_ == 0) ? kHistoryLength : history_position_) - 1;
 
+  const double total_loop_time = ::aos::time::DurationInSeconds(
+      std::get<1>(history_[history_position_]) -
+      std::get<1>(history_[oldest_history_position]));
+
+  const double distance_traveled =
+      std::get<0>(history_[history_position_]) -
+      std::get<0>(history_[oldest_history_position]);
+
   // Compute the distance moved over that time period.
-  const double avg_angular_velocity =
-      (history_[oldest_history_position] - history_[history_position_]) /
-      (::aos::time::DurationInSeconds(::aos::controls::kLoopFrequency) *
-       static_cast<double>(kHistoryLength - 1));
+  avg_angular_velocity_ = (distance_traveled) / (total_loop_time);
 
   FlywheelControllerStatusBuilder builder(*fbb);
 
-  builder.add_avg_angular_velocity(avg_angular_velocity);
+  builder.add_avg_angular_velocity(avg_angular_velocity_);
   builder.add_angular_velocity(loop_->X_hat(1, 0));
   builder.add_angular_velocity_goal(last_goal_);
   return builder.Finish();
diff --git a/y2020/control_loops/superstructure/shooter/flywheel_controller.h b/y2020/control_loops/superstructure/shooter/flywheel_controller.h
index 20cd681..a3e9bdb 100644
--- a/y2020/control_loops/superstructure/shooter/flywheel_controller.h
+++ b/y2020/control_loops/superstructure/shooter/flywheel_controller.h
@@ -6,8 +6,8 @@
 #include "aos/controls/control_loop.h"
 #include "aos/time/time.h"
 #include "frc971/control_loops/state_feedback_loop.h"
-#include "y2020/control_loops/superstructure/accelerator/accelerator_integral_plant.h"
-#include "y2020/control_loops/superstructure/finisher/finisher_integral_plant.h"
+#include "y2020/control_loops/superstructure/accelerator/integral_accelerator_plant.h"
+#include "y2020/control_loops/superstructure/finisher/integral_finisher_plant.h"
 #include "y2020/control_loops/superstructure/superstructure_status_generated.h"
 
 namespace y2020 {
@@ -23,7 +23,8 @@
   // Sets the velocity goal in radians/sec
   void set_goal(double angular_velocity_goal);
   // Sets the current encoder position in radians
-  void set_position(double current_position);
+  void set_position(double current_position,
+                    const aos::monotonic_clock::time_point position_timestamp);
 
   // Populates the status structure.
   flatbuffers::Offset<FlywheelControllerStatus> SetStatus(
@@ -38,6 +39,8 @@
   // Executes the control loop for a cycle.
   void Update(bool disabled);
 
+  double avg_angular_velocity() { return avg_angular_velocity_; }
+
  private:
   // The current sensor measurement.
   Eigen::Matrix<double, 1, 1> Y_;
@@ -46,8 +49,14 @@
 
   // History array for calculating a filtered angular velocity.
   static constexpr int kHistoryLength = 10;
-  ::std::array<double, kHistoryLength> history_;
+  ::std::array<std::pair<double, ::aos::monotonic_clock::time_point>,
+               kHistoryLength>
+      history_;
   ptrdiff_t history_position_ = 0;
+
+  // Average velocity logging.
+  double avg_angular_velocity_;
+
   double last_goal_ = 0;
 
   DISALLOW_COPY_AND_ASSIGN(FlywheelController);
diff --git a/y2020/control_loops/superstructure/shooter/shooter.cc b/y2020/control_loops/superstructure/shooter/shooter.cc
index 6a1d201..25f6a6a 100644
--- a/y2020/control_loops/superstructure/shooter/shooter.cc
+++ b/y2020/control_loops/superstructure/shooter/shooter.cc
@@ -11,29 +11,59 @@
 namespace superstructure {
 namespace shooter {
 
+namespace {
+const double kVelocityTolerance = 0.01;
+}  // namespace
+
 Shooter::Shooter()
     : finisher_(finisher::MakeIntegralFinisherLoop()),
       accelerator_left_(accelerator::MakeIntegralAcceleratorLoop()),
       accelerator_right_(accelerator::MakeIntegralAcceleratorLoop()) {}
 
+bool Shooter::UpToSpeed(const ShooterGoal *goal) {
+  return (
+      std::abs(goal->velocity_finisher() - finisher_.avg_angular_velocity()) <
+          kVelocityTolerance &&
+      std::abs(goal->velocity_accelerator() -
+               accelerator_left_.avg_angular_velocity()) < kVelocityTolerance &&
+      std::abs(goal->velocity_accelerator() -
+               accelerator_right_.avg_angular_velocity()) < kVelocityTolerance &&
+      std::abs(goal->velocity_finisher() - finisher_.velocity()) < kVelocityTolerance &&
+      std::abs(goal->velocity_accelerator() - accelerator_left_.velocity()) <
+          kVelocityTolerance &&
+      std::abs(goal->velocity_accelerator() - accelerator_right_.velocity()) <
+          kVelocityTolerance);
+}
+
 flatbuffers::Offset<ShooterStatus> Shooter::RunIteration(
     const ShooterGoal *goal, const ShooterPosition *position,
-    flatbuffers::FlatBufferBuilder *fbb, OutputT *output) {
-  if (goal) {
-    // Update position/goal for our two shooter sides.
-    finisher_.set_goal(goal->velocity_finisher());
-    accelerator_left_.set_goal(goal->velocity_accelerator());
-    accelerator_right_.set_goal(goal->velocity_accelerator());
-  }
-
-  finisher_.set_position(position->theta_finisher());
-  accelerator_left_.set_position(position->theta_accelerator_left());
-  accelerator_right_.set_position(position->theta_accelerator_right());
+    flatbuffers::FlatBufferBuilder *fbb, OutputT *output,
+    const aos::monotonic_clock::time_point position_timestamp) {
+  // Update position, output, and status for our two shooter sides.
+  finisher_.set_position(position->theta_finisher(), position_timestamp);
+  accelerator_left_.set_position(position->theta_accelerator_left(),
+                                 position_timestamp);
+  accelerator_right_.set_position(position->theta_accelerator_right(),
+                                  position_timestamp);
 
   finisher_.Update(output == nullptr);
   accelerator_left_.Update(output == nullptr);
   accelerator_right_.Update(output == nullptr);
 
+  // Update goal.
+  if (goal) {
+    finisher_.set_goal(goal->velocity_finisher());
+    accelerator_left_.set_goal(goal->velocity_accelerator());
+    accelerator_right_.set_goal(goal->velocity_accelerator());
+
+    if (UpToSpeed(goal) && goal->velocity_finisher() > kVelocityTolerance &&
+        goal->velocity_accelerator() > kVelocityTolerance) {
+      ready_ = true;
+    } else {
+      ready_ = false;
+    }
+  }
+
   flatbuffers::Offset<FlywheelControllerStatus> finisher_status_offset =
       finisher_.SetStatus(fbb);
   flatbuffers::Offset<FlywheelControllerStatus> accelerator_left_status_offset =
diff --git a/y2020/control_loops/superstructure/shooter/shooter.h b/y2020/control_loops/superstructure/shooter/shooter.h
index 88bcb3b..f72eeeb 100644
--- a/y2020/control_loops/superstructure/shooter/shooter.h
+++ b/y2020/control_loops/superstructure/shooter/shooter.h
@@ -21,11 +21,17 @@
 
   flatbuffers::Offset<ShooterStatus> RunIteration(
       const ShooterGoal *goal, const ShooterPosition *position,
-      flatbuffers::FlatBufferBuilder *fbb, OutputT *output);
+      flatbuffers::FlatBufferBuilder *fbb, OutputT *output,
+      const aos::monotonic_clock::time_point position_timestamp);
+
+  bool ready() { return ready_; }
 
  private:
   FlywheelController finisher_, accelerator_left_, accelerator_right_;
 
+  bool UpToSpeed(const ShooterGoal *goal);
+  bool ready_ = false;
+
   DISALLOW_COPY_AND_ASSIGN(Shooter);
 };
 
diff --git a/y2020/control_loops/superstructure/shooter_plot.pb b/y2020/control_loops/superstructure/shooter_plot.pb
new file mode 100644
index 0000000..01a1e20
--- /dev/null
+++ b/y2020/control_loops/superstructure/shooter_plot.pb
@@ -0,0 +1,44 @@
+channel {
+  name: "/superstructure"
+  type: "y2020.control_loops.superstructure.Status"
+  alias: "Status"
+}
+channel {
+  name: "/superstructure"
+  type: "y2020.control_loops.superstructure.Output"
+  alias: "Output"
+}
+channel {
+  name: "/superstructure"
+  type: "y2020.control_loops.superstructure.Position"
+  alias: "Position"
+}
+channel {
+  name: "/superstructure"
+  type: "y2020.control_loops.superstructure.Goal"
+  alias: "Goal"
+}
+
+figure {
+  axes {
+    line {
+      y_signal {
+        channel: "Status"
+        field: "hood.position"
+      }
+    }
+    line {
+      y_signal {
+        channel: "Goal"
+        field: "hood.unsafe_goal"
+      }
+    }
+    line {
+      y_signal {
+        channel: "Position"
+        field: "hood.encoder"
+      }
+    }
+    ylabel: "hood position"
+  }
+}
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index c2512db..39c913e 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -15,7 +15,8 @@
                                                                  name),
       hood_(constants::GetValues().hood),
       intake_joint_(constants::GetValues().intake),
-      turret_(constants::GetValues().turret.subsystem_params) {
+      turret_(constants::GetValues().turret.subsystem_params),
+      shooter_() {
   event_loop->SetRuntimeRealtimePriority(30);
 }
 
@@ -30,6 +31,9 @@
     turret_.Reset();
   }
 
+  const aos::monotonic_clock::time_point position_timestamp =
+      event_loop()->context().monotonic_event_time;
+
   OutputT output_struct;
 
   flatbuffers::Offset<AbsoluteEncoderProfiledJointStatus> hood_status_offset =
@@ -52,6 +56,12 @@
           output != nullptr ? &(output_struct.turret_voltage) : nullptr,
           status->fbb());
 
+  flatbuffers::Offset<ShooterStatus> shooter_status_offset =
+      shooter_.RunIteration(
+          unsafe_goal != nullptr ? unsafe_goal->shooter() : nullptr,
+          position->shooter(), status->fbb(),
+          output != nullptr ? &(output_struct) : nullptr, position_timestamp);
+
   climber_.Iterate(unsafe_goal, output != nullptr ? &(output_struct) : nullptr);
 
   bool zeroed;
@@ -81,6 +91,7 @@
   status_builder.add_hood(hood_status_offset);
   status_builder.add_intake(intake_status_offset);
   status_builder.add_turret(turret_status_offset);
+  status_builder.add_shooter(shooter_status_offset);
 
   status->Send(status_builder.Finish());
 
diff --git a/y2020/control_loops/superstructure/superstructure.h b/y2020/control_loops/superstructure/superstructure.h
index 2bc0cda..d8ce917 100644
--- a/y2020/control_loops/superstructure/superstructure.h
+++ b/y2020/control_loops/superstructure/superstructure.h
@@ -4,6 +4,7 @@
 #include "aos/controls/control_loop.h"
 #include "aos/events/event_loop.h"
 #include "y2020/constants.h"
+#include "y2020/control_loops/superstructure/shooter/shooter.h"
 #include "y2020/control_loops/superstructure/superstructure_goal_generated.h"
 #include "y2020/control_loops/superstructure/superstructure_output_generated.h"
 #include "y2020/control_loops/superstructure/superstructure_position_generated.h"
@@ -32,6 +33,7 @@
   const AbsoluteEncoderSubsystem &hood() const { return hood_; }
   const AbsoluteEncoderSubsystem &intake_joint() const { return intake_joint_; }
   const PotAndAbsoluteEncoderSubsystem &turret() const { return turret_; }
+  const shooter::Shooter &shooter() const { return shooter_; }
 
  protected:
   virtual void RunIteration(const Goal *unsafe_goal, const Position *position,
@@ -42,6 +44,7 @@
   AbsoluteEncoderSubsystem hood_;
   AbsoluteEncoderSubsystem intake_joint_;
   PotAndAbsoluteEncoderSubsystem turret_;
+  shooter::Shooter shooter_;
 
   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 458a771..8ada461 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -4,15 +4,21 @@
 #include <memory>
 
 #include "aos/controls/control_loop_test.h"
+#include "aos/events/logging/logger.h"
 #include "frc971/control_loops/capped_test_plant.h"
 #include "frc971/control_loops/position_sensor_sim.h"
 #include "frc971/control_loops/team_number_test_environment.h"
 #include "gtest/gtest.h"
 #include "y2020/constants.h"
+#include "y2020/control_loops/superstructure/accelerator/accelerator_plant.h"
+#include "y2020/control_loops/superstructure/finisher/finisher_plant.h"
 #include "y2020/control_loops/superstructure/hood/hood_plant.h"
 #include "y2020/control_loops/superstructure/intake/intake_plant.h"
 #include "y2020/control_loops/superstructure/superstructure.h"
 
+DEFINE_string(output_file, "",
+              "If set, logs all channels to the provided logfile.");
+
 namespace y2020 {
 namespace control_loops {
 namespace superstructure {
@@ -34,6 +40,25 @@
 typedef Superstructure::PotAndAbsoluteEncoderSubsystem
     PotAndAbsoluteEncoderSubsystem;
 
+class FlywheelPlant : public StateFeedbackPlant<2, 1, 1> {
+ public:
+  explicit FlywheelPlant(StateFeedbackPlant<2, 1, 1> &&other)
+      : StateFeedbackPlant<2, 1, 1>(::std::move(other)) {}
+
+  void CheckU(const Eigen::Matrix<double, 1, 1> &U) override {
+    EXPECT_LE(U(0, 0), U_max(0, 0) + 0.00001 + voltage_offset_);
+    EXPECT_GE(U(0, 0), U_min(0, 0) - 0.00001 + voltage_offset_);
+  }
+
+  double voltage_offset() const { return voltage_offset_; }
+  void set_voltage_offset(double voltage_offset) {
+    voltage_offset_ = voltage_offset;
+  }
+
+ private:
+  double voltage_offset_ = 0.0;
+};
+
 // Class which simulates the superstructure and sends out queue messages with
 // the position.
 class SuperstructureSimulation {
@@ -57,7 +82,12 @@
         turret_plant_(new CappedTestPlant(turret::MakeTurretPlant())),
         turret_encoder_(constants::GetValues()
                             .turret.subsystem_params.zeroing_constants
-                            .one_revolution_distance) {
+                            .one_revolution_distance),
+        accelerator_left_plant_(
+            new FlywheelPlant(accelerator::MakeAcceleratorPlant())),
+        accelerator_right_plant_(
+            new FlywheelPlant(accelerator::MakeAcceleratorPlant())),
+        finisher_plant_(new FlywheelPlant(finisher::MakeFinisherPlant())) {
     InitializeHoodPosition(constants::Values::kHoodRange().upper);
     InitializeIntakePosition(constants::Values::kIntakeRange().upper);
     InitializeTurretPosition(constants::Values::kTurretRange().middle());
@@ -104,6 +134,14 @@
                                    .measured_absolute_position);
   }
 
+  flatbuffers::Offset<ShooterPosition> shooter_pos_offset(
+      ShooterPositionBuilder *builder) {
+    builder->add_theta_finisher(finisher_plant_->Y(0, 0));
+    builder->add_theta_accelerator_left(accelerator_left_plant_->Y(0, 0));
+    builder->add_theta_accelerator_right(accelerator_right_plant_->Y(0, 0));
+    return builder->Finish();
+  }
+
   // Sends a queue message with the position of the superstructure.
   void SendPositionMessage() {
     ::aos::Sender<Position>::Builder builder =
@@ -124,11 +162,17 @@
     flatbuffers::Offset<frc971::PotAndAbsolutePosition> turret_offset =
         turret_encoder_.GetSensorValues(&turret_builder);
 
+    ShooterPosition::Builder shooter_builder =
+        builder.MakeBuilder<ShooterPosition>();
+    flatbuffers::Offset<ShooterPosition> shooter_offset =
+        shooter_pos_offset(&shooter_builder);
+
     Position::Builder position_builder = builder.MakeBuilder<Position>();
 
     position_builder.add_hood(hood_offset);
     position_builder.add_intake_joint(intake_offset);
     position_builder.add_turret(turret_offset);
+    position_builder.add_shooter(shooter_offset);
 
     builder.Send(position_builder.Finish());
   }
@@ -142,6 +186,16 @@
   double turret_position() const { return turret_plant_->X(0, 0); }
   double turret_velocity() const { return turret_plant_->X(1, 0); }
 
+  double accelerator_left_velocity() const {
+    return accelerator_left_plant_->X(1, 0);
+  }
+
+  double accelerator_right_velocity() const {
+    return accelerator_right_plant_->X(1, 0);
+  }
+
+  double finisher_velocity() const { return finisher_plant_->X(1, 0); }
+
   // Simulates the superstructure for a single timestep.
   void Simulate() {
     const double last_hood_velocity = hood_velocity();
@@ -193,9 +247,26 @@
     turret_U << superstructure_output_fetcher_->turret_voltage() +
                     turret_plant_->voltage_offset();
 
+    ::Eigen::Matrix<double, 1, 1> accelerator_left_U;
+    accelerator_left_U
+        << superstructure_output_fetcher_->accelerator_left_voltage() +
+               accelerator_left_plant_->voltage_offset();
+
+    ::Eigen::Matrix<double, 1, 1> accelerator_right_U;
+    accelerator_right_U
+        << superstructure_output_fetcher_->accelerator_right_voltage() +
+               accelerator_right_plant_->voltage_offset();
+
+    ::Eigen::Matrix<double, 1, 1> finisher_U;
+    finisher_U << superstructure_output_fetcher_->finisher_voltage() +
+                      finisher_plant_->voltage_offset();
+
     hood_plant_->Update(hood_U);
     intake_plant_->Update(intake_U);
     turret_plant_->Update(turret_U);
+    accelerator_left_plant_->Update(accelerator_left_U);
+    accelerator_right_plant_->Update(accelerator_right_U);
+    finisher_plant_->Update(finisher_U);
 
     const double position_hood = hood_plant_->Y(0, 0);
     const double position_intake = intake_plant_->Y(0, 0);
@@ -280,6 +351,10 @@
   ::std::unique_ptr<CappedTestPlant> turret_plant_;
   PositionSensorSimulator turret_encoder_;
 
+  ::std::unique_ptr<FlywheelPlant> accelerator_left_plant_;
+  ::std::unique_ptr<FlywheelPlant> accelerator_right_plant_;
+  ::std::unique_ptr<FlywheelPlant> finisher_plant_;
+
   // The acceleration limits to check for while moving.
   double peak_hood_acceleration_ = 1e10;
   double peak_intake_acceleration_ = 1e10;
@@ -315,6 +390,15 @@
         superstructure_plant_event_loop_(MakeEventLoop("plant")),
         superstructure_plant_(superstructure_plant_event_loop_.get(), dt()) {
     set_team_id(::frc971::control_loops::testing::kTeamNumber);
+
+    if (!FLAGS_output_file.empty()) {
+      unlink(FLAGS_output_file.c_str());
+      log_buffer_writer_ = std::make_unique<aos::logger::DetachedBufferWriter>(
+          FLAGS_output_file);
+      logger_event_loop_ = MakeEventLoop("logger");
+      logger_ = std::make_unique<aos::logger::Logger>(log_buffer_writer_.get(),
+                                                      logger_event_loop_.get());
+    }
   }
 
   void VerifyNearGoal() {
@@ -336,6 +420,48 @@
       EXPECT_NEAR(superstructure_goal_fetcher_->turret()->unsafe_goal(),
                   superstructure_status_fetcher_->turret()->position(), 0.001);
     }
+
+    if (superstructure_goal_fetcher_->has_shooter()) {
+      EXPECT_NEAR(
+          superstructure_goal_fetcher_->shooter()->velocity_accelerator(),
+          superstructure_status_fetcher_->shooter()
+              ->accelerator_left()
+              ->angular_velocity(),
+          0.001);
+
+      EXPECT_NEAR(
+          superstructure_goal_fetcher_->shooter()->velocity_accelerator(),
+          superstructure_status_fetcher_->shooter()
+              ->accelerator_right()
+              ->angular_velocity(),
+          0.001);
+
+      EXPECT_NEAR(superstructure_goal_fetcher_->shooter()->velocity_finisher(),
+                  superstructure_status_fetcher_->shooter()
+                      ->finisher()
+                      ->angular_velocity(),
+                  0.001);
+
+      EXPECT_NEAR(
+          superstructure_goal_fetcher_->shooter()->velocity_accelerator(),
+          superstructure_status_fetcher_->shooter()
+              ->accelerator_left()
+              ->avg_angular_velocity(),
+          0.001);
+
+      EXPECT_NEAR(
+          superstructure_goal_fetcher_->shooter()->velocity_accelerator(),
+          superstructure_status_fetcher_->shooter()
+              ->accelerator_right()
+              ->avg_angular_velocity(),
+          0.001);
+
+      EXPECT_NEAR(superstructure_goal_fetcher_->shooter()->velocity_finisher(),
+                  superstructure_status_fetcher_->shooter()
+                      ->finisher()
+                      ->avg_angular_velocity(),
+                  0.001);
+    }
   }
 
   void CheckIfZeroed() {
@@ -352,8 +478,8 @@
       // 2 Seconds
       ASSERT_LE(i, 2.0 / ::aos::time::DurationInSeconds(dt()));
 
-      // Since there is a delay when sending running, make sure we have a status
-      // before checking it.
+      // Since there is a delay when sending running, make sure we have a
+      // status before checking it.
     } while (superstructure_status_fetcher_.get() == nullptr ||
              !superstructure_status_fetcher_.get()->zeroed());
   }
@@ -372,9 +498,14 @@
 
   ::std::unique_ptr<::aos::EventLoop> superstructure_plant_event_loop_;
   SuperstructureSimulation superstructure_plant_;
+
+  std::unique_ptr<aos::EventLoop> logger_event_loop_;
+  std::unique_ptr<aos::logger::DetachedBufferWriter> log_buffer_writer_;
+  std::unique_ptr<aos::logger::Logger> logger_;
 };
 
-// Tests that the superstructure does nothing when the goal is to remain still.
+// Tests that the superstructure does nothing when the goal is to remain
+// still.
 TEST_F(SuperstructureTest, DoesNothing) {
   SetEnabled(true);
   superstructure_plant_.InitializeHoodPosition(
@@ -399,11 +530,15 @@
         turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
             *builder.fbb(), constants::Values::kTurretRange().middle() + 1.0);
 
+    flatbuffers::Offset<ShooterGoal> shooter_offset =
+        CreateShooterGoal(*builder.fbb(), 0.0, 0.0);
+
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
 
     goal_builder.add_hood(hood_offset);
     goal_builder.add_intake(intake_offset);
     goal_builder.add_turret(turret_offset);
+    goal_builder.add_shooter(shooter_offset);
 
     ASSERT_TRUE(builder.Send(goal_builder.Finish()));
   }
@@ -418,8 +553,10 @@
   SetEnabled(true);
   // Set a reasonable goal.
 
-  superstructure_plant_.InitializeHoodPosition(0.7);
-  superstructure_plant_.InitializeIntakePosition(0.7);
+  superstructure_plant_.InitializeHoodPosition(
+      constants::Values::kHoodRange().middle());
+  superstructure_plant_.InitializeIntakePosition(
+      constants::Values::kIntakeRange().middle());
 
   WaitUntilZeroed();
   {
@@ -427,23 +564,27 @@
 
     flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
         hood_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
-            *builder.fbb(), 0.2,
+            *builder.fbb(), constants::Values::kHoodRange().upper,
             CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
 
     flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
         intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
-            *builder.fbb(), 0.2,
+            *builder.fbb(), constants::Values::kIntakeRange().upper,
             CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
 
     flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
         turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
             *builder.fbb(), constants::Values::kTurretRange().middle() + 1.0);
 
+    flatbuffers::Offset<ShooterGoal> shooter_offset =
+        CreateShooterGoal(*builder.fbb(), 300.0, 300.0);
+
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
 
     goal_builder.add_hood(hood_offset);
     goal_builder.add_intake(intake_offset);
     goal_builder.add_turret(turret_offset);
+    goal_builder.add_shooter(shooter_offset);
 
     ASSERT_TRUE(builder.Send(goal_builder.Finish()));
   }
@@ -453,8 +594,10 @@
 
   VerifyNearGoal();
 }
+
 // Makes sure that the voltage on a motor is properly pulled back after
-// saturation such that we don't get weird or bad (e.g. oscillating) behaviour.
+// saturation such that we don't get weird or bad (e.g. oscillating)
+// behaviour.
 TEST_F(SuperstructureTest, SaturationTest) {
   SetEnabled(true);
   // Zero it before we move.
@@ -474,11 +617,15 @@
         turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
             *builder.fbb(), constants::Values::kTurretRange().middle() + 1.0);
 
+    flatbuffers::Offset<ShooterGoal> shooter_offset =
+        CreateShooterGoal(*builder.fbb(), 0.0, 0.0);
+
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
 
     goal_builder.add_hood(hood_offset);
     goal_builder.add_intake(intake_offset);
     goal_builder.add_turret(turret_offset);
+    goal_builder.add_shooter(shooter_offset);
 
     ASSERT_TRUE(builder.Send(goal_builder.Finish()));
   }
@@ -504,11 +651,15 @@
         turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
             *builder.fbb(), constants::Values::kTurretRange().middle() + 1.0);
 
+    flatbuffers::Offset<ShooterGoal> shooter_offset =
+        CreateShooterGoal(*builder.fbb(), 0.0, 0.0);
+
     Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
 
     goal_builder.add_hood(hood_offset);
     goal_builder.add_intake(intake_offset);
     goal_builder.add_turret(turret_offset);
+    goal_builder.add_shooter(shooter_offset);
 
     ASSERT_TRUE(builder.Send(goal_builder.Finish()));
   }
@@ -526,6 +677,81 @@
   VerifyNearGoal();
 }
 
+// Tests the shooter can spin up correctly.
+TEST_F(SuperstructureTest, SpinUp) {
+  SetEnabled(true);
+  superstructure_plant_.InitializeHoodPosition(
+      constants::Values::kHoodRange().upper);
+  superstructure_plant_.InitializeIntakePosition(
+      constants::Values::kIntakeRange().upper);
+
+  WaitUntilZeroed();
+  {
+    auto builder = superstructure_goal_sender_.MakeBuilder();
+
+    flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+        hood_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+            *builder.fbb(), constants::Values::kHoodRange().upper,
+            CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
+
+    flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+        intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+            *builder.fbb(), constants::Values::kIntakeRange().upper,
+            CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
+
+    ShooterGoal::Builder shooter_builder = builder.MakeBuilder<ShooterGoal>();
+
+    // Start up the accelerator and make sure both run.
+    shooter_builder.add_velocity_accelerator(20.0);
+    shooter_builder.add_velocity_finisher(20.0);
+
+    flatbuffers::Offset<ShooterGoal> shooter_offset = shooter_builder.Finish();
+
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+    goal_builder.add_hood(hood_offset);
+    goal_builder.add_intake(intake_offset);
+    goal_builder.add_shooter(shooter_offset);
+
+    ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+  }
+
+  // Give it a lot of time to get there.
+  RunFor(chrono::seconds(8));
+
+  VerifyNearGoal();
+
+  {
+    auto builder = superstructure_goal_sender_.MakeBuilder();
+
+    flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+        hood_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+            *builder.fbb(), 0.7,
+            CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
+
+    flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+        intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+            *builder.fbb(), 0.7,
+            CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
+
+    flatbuffers::Offset<ShooterGoal> shooter_offset =
+        CreateShooterGoal(*builder.fbb(), 0.0, 0.0);
+
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+    goal_builder.add_hood(hood_offset);
+    goal_builder.add_intake(intake_offset);
+    goal_builder.add_shooter(shooter_offset);
+
+    ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+  }
+
+  // Give it a lot of time to get there.
+  RunFor(chrono::seconds(9));
+
+  VerifyNearGoal();
+}
+
 // Tests that the loop zeroes when run for a while without a goal.
 TEST_F(SuperstructureTest, ZeroNoGoal) {
   SetEnabled(true);
@@ -552,9 +778,6 @@
   SetEnabled(true);
   // Set a reasonable goal.
 
-  superstructure_plant_.InitializeHoodPosition(0.7);
-  superstructure_plant_.InitializeIntakePosition(0.7);
-
   WaitUntilZeroed();
   {
     auto builder = superstructure_goal_sender_.MakeBuilder();
diff --git a/y2020/control_loops/superstructure/superstructure_status.fbs b/y2020/control_loops/superstructure/superstructure_status.fbs
index 023a242..e8f7637 100644
--- a/y2020/control_loops/superstructure/superstructure_status.fbs
+++ b/y2020/control_loops/superstructure/superstructure_status.fbs
@@ -21,7 +21,7 @@
   finisher:FlywheelControllerStatus;
 
   // The subsystem to accelerate the ball before the finisher
-  // Velocity is the slowest (lowest) wheel
+  // Velocity is the fastest (top) wheel
   accelerator_left:FlywheelControllerStatus;
   accelerator_right:FlywheelControllerStatus;
 }
@@ -33,7 +33,7 @@
   // If true, we have aborted. This is if one or all subsystem estops.
   estopped:bool;
 
-  //Subsystem status.
+  // Subsystem status.
   hood:frc971.control_loops.AbsoluteEncoderProfiledJointStatus;
   intake:frc971.control_loops.AbsoluteEncoderProfiledJointStatus;
   turret:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus;
diff --git a/y2020/wpilib_interface.cc b/y2020/wpilib_interface.cc
index ab8b967..a1083eb 100644
--- a/y2020/wpilib_interface.cc
+++ b/y2020/wpilib_interface.cc
@@ -66,18 +66,6 @@
 // DMA stuff and then removing the * 2.0 in *_translate.
 // The low bit is direction.
 
-// TODO(brian): Use ::std::max instead once we have C++14 so that can be
-// constexpr.
-template <typename T>
-constexpr T max(T a, T b) {
-  return (a > b) ? a : b;
-}
-
-template <typename T, typename... Rest>
-constexpr T max(T a, T b, T c, Rest... rest) {
-  return max(max(a, b), c, rest...);
-}
-
 double drivetrain_translate(int32_t in) {
   return ((static_cast<double>(in) /
            Values::kDrivetrainEncoderCountsPerRevolution()) *
@@ -98,15 +86,19 @@
          (10.0 /*turns*/ / 5.0 /*volts*/) * (2 * M_PI /*radians*/);
 }
 
-// TODO(Ravago): check with constants which are fast.
 constexpr double kMaxFastEncoderPulsesPerSecond =
-    Values::kMaxDrivetrainEncoderPulsesPerSecond();
-static_assert(kMaxFastEncoderPulsesPerSecond <= 1300000,
+    std::max({Values::kMaxControlPanelEncoderPulsesPerSecond(),
+              Values::kMaxFinisherEncoderPulsesPerSecond(),
+              Values::kMaxAcceleratorEncoderPulsesPerSecond()});
+static_assert(kMaxFastEncoderPulsesPerSecond <= 1000000.0,
               "fast encoders are too fast");
 constexpr double kMaxMediumEncoderPulsesPerSecond =
-    kMaxFastEncoderPulsesPerSecond;
+    std::max({Values::kMaxDrivetrainEncoderPulsesPerSecond(),
+              Values::kMaxHoodEncoderPulsesPerSecond(),
+              Values::kMaxIntakeEncoderPulsesPerSecond(),
+              Values::kMaxTurretEncoderPulsesPerSecond()});
 
-static_assert(kMaxMediumEncoderPulsesPerSecond <= 400000,
+static_assert(kMaxMediumEncoderPulsesPerSecond <= 400000.0,
               "medium encoders are too fast");
 
 }  // namespace
@@ -193,12 +185,13 @@
   // Control Panel
 
   void set_control_panel_encoder(::std::unique_ptr<frc::Encoder> encoder) {
-    medium_encoder_filter_.Add(encoder.get());
+    fast_encoder_filter_.Add(encoder.get());
     control_panel_encoder_ = ::std::move(encoder);
   }
 
   // Auto mode switches.
   void set_autonomous_mode(int i, ::std::unique_ptr<frc::DigitalInput> sensor) {
+    medium_encoder_filter_.Add(sensor.get());
     autonomous_modes_.at(i) = ::std::move(sensor);
   }