Merge changes Id7617d6c,I4f3ae9b6
* changes:
Make dual shooter.
Import shooter from 2013
diff --git a/y2016/control_loops/python/BUILD b/y2016/control_loops/python/BUILD
index b424625..0134379 100644
--- a/y2016/control_loops/python/BUILD
+++ b/y2016/control_loops/python/BUILD
@@ -37,3 +37,15 @@
'//frc971/control_loops/python:controls',
],
)
+
+py_binary(
+ name = 'shooter',
+ srcs = [
+ 'shooter.py',
+ ],
+ deps = [
+ '//external:python-gflags',
+ '//external:python-glog',
+ '//frc971/control_loops/python:controls',
+ ]
+)
diff --git a/y2016/control_loops/python/shooter.py b/y2016/control_loops/python/shooter.py
new file mode 100755
index 0000000..e47add9
--- /dev/null
+++ b/y2016/control_loops/python/shooter.py
@@ -0,0 +1,73 @@
+#!/usr/bin/python
+
+from frc971.control_loops.python import control_loop
+import numpy
+import sys
+from matplotlib import pylab
+
+class Shooter(control_loop.ControlLoop):
+ def __init__(self):
+ super(Shooter, self).__init__("Shooter")
+ # Stall Torque in N m
+ self.stall_torque = 0.49819248
+ # Stall Current in Amps
+ self.stall_current = 85
+ # Free Speed in RPM
+ self.free_speed = 19300.0 - 1500.0
+ # Free Current in Amps
+ self.free_current = 1.4
+ # Moment of inertia of the shooter wheel in kg m^2
+ self.J = 0.0032
+ # Resistance of the motor, divided by 2 to account for the 2 motors
+ self.R = 12.0 / self.stall_current / 2
+ # Motor velocity constant
+ self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ (12.0 - self.R * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Gear ratio
+ self.G = 11.0 / 34.0
+ # Control loop time step
+ self.dt = 0.005
+
+ # State feedback matrices
+ # [position, angular velocity]
+ self.A_continuous = numpy.matrix(
+ [[0, 1],
+ [0, -self.Kt / self.Kv / (self.J * self.G * self.G * self.R)]])
+ self.B_continuous = numpy.matrix(
+ [[0],
+ [self.Kt / (self.J * self.G * self.R)]])
+ self.C = numpy.matrix([[1, 0]])
+ self.D = numpy.matrix([[0]])
+
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ self.PlaceControllerPoles([.6, .981])
+
+ self.rpl = .45
+ self.ipl = 0.07
+ self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+ self.rpl - 1j * self.ipl])
+
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+
+def main(argv):
+ if len(argv) != 3:
+ print "Expected .h file name and .cc file name"
+ else:
+ namespaces = ['y2016', 'control_loops', 'shooter']
+ shooter = Shooter()
+ loop_writer = control_loop.ControlLoopWriter("Shooter", [shooter],
+ namespaces=namespaces)
+ if argv[1][-3:] == '.cc':
+ loop_writer.Write(argv[2], argv[1])
+ else:
+ loop_writer.Write(argv[1], argv[2])
+
+
+if __name__ == '__main__':
+ sys.exit(main(sys.argv))
diff --git a/y2016/control_loops/shooter/BUILD b/y2016/control_loops/shooter/BUILD
new file mode 100644
index 0000000..cb3c0bc
--- /dev/null
+++ b/y2016/control_loops/shooter/BUILD
@@ -0,0 +1,83 @@
+package(default_visibility = ['//visibility:public'])
+
+load('/aos/build/queues', 'queue_library')
+
+queue_library(
+ name = 'shooter_queue',
+ srcs = [
+ 'shooter.q',
+ ],
+ deps = [
+ '//aos/common/controls:control_loop_queues',
+ '//frc971/control_loops:queues',
+ ],
+)
+
+genrule(
+ name = 'genrule_shooter',
+ visibility = ['//visibility:private'],
+ cmd = '$(location //y2016/control_loops/python:shooter) $(OUTS)',
+ tools = [
+ '//y2016/control_loops/python:shooter',
+ ],
+ outs = [
+ 'shooter_plant.h',
+ 'shooter_plant.cc',
+ ],
+)
+
+cc_library(
+ name = 'shooter_plants',
+ srcs = [
+ 'shooter_plant.cc',
+ ],
+ hdrs = [
+ 'shooter_plant.h',
+ ],
+ deps = [
+ '//frc971/control_loops:state_feedback_loop',
+ ],
+)
+
+cc_library(
+ name = 'shooter_lib',
+ srcs = [
+ 'shooter.cc',
+ ],
+ hdrs = [
+ 'shooter.h',
+ ],
+ deps = [
+ ':shooter_queue',
+ ':shooter_plants',
+ '//aos/common/controls:control_loop',
+ ],
+)
+
+cc_test(
+ name = 'shooter_lib_test',
+ srcs = [
+ 'shooter_lib_test.cc',
+ ],
+ deps = [
+ ':shooter_queue',
+ ':shooter_lib',
+ '//aos/testing:googletest',
+ '//aos/common:queues',
+ '//aos/common/controls:control_loop_test',
+ '//frc971/control_loops:state_feedback_loop',
+ '//frc971/control_loops:team_number_test_environment',
+ ],
+)
+
+cc_binary(
+ name = 'shooter',
+ srcs = [
+ 'shooter_main.cc',
+ ],
+ deps = [
+ '//aos/linux_code:init',
+ ':shooter_lib',
+ ':shooter_queue',
+ ],
+)
diff --git a/y2016/control_loops/shooter/shooter.cc b/y2016/control_loops/shooter/shooter.cc
new file mode 100644
index 0000000..9a46906
--- /dev/null
+++ b/y2016/control_loops/shooter/shooter.cc
@@ -0,0 +1,137 @@
+#include "y2016/control_loops/shooter/shooter.h"
+
+#include "aos/common/controls/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/logging/queue_logging.h"
+
+#include "y2016/control_loops/shooter/shooter_plant.h"
+
+
+namespace y2016 {
+namespace control_loops {
+
+ShooterSide::ShooterSide()
+ : loop_(new StateFeedbackLoop<2, 1, 1>(
+ ::y2016::control_loops::shooter::MakeShooterLoop())) {
+ memset(history_, 0, sizeof(history_));
+}
+
+void ShooterSide::SetGoal(double angular_velocity_goal_uncapped) {
+ angular_velocity_goal_ = std::min(angular_velocity_goal_uncapped,
+ kMaxSpeed);
+}
+
+void ShooterSide::EstimatePositionTimestep() {
+ // NULL position, so look at the loop.
+ SetPosition(loop_->X_hat(0, 0));
+}
+
+void ShooterSide::SetPosition(double current_position) {
+ current_position_ = current_position;
+
+ // Track the current position if the velocity goal is small.
+ if (angular_velocity_goal_ <= 1.0) position_goal_ = current_position_;
+
+ // Update position in the model.
+ Eigen::Matrix<double, 1, 1> Y;
+ Y << current_position_;
+ loop_->Correct(Y);
+
+ // Prevents integral windup by limiting the position error such that the
+ // error can't produce much more than full power.
+ const double max_reference =
+ (loop_->U_max(0, 0) -
+ kAngularVelocityWeightScalar *
+ (angular_velocity_goal_ - loop_->X_hat(1, 0)) * loop_->K(0, 1)) /
+ loop_->K(0, 0) +
+ loop_->X_hat(0, 0);
+ const double min_reference =
+ (loop_->U_min(0, 0) -
+ kAngularVelocityWeightScalar *
+ (angular_velocity_goal_ - loop_->X_hat(1, 0)) * loop_->K(0, 1)) /
+ loop_->K(0, 0) +
+ loop_->X_hat(0, 0);
+ position_goal_ =
+ ::std::max(::std::min(position_goal_, max_reference), min_reference);
+
+ loop_->mutable_R() << position_goal_, angular_velocity_goal_;
+ position_goal_ +=
+ angular_velocity_goal_ * ::aos::controls::kLoopFrequency.ToSeconds();
+
+ // Add the position to the history.
+ history_[history_position_] = current_position_;
+ history_position_ = (history_position_ + 1) % kHistoryLength;
+}
+
+const ShooterStatus ShooterSide::GetStatus() {
+ // Calculate average over dt * kHistoryLength.
+ int old_history_position =
+ ((history_position_ == 0) ? kHistoryLength : history_position_) - 1;
+ double avg_angular_velocity =
+ (history_[old_history_position] - history_[history_position_]) /
+ ::aos::controls::kLoopFrequency.ToSeconds() /
+ static_cast<double>(kHistoryLength - 1);
+
+ // Ready if average angular velocity is close to the goal.
+ bool ready = (std::abs(angular_velocity_goal_ - avg_angular_velocity) <
+ kTolerance &&
+ angular_velocity_goal_ != 0.0);
+
+ return {avg_angular_velocity, ready};
+}
+
+double ShooterSide::GetOutput() {
+ if (angular_velocity_goal_ < 1.0) {
+ // Kill power at low angular velocities.
+ return 0.0;
+ }
+
+ return loop_->U(0, 0);
+}
+
+void ShooterSide::UpdateLoop(bool output_is_null) {
+ loop_->Update(output_is_null);
+}
+
+Shooter::Shooter(control_loops::ShooterQueue *my_shooter)
+ : aos::controls::ControlLoop<control_loops::ShooterQueue>(my_shooter) {}
+
+void Shooter::RunIteration(
+ const control_loops::ShooterQueue::Goal *goal,
+ const control_loops::ShooterQueue::Position *position,
+ control_loops::ShooterQueue::Output *output,
+ control_loops::ShooterQueue::Status *status) {
+ if (goal) {
+ // Update position/goal for our two shooter sides.
+ left_.SetGoal(goal->angular_velocity_left);
+ right_.SetGoal(goal->angular_velocity_right);
+
+ if (position == nullptr) {
+ left_.EstimatePositionTimestep();
+ right_.EstimatePositionTimestep();
+ } else {
+ left_.SetPosition(position->theta_left);
+ right_.SetPosition(position->theta_right);
+ }
+ }
+
+ ShooterStatus status_left = left_.GetStatus();
+ ShooterStatus status_right = right_.GetStatus();
+ status->avg_angular_velocity_left = status_left.avg_angular_velocity;
+ status->avg_angular_velocity_right = status_right.avg_angular_velocity;
+
+ status->ready_left = status_left.ready;
+ status->ready_right = status_right.ready;
+ status->ready_both = (status_left.ready && status_right.ready);
+
+ left_.UpdateLoop(output == nullptr);
+ right_.UpdateLoop(output == nullptr);
+
+ if (output) {
+ output->voltage_left = left_.GetOutput();
+ output->voltage_right = right_.GetOutput();
+ }
+}
+
+} // namespace control_loops
+} // namespace y2016
diff --git a/y2016/control_loops/shooter/shooter.h b/y2016/control_loops/shooter/shooter.h
new file mode 100644
index 0000000..6a3fa4d
--- /dev/null
+++ b/y2016/control_loops/shooter/shooter.h
@@ -0,0 +1,74 @@
+#ifndef Y2016_CONTROL_LOOPS_SHOOTER_SHOOTER_H_
+#define Y2016_CONTROL_LOOPS_SHOOTER_SHOOTER_H_
+
+#include <memory>
+
+#include "aos/common/controls/control_loop.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+
+#include "y2016/control_loops/shooter/shooter_plant.h"
+#include "y2016/control_loops/shooter/shooter.q.h"
+
+namespace y2016 {
+namespace control_loops {
+
+namespace {
+// TODO(constants): Update.
+const double kTolerance = 10.0;
+const double kMaxSpeed = 10000.0 * (2.0 * M_PI) / 60.0 * 15.0 / 34.0;
+const double kAngularVelocityWeightScalar = 0.35;
+} // namespace
+
+struct ShooterStatus {
+ double avg_angular_velocity;
+ bool ready;
+};
+
+class ShooterSide {
+ public:
+ ShooterSide();
+
+ void SetGoal(double angular_velocity_goal);
+ void EstimatePositionTimestep();
+ void SetPosition(double current_position);
+ const ShooterStatus GetStatus();
+ double GetOutput();
+ void UpdateLoop(bool output_is_null);
+
+ private:
+ ::std::unique_ptr<StateFeedbackLoop<2, 1, 1>> loop_;
+
+ double current_position_ = 0.0;
+ double position_goal_ = 0.0;
+ double angular_velocity_goal_ = 0.0;
+
+ // History array for calculating a filtered angular velocity.
+ static const int kHistoryLength = 5;
+ double history_[kHistoryLength];
+ ptrdiff_t history_position_;
+
+ DISALLOW_COPY_AND_ASSIGN(ShooterSide);
+};
+
+class Shooter
+ : public ::aos::controls::ControlLoop<control_loops::ShooterQueue> {
+ public:
+ explicit Shooter(control_loops::ShooterQueue *shooter_queue =
+ &control_loops::shooter_queue);
+
+ protected:
+ void RunIteration(const control_loops::ShooterQueue::Goal *goal,
+ const control_loops::ShooterQueue::Position *position,
+ control_loops::ShooterQueue::Output *output,
+ control_loops::ShooterQueue::Status *status) override;
+
+ private:
+ ShooterSide left_, right_;
+
+ DISALLOW_COPY_AND_ASSIGN(Shooter);
+};
+
+} // namespace control_loops
+} // namespace y2016
+
+#endif // Y2016_CONTROL_LOOPS_SHOOTER_SHOOTER_H_
diff --git a/y2016/control_loops/shooter/shooter.q b/y2016/control_loops/shooter/shooter.q
new file mode 100644
index 0000000..49575fb
--- /dev/null
+++ b/y2016/control_loops/shooter/shooter.q
@@ -0,0 +1,46 @@
+package y2016.control_loops;
+
+import "aos/common/controls/control_loops.q";
+import "frc971/control_loops/control_loops.q";
+
+queue_group ShooterQueue {
+ implements aos.control_loops.ControlLoop;
+
+ // All angles are in radians, and angular velocities are in rad/sec. For all
+ // angular velocities, positive is shooting the ball out of the robot.
+
+ message Goal {
+ // Angular velocity goals in rad/sec.
+ double angular_velocity_left;
+ double angular_velocity_right;
+ };
+
+ message Position {
+ // Wheel angle in rad/sec.
+ double theta_left;
+ double theta_right;
+ };
+
+ message Status {
+ bool ready_left;
+ bool ready_right;
+ // Is the shooter ready to shoot?
+ bool ready_both;
+
+ // Average angular velocities over dt * kHistoryLength.
+ double avg_angular_velocity_left;
+ double avg_angular_velocity_right;
+ };
+
+ message Output {
+ double voltage_left;
+ double voltage_right;
+ };
+
+ queue Goal goal;
+ queue Position position;
+ queue Output output;
+ queue Status status;
+};
+
+queue_group ShooterQueue shooter_queue;
diff --git a/y2016/control_loops/shooter/shooter_lib_test.cc b/y2016/control_loops/shooter/shooter_lib_test.cc
new file mode 100644
index 0000000..9ec0334
--- /dev/null
+++ b/y2016/control_loops/shooter/shooter_lib_test.cc
@@ -0,0 +1,263 @@
+#include "y2016/control_loops/shooter/shooter.h"
+
+#include <unistd.h>
+
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "aos/common/queue.h"
+#include "aos/common/controls/control_loop_test.h"
+#include "frc971/control_loops/team_number_test_environment.h"
+#include "y2016/control_loops/shooter/shooter.q.h"
+#include "y2016/control_loops/shooter/shooter.h"
+
+using ::aos::time::Time;
+using ::frc971::control_loops::testing::kTeamNumber;
+
+namespace y2016 {
+namespace control_loops {
+namespace testing {
+
+// Class which simulates the shooter and sends out queue messages with the
+// position.
+class ShooterSimulation {
+ public:
+ // Constructs a shooter simulation.
+ ShooterSimulation()
+ : shooter_plant_left_(new StateFeedbackPlant<2, 1, 1>(
+ ::y2016::control_loops::shooter::MakeShooterPlant())),
+ shooter_plant_right_(new StateFeedbackPlant<2, 1, 1>(
+ ::y2016::control_loops::shooter::MakeShooterPlant())),
+ shooter_queue_(".y2016.control_loops.shooter", 0x78d8e372,
+ ".y2016.control_loops.shooter.goal",
+ ".y2016.control_loops.shooter.position",
+ ".y2016.control_loops.shooter.output",
+ ".y2016.control_loops.shooter.status") {}
+
+ // Sends a queue message with the position of the shooter.
+ void SendPositionMessage() {
+ ::aos::ScopedMessagePtr<control_loops::ShooterQueue::Position> position =
+ shooter_queue_.position.MakeMessage();
+
+ position->theta_left = shooter_plant_left_->Y(0, 0);
+ position->theta_right = shooter_plant_right_->Y(0, 0);
+ position.Send();
+ }
+
+ // Simulates shooter for a single timestep.
+ void Simulate() {
+ EXPECT_TRUE(shooter_queue_.output.FetchLatest());
+
+ shooter_plant_left_->mutable_U(0, 0) = shooter_queue_.output->voltage_left;
+ shooter_plant_right_->mutable_U(0, 0) =
+ shooter_queue_.output->voltage_right;
+
+ shooter_plant_left_->Update();
+ shooter_plant_right_->Update();
+ }
+
+ ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> shooter_plant_left_,
+ shooter_plant_right_;
+
+ private:
+ ShooterQueue shooter_queue_;
+};
+
+class ShooterTest : public ::aos::testing::ControlLoopTest {
+ protected:
+ ShooterTest()
+ : shooter_queue_(".y2016.control_loops.shooter", 0x78d8e372,
+ ".y2016.control_loops.shooter.goal",
+ ".y2016.control_loops.shooter.position",
+ ".y2016.control_loops.shooter.output",
+ ".y2016.control_loops.shooter.status"),
+ shooter_(&shooter_queue_),
+ shooter_plant_() {
+ set_team_id(kTeamNumber);
+ }
+
+ void VerifyNearGoal() {
+ shooter_queue_.goal.FetchLatest();
+ shooter_queue_.status.FetchLatest();
+
+ EXPECT_TRUE(shooter_queue_.goal.get() != nullptr);
+ EXPECT_TRUE(shooter_queue_.status.get() != nullptr);
+
+ EXPECT_NEAR(shooter_queue_.goal->angular_velocity_left,
+ shooter_queue_.status->avg_angular_velocity_left, 10.0);
+ EXPECT_NEAR(shooter_queue_.goal->angular_velocity_right,
+ shooter_queue_.status->avg_angular_velocity_right, 10.0);
+ }
+
+ // Runs one iteration of the whole simulation.
+ void RunIteration(bool enabled = true) {
+ SendMessages(enabled);
+
+ shooter_plant_.SendPositionMessage();
+ shooter_.Iterate();
+ shooter_plant_.Simulate();
+
+ TickTime();
+ }
+
+ // Runs iterations until the specified amount of simulated time has elapsed.
+ void RunForTime(const Time &run_for, bool enabled = true) {
+ const auto start_time = Time::Now();
+ while (Time::Now() < start_time + run_for) {
+ RunIteration(enabled);
+ }
+ }
+
+ // Create a new instance of the test queue so that it invalidates the queue
+ // that it points to. Otherwise, we will have a pointer to shared memory that
+ // is no longer valid.
+ ShooterQueue shooter_queue_;
+
+ // Create a control loop and simulation.
+ Shooter shooter_;
+ ShooterSimulation shooter_plant_;
+};
+
+// Tests that the shooter does nothing when the goal is zero.
+TEST_F(ShooterTest, DoesNothing) {
+ ASSERT_TRUE(shooter_queue_.goal.MakeWithBuilder()
+ .angular_velocity_left(0.0)
+ .angular_velocity_right(0.0)
+ .Send());
+ RunIteration();
+
+ VerifyNearGoal();
+
+ EXPECT_TRUE(shooter_queue_.output.FetchLatest());
+ EXPECT_EQ(shooter_queue_.output->voltage_left, 0.0);
+ EXPECT_EQ(shooter_queue_.output->voltage_right, 0.0);
+}
+
+// Tests that the shooter spins up to speed and that it then spins down
+// without applying any power.
+TEST_F(ShooterTest, SpinUpAndDown) {
+ // Spin up.
+ EXPECT_TRUE(shooter_queue_.goal.MakeWithBuilder()
+ .angular_velocity_left(450.0)
+ .angular_velocity_right(450.0)
+ .Send());
+ RunForTime(Time::InSeconds(10));
+ VerifyNearGoal();
+ EXPECT_TRUE(shooter_queue_.status->ready_both);
+ EXPECT_TRUE(shooter_queue_.goal.MakeWithBuilder()
+ .angular_velocity_left(0.0)
+ .angular_velocity_right(0.0)
+ .Send());
+
+ // Make sure we don't apply voltage on spin-down.
+ RunIteration();
+ EXPECT_TRUE(shooter_queue_.output.FetchLatest());
+ EXPECT_EQ(0.0, shooter_queue_.output->voltage_left);
+ EXPECT_EQ(0.0, shooter_queue_.output->voltage_right);
+
+ // Continue to stop.
+ RunForTime(Time::InSeconds(5));
+ EXPECT_TRUE(shooter_queue_.output.FetchLatest());
+ EXPECT_EQ(0.0, shooter_queue_.output->voltage_left);
+ EXPECT_EQ(0.0, shooter_queue_.output->voltage_right);
+}
+
+// Tests that the shooter doesn't say it is ready if one side isn't up to speed.
+// According to our tuning, we may overshoot the goal on one shooter and
+// mistakenly say that we are ready. This test should look at both extremes.
+TEST_F(ShooterTest, SideLagTest) {
+ // Spin up.
+ EXPECT_TRUE(shooter_queue_.goal.MakeWithBuilder()
+ .angular_velocity_left(20.0)
+ .angular_velocity_right(450.0)
+ .Send());
+ RunForTime(Time::InSeconds(0.1));
+
+ shooter_queue_.goal.FetchLatest();
+ shooter_queue_.status.FetchLatest();
+
+ EXPECT_TRUE(shooter_queue_.goal.get() != nullptr);
+ EXPECT_TRUE(shooter_queue_.status.get() != nullptr);
+
+ // Left should be up to speed, right shouldn't.
+ EXPECT_TRUE(shooter_queue_.status->ready_left);
+ EXPECT_FALSE(shooter_queue_.status->ready_right);
+ EXPECT_FALSE(shooter_queue_.status->ready_both);
+
+ RunForTime(Time::InSeconds(5));
+
+ shooter_queue_.goal.FetchLatest();
+ shooter_queue_.status.FetchLatest();
+
+ EXPECT_TRUE(shooter_queue_.goal.get() != nullptr);
+ EXPECT_TRUE(shooter_queue_.status.get() != nullptr);
+
+ // Both should be up to speed.
+ EXPECT_TRUE(shooter_queue_.status->ready_left);
+ EXPECT_TRUE(shooter_queue_.status->ready_right);
+ EXPECT_TRUE(shooter_queue_.status->ready_both);
+}
+
+// Test to make sure that it does not exceed the encoder's rated speed.
+TEST_F(ShooterTest, RateLimitTest) {
+ ASSERT_TRUE(shooter_queue_.goal.MakeWithBuilder()
+ .angular_velocity_left(1000.0)
+ .angular_velocity_right(1000.0)
+ .Send());
+
+ RunForTime(Time::InSeconds(10));
+ EXPECT_TRUE(shooter_queue_.status.FetchLatest());
+ EXPECT_TRUE(shooter_queue_.status->ready_both);
+
+ shooter_queue_.goal.FetchLatest();
+ shooter_queue_.status.FetchLatest();
+
+ EXPECT_TRUE(shooter_queue_.goal.get() != nullptr);
+ EXPECT_TRUE(shooter_queue_.status.get() != nullptr);
+
+ EXPECT_GT(::y2016::control_loops::kMaxSpeed,
+ shooter_queue_.status->avg_angular_velocity_left);
+
+ EXPECT_GT(::y2016::control_loops::kMaxSpeed,
+ shooter_queue_.status->avg_angular_velocity_right);
+}
+
+// Tests that the shooter can spin up nicely while missing position packets.
+TEST_F(ShooterTest, MissingPositionMessages) {
+ ASSERT_TRUE(shooter_queue_.goal.MakeWithBuilder()
+ .angular_velocity_left(200.0)
+ .angular_velocity_right(200.0)
+ .Send());
+ for (int i = 0; i < 100; ++i) {
+ if (i % 7) {
+ shooter_plant_.SendPositionMessage();
+ }
+
+ RunIteration();
+ }
+
+ VerifyNearGoal();
+}
+
+// Tests that the shooter can spin up nicely while disabled for a while.
+// TODO(comran): Update this test, since it's the same as
+// MissingPositionMessages.
+TEST_F(ShooterTest, Disabled) {
+ ASSERT_TRUE(shooter_queue_.goal.MakeWithBuilder()
+ .angular_velocity_left(200.0)
+ .angular_velocity_right(200.0)
+ .Send());
+ for (int i = 0; i < 100; ++i) {
+ if (i % 7) {
+ shooter_plant_.SendPositionMessage();
+ }
+
+ RunIteration();
+ }
+
+ VerifyNearGoal();
+}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace y2016
diff --git a/y2016/control_loops/shooter/shooter_main.cc b/y2016/control_loops/shooter/shooter_main.cc
new file mode 100644
index 0000000..911ab29
--- /dev/null
+++ b/y2016/control_loops/shooter/shooter_main.cc
@@ -0,0 +1,11 @@
+#include "y2016/control_loops/shooter/shooter.h"
+
+#include "aos/linux_code/init.h"
+
+int main() {
+ ::aos::Init();
+ ::y2016::control_loops::Shooter shooter;
+ shooter.Run();
+ ::aos::Cleanup();
+ return 0;
+}