Added initial indexer loop. It pulls in discs correctly, but doesn't do anything else.
diff --git a/frc971/control_loops/index_lib_test.cc b/frc971/control_loops/index_lib_test.cc
new file mode 100644
index 0000000..418a9b6
--- /dev/null
+++ b/frc971/control_loops/index_lib_test.cc
@@ -0,0 +1,315 @@
+#include <unistd.h>
+
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "aos/common/queue.h"
+#include "aos/common/queue_testutils.h"
+#include "frc971/control_loops/index_motor.q.h"
+#include "frc971/control_loops/index.h"
+#include "frc971/control_loops/index_motor_plant.h"
+#include "frc971/control_loops/transfer_motor_plant.h"
+#include "frc971/constants.h"
+
+
+using ::aos::time::Time;
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+
+// TODO(aschuh): Figure out these constants.
+const double kTransferStartPosition = 0.0;
+const double kIndexStartPosition = 0.5;
+const double kIndexStopPosition = 2.5;
+const double kGrabberStopPosition = 2.625;
+const double kGrabberMovementVelocity = 0.4;
+
+// Start and stop position of the bottom disc detect sensor in meters.
+const double kBottomDiscDetectStart = -0.08;
+const double kBottomDiscDetectStop = 0.200025;
+
+const double kTopDiscDetectStart = 18.0;
+const double kTopDiscDetectStop = 19.0;
+
+// Disc radius in meters.
+const double kDiscRadius = 11.875 * 0.0254 / 2;
+// Roller radius in meters.
+const double kRollerRadius = 2.0 * 0.0254 / 2;
+
+class Frisbee {
+ public:
+ // Creates a frisbee starting at the specified position in the frisbee path,
+ // and with the transfer and index rollers at the specified positions.
+ Frisbee(double transfer_roller_position,
+ double index_roller_position,
+ double position = 0.0)
+ : transfer_roller_position_(transfer_roller_position),
+ index_roller_position_(index_roller_position),
+ clamped_(false),
+ position_(position) {
+ }
+
+ // Returns true if the frisbee is controlled by the transfer roller.
+ bool IsTouchingTransfer() const {
+ return (position_ >= kTransferStartPosition &&
+ position_ <= kIndexStartPosition);
+ }
+
+ // Returns true if the frisbee is controlled by the indexing roller.
+ bool IsTouchingIndex() const {
+ return (position_ >= kIndexStartPosition &&
+ position_ <= kIndexStopPosition);
+ }
+
+ // Returns true if the frisbee is in a position such that the grabber will
+ // pull it into the loader.
+ bool IsTouchingGrabber() const {
+ return (position_ >= kIndexStopPosition &&
+ position_ <= kGrabberStopPosition);
+ }
+
+ // Returns true if the disc is triggering the bottom disc detect sensor.
+ bool bottom_disc_detect() const {
+ return (position_ >= kBottomDiscDetectStart &&
+ position_ <= kBottomDiscDetectStop);
+ }
+
+ // Returns true if the disc is triggering the top disc detect sensor.
+ bool top_disc_detect() const {
+ return (position_ >= kTopDiscDetectStart &&
+ position_ <= kTopDiscDetectStop);
+ }
+
+ // Converts the angle of the indexer to the distance traveled by the center of
+ // the disc.
+ double ConvertIndexToDiscPosition(const double angle) const {
+ return (angle * (kDiscRadius + kRollerRadius) /
+ (1 + (kDiscRadius * 2 + kRollerRadius) / kRollerRadius));
+ }
+
+ // Converts the angle of the transfer to the distance traveled by the center
+ // of the disc.
+ double ConvertTransferToDiscPosition(const double angle) const {
+ return ConvertIndexToDiscPosition(angle);
+ }
+
+ // Updates the position of the frisbee in the frisbee path.
+ void UpdatePosition(double transfer_roller_position,
+ double index_roller_position,
+ bool clamped) {
+ // TODO(aschuh): Assert that you can't slide the frisbee through the
+ // clamp.
+ if (IsTouchingTransfer()) {
+ position_ += ConvertTransferToDiscPosition(transfer_roller_position -
+ transfer_roller_position_);
+ } else if (IsTouchingIndex()) {
+ position_ += ConvertIndexToDiscPosition(index_roller_position -
+ index_roller_position_);
+ } else if (IsTouchingGrabber()) {
+ if (clamped) {
+ position_ = ::std::min(position_ + kGrabberMovementVelocity / 100.0,
+ kGrabberStopPosition);
+ }
+ } else {
+ // TODO(aschuh): Deal with lifting.
+ // TODO(aschuh): Deal with shooting.
+ // We must wait long enough for the disc to leave the loader before
+ // lowering.
+ }
+ transfer_roller_position_ = transfer_roller_position;
+ index_roller_position_ = index_roller_position;
+ clamped_ = clamped;
+ printf("Disc is at %f\n", position_);
+ }
+
+ double position() const {
+ return position_;
+ }
+
+ private:
+ double transfer_roller_position_;
+ double index_roller_position_;
+ bool clamped_;
+ double position_;
+};
+
+
+// Class which simulates the index and sends out queue messages containing the
+// position.
+class IndexMotorSimulation {
+ public:
+ // Constructs a motor simulation. initial_position is the inital angle of the
+ // index, which will be treated as 0 by the encoder.
+ IndexMotorSimulation()
+ : index_plant_(new StateFeedbackPlant<2, 1, 1>(MakeIndexPlant())),
+ transfer_plant_(new StateFeedbackPlant<2, 1, 1>(MakeTransferPlant())),
+ my_index_loop_(".frc971.control_loops.index",
+ 0x1a7b7094, ".frc971.control_loops.index.goal",
+ ".frc971.control_loops.index.position",
+ ".frc971.control_loops.index.output",
+ ".frc971.control_loops.index.status") {
+ }
+
+ // Starts a disc at the start of the index.
+ void InsertDisc() {
+ frisbees.push_back(Frisbee(transfer_roller_position(),
+ index_roller_position()));
+ }
+
+ // Returns true if the bottom disc sensor is triggered.
+ bool BottomDiscDetect() const {
+ bool bottom_disc_detect = false;
+ for (const Frisbee &frisbee : frisbees) {
+ bottom_disc_detect |= frisbee.bottom_disc_detect();
+ }
+ return bottom_disc_detect;
+ }
+
+ // Returns true if the top disc sensor is triggered.
+ bool TopDiscDetect() const {
+ bool top_disc_detect = false;
+ for (const Frisbee &frisbee : frisbees) {
+ top_disc_detect |= frisbee.top_disc_detect();
+ }
+ return top_disc_detect;
+ }
+
+ void UpdateDiscs(bool clamped) {
+ for (Frisbee &frisbee : frisbees) {
+ // TODO(aschuh): Simulate clamping
+ frisbee.UpdatePosition(transfer_roller_position(),
+ index_roller_position(),
+ clamped);
+ }
+ }
+
+ // Sends out the position queue messages.
+ void SendPositionMessage() {
+ ::aos::ScopedMessagePtr<control_loops::IndexLoop::Position> position =
+ my_index_loop_.position.MakeMessage();
+ position->index_position = index_roller_position();
+ position->bottom_disc_detect = BottomDiscDetect();
+ position->top_disc_detect = TopDiscDetect();
+ printf("bdd: %x tdd: %x\n", position->bottom_disc_detect,
+ position->top_disc_detect);
+ position.Send();
+ }
+
+ // Simulates the index moving for one timestep.
+ void Simulate() {
+ EXPECT_TRUE(my_index_loop_.output.FetchLatest());
+
+ index_plant_->U << my_index_loop_.output->index_voltage;
+ index_plant_->Update();
+
+ transfer_plant_->U << my_index_loop_.output->transfer_voltage;
+ transfer_plant_->Update();
+ printf("tv: %f iv: %f tp : %f ip: %f\n",
+ my_index_loop_.output->transfer_voltage,
+ my_index_loop_.output->index_voltage,
+ transfer_roller_position(), index_roller_position());
+
+ UpdateDiscs(my_index_loop_.output->disc_clamped);
+ }
+
+ ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> index_plant_;
+ ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> transfer_plant_;
+
+ // Returns the absolute angle of the index.
+ double index_roller_position() const {
+ return index_plant_->Y(0, 0);
+ }
+
+ // Returns the absolute angle of the index.
+ double transfer_roller_position() const {
+ return transfer_plant_->Y(0, 0);
+ }
+
+ ::std::vector<Frisbee> frisbees;
+
+ private:
+ IndexLoop my_index_loop_;
+};
+
+
+class IndexTest : public ::testing::Test {
+ protected:
+ IndexTest() : my_index_loop_(".frc971.control_loops.index",
+ 0x1a7b7094, ".frc971.control_loops.index.goal",
+ ".frc971.control_loops.index.position",
+ ".frc971.control_loops.index.output",
+ ".frc971.control_loops.index.status"),
+ index_motor_(&my_index_loop_),
+ index_motor_plant_(),
+ loop_count_(0) {
+ // Flush the robot state queue so we can use clean shared memory for this
+ // test.
+ ::aos::robot_state.Clear();
+ SendDSPacket(true);
+ Time::EnableMockTime(Time(0, 0));
+ }
+
+ virtual ~IndexTest() {
+ ::aos::robot_state.Clear();
+ Time::DisableMockTime();
+ }
+
+ // Sends a DS packet with the enable bit set to enabled.
+ void SendDSPacket(bool enabled) {
+ ::aos::robot_state.MakeWithBuilder().enabled(enabled)
+ .autonomous(false)
+ .team_id(971).Send();
+ ::aos::robot_state.FetchLatest();
+ }
+
+ // Updates the current mock time.
+ void UpdateTime() {
+ loop_count_ += 1;
+ Time::SetMockTime(Time::InMS(10 * loop_count_));
+ }
+
+ ::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 pointer to shared memory that
+ // is no longer valid.
+ IndexLoop my_index_loop_;
+
+ // Create a loop and simulation plant.
+ IndexMotor index_motor_;
+ IndexMotorSimulation index_motor_plant_;
+
+ int loop_count_;
+};
+
+// Tests that the index grabs 1 disc and places it at the correct position.
+TEST_F(IndexTest, GrabSingleDisc) {
+ my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send();
+ for (int i = 0; i < 250; ++i) {
+ index_motor_plant_.SendPositionMessage();
+ index_motor_.Iterate();
+ if (i == 100) {
+ EXPECT_EQ(0, index_motor_plant_.index_roller_position());
+ index_motor_plant_.InsertDisc();
+ }
+ index_motor_plant_.Simulate();
+ SendDSPacket(true);
+ UpdateTime();
+ }
+
+ EXPECT_TRUE(my_index_loop_.status.FetchLatest());
+ EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 1);
+ EXPECT_EQ(static_cast<size_t>(1), index_motor_plant_.frisbees.size());
+ EXPECT_NEAR(
+ kIndexStartPosition + IndexMotor::ConvertDiscAngleToDiscPosition(M_PI),
+ index_motor_plant_.frisbees[0].position(), 0.01);
+}
+
+// Test that pulling in a second disc works correctly.
+// Test that HOLD still finishes the disc correctly.
+// Test that pulling a disc down works correctly and ready_to_intake waits.
+
+} // namespace testing
+} // namespace control_loops
+} // namespace frc971