Added shooter loop.
Still needs to actually be tested and tuned on a robot. Passes all of its tests.
diff --git a/frc971/control_loops/shooter_lib_test.cc b/frc971/control_loops/shooter_lib_test.cc
new file mode 100644
index 0000000..63afb80
--- /dev/null
+++ b/frc971/control_loops/shooter_lib_test.cc
@@ -0,0 +1,165 @@
+#include <unistd.h>
+
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "aos/common/queue.h"
+#include "aos/common/queue_testutils.h"
+#include "frc971/control_loops/shooter_motor.q.h"
+#include "frc971/control_loops/shooter.h"
+#include "frc971/constants.h"
+
+using ::aos::time::Time;
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+
+// Class which simulates the shooter and sends out queue messages with the
+// position.
+class ShooterMotorSimulation {
+ public:
+ // Constructs a shooter simulation. I'm not sure how the construction of
+ // the queue (my_shooter_loop_) actually works (same format as wrist).
+ ShooterMotorSimulation()
+ : shooter_plant_(new StateFeedbackPlant<2, 1, 1>(MakeShooterPlant())),
+ my_shooter_loop_(".frc971.control_loops.shooter",
+ 0x78d8e372, ".frc971.control_loops.shooter.goal",
+ ".frc971.control_loops.shooter.position",
+ ".frc971.control_loops.shooter.output",
+ ".frc971.control_loops.shooter.status") {
+ Reinitialize();
+ }
+
+ // Resets the shooter simulation.
+ void Reinitialize() {
+ shooter_plant_->X(0, 0) = 0;
+ shooter_plant_->X(1, 0) = 0;
+ shooter_plant_->Y = shooter_plant_->C * shooter_plant_->X;
+ }
+
+ // Sends a queue message with the position of the shooter.
+ void SendPositionMessage() {
+ ::aos::ScopedMessagePtr<control_loops::ShooterLoop::Position> position =
+ my_shooter_loop_.position.MakeMessage();
+ position->position = shooter_plant_->Y(0, 0);
+ position.Send();
+ }
+
+ // Simulates shooter for a single timestep.
+ void Simulate() {
+ EXPECT_TRUE(my_shooter_loop_.output.FetchLatest());
+ shooter_plant_->U << my_shooter_loop_.output->voltage * 12;
+ shooter_plant_->Update();
+ }
+
+ ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> shooter_plant_;
+ private:
+ ShooterLoop my_shooter_loop_;
+};
+
+class ShooterTest : public ::testing::Test {
+ protected:
+ // I haven't the faintest idea exactly what this line does,
+ // but without it things break.
+ ::aos::common::testing::GlobalCoreInstance my_core;
+
+ // Create a new instance of the test queue "so that it invalidates the queue
+ // that it points to Otherwise, we will have a pointed to
+ // shared memory that is no longer valid."
+ ShooterLoop my_shooter_loop_;
+
+ // Create a control loop and simulation.
+ ShooterMotor shooter_motor_;
+ ShooterMotorSimulation shooter_motor_plant_;
+
+ ShooterTest() : my_shooter_loop_(".frc971.control_loops.shooter",
+ 0x78d8e372,
+ ".frc971.control_loops.shooter.goal",
+ ".frc971.control_loops.shooter.position",
+ ".frc971.control_loops.shooter.output",
+ ".frc971.control_loops.shooter.status"),
+ shooter_motor_(&my_shooter_loop_),
+ shooter_motor_plant_() {
+ // Flush the robot state queue so we can use clean shared memory for this
+ // test.
+ ::aos::robot_state.Clear();
+ SendDSPacket(true);
+ }
+
+ // Update the robot state. Without this, the Iteration of the control loop
+ // will stop all the motors and the shooter won't go anywhere.
+ void SendDSPacket(bool enabled) {
+ ::aos::robot_state.MakeWithBuilder().enabled(enabled)
+ .autonomous(false)
+ .team_id(971).Send();
+ ::aos::robot_state.FetchLatest();
+ }
+
+ void VerifyNearGoal() {
+ my_shooter_loop_.goal.FetchLatest();
+ my_shooter_loop_.status.FetchLatest();
+ EXPECT_NEAR(my_shooter_loop_.goal->velocity,
+ my_shooter_loop_.status->average_velocity,
+ 10.0);
+ }
+
+ virtual ~ShooterTest() {
+ ::aos::robot_state.Clear();
+ }
+};
+
+// Tests that the shooter does nothing when the goal is zero.
+TEST_F(ShooterTest, DoesNothing) {
+ my_shooter_loop_.goal.MakeWithBuilder().velocity(0.0).Send();
+ SendDSPacket(true);
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ VerifyNearGoal();
+}
+
+// Tests that the shooter spins up to speed and that it then spins down
+// without applying any power.
+TEST_F(ShooterTest, SpinUpAndDown) {
+ my_shooter_loop_.goal.MakeWithBuilder().velocity(450.0).Send();
+ bool is_done = false;
+ while (!is_done) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ EXPECT_TRUE(my_shooter_loop_.status.FetchLatest());
+ is_done = my_shooter_loop_.status->ready;
+ }
+ VerifyNearGoal();
+ my_shooter_loop_.goal.MakeWithBuilder().velocity(0.0).Send();
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ EXPECT_TRUE(my_shooter_loop_.output.FetchLatest());
+ EXPECT_EQ(0.0, my_shooter_loop_.output->voltage);
+}
+
+// Test to make sure that it does not exceed the encoder's rated speed.
+TEST_F(ShooterTest, RateLimitTest) {
+ my_shooter_loop_.goal.MakeWithBuilder().velocity(1000.0).Send();
+ bool is_done = false;
+ while (!is_done) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ EXPECT_TRUE(my_shooter_loop_.status.FetchLatest());
+ is_done = my_shooter_loop_.status->ready;
+ }
+ my_shooter_loop_.goal.FetchLatest();
+ my_shooter_loop_.status.FetchLatest();
+ EXPECT_FALSE(std::abs(my_shooter_loop_.goal->velocity -
+ my_shooter_loop_.status->average_velocity) < 10.0);
+}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace frc971