| #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 { |
| |
| 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 = IndexMotor::kBottomDiscDetectStart) |
| : transfer_roller_position_(transfer_roller_position), |
| index_roller_position_(index_roller_position), |
| position_(position), |
| has_been_shot_(false) { |
| } |
| |
| // Returns true if the frisbee is controlled by the transfer roller. |
| bool IsTouchingTransfer() const { |
| return (position_ >= IndexMotor::kBottomDiscDetectStart && |
| position_ <= IndexMotor::kIndexStartPosition); |
| } |
| |
| // Returns true if the frisbee is in a place where it is unsafe to grab. |
| bool IsUnsafeToGrab() const { |
| return (position_ > (IndexMotor::kLoaderFreeStopPosition) && |
| position_ < IndexMotor::kGrabberStartPosition); |
| } |
| |
| // Returns true if the frisbee is controlled by the indexing roller. |
| bool IsTouchingIndex() const { |
| return (position_ >= IndexMotor::kIndexStartPosition && |
| position_ < IndexMotor::kGrabberStartPosition); |
| } |
| |
| // Returns true if the frisbee is in a position such that the disc can be |
| // lifted. |
| bool IsUnsafeToLift() const { |
| return (position_ >= IndexMotor::kLoaderFreeStopPosition && |
| position_ <= IndexMotor::kReadyToLiftPosition); |
| } |
| |
| // Returns true if the frisbee is in a position such that the grabber will |
| // pull it into the loader. |
| bool IsTouchingGrabber() const { |
| return (position_ >= IndexMotor::kGrabberStartPosition && |
| position_ < IndexMotor::kReadyToLiftPosition); |
| } |
| |
| // Returns true if the frisbee is in a position such that the disc can be |
| // lifted. |
| bool IsTouchingLoader() const { |
| return (position_ >= IndexMotor::kReadyToLiftPosition && |
| position_ < IndexMotor::kLifterStopPosition); |
| } |
| |
| // Returns true if the frisbee is touching the ejector. |
| bool IsTouchingEjector() const { |
| return (position_ >= IndexMotor::kLifterStopPosition && |
| position_ < IndexMotor::kEjectorStopPosition); |
| } |
| |
| // Returns true if the disc is triggering the bottom disc detect sensor. |
| bool bottom_disc_detect() const { |
| return (position_ >= IndexMotor::kBottomDiscDetectStart && |
| position_ <= IndexMotor::kBottomDiscDetectStop); |
| } |
| |
| // Returns true if the disc is triggering the top disc detect sensor. |
| bool top_disc_detect() const { |
| return (position_ >= IndexMotor::kTopDiscDetectStart && |
| position_ <= IndexMotor::kTopDiscDetectStop); |
| } |
| |
| // Updates the position of the frisbee in the frisbee path. |
| void UpdatePosition(double transfer_roller_position, |
| double index_roller_position, |
| bool clamped, |
| bool lifted, |
| bool ejected) { |
| if (IsTouchingTransfer() || position() < 0.0) { |
| position_ += IndexMotor::ConvertTransferToDiscPosition( |
| transfer_roller_position - transfer_roller_position_); |
| printf("Transfer Roller: "); |
| } else if (IsTouchingIndex()) { |
| position_ += ::std::min( |
| IndexMotor::ConvertIndexToDiscPosition( |
| index_roller_position - index_roller_position_), |
| IndexMotor::kGrabberStartPosition); |
| // Verify that we aren't trying to grab or lift when it isn't safe. |
| EXPECT_FALSE(clamped && IsUnsafeToGrab()); |
| EXPECT_FALSE(lifted && IsUnsafeToLift()); |
| printf("Index: "); |
| } else if (IsTouchingGrabber()) { |
| if (clamped) { |
| const double grabber_dx = IndexMotor::kGrabberMovementVelocity / 100.0; |
| position_ = ::std::min(position_ + grabber_dx, |
| IndexMotor::kReadyToLiftPosition); |
| } |
| EXPECT_FALSE(lifted); |
| EXPECT_FALSE(ejected); |
| printf("Grabber: "); |
| } else if (IsTouchingLoader()) { |
| if (lifted) { |
| const double lifter_dx = IndexMotor::kLifterMovementVelocity / 100.0; |
| position_ = ::std::min(position_ + lifter_dx, |
| IndexMotor::kLifterStopPosition); |
| } |
| EXPECT_TRUE(clamped); |
| EXPECT_FALSE(ejected); |
| printf("Loader: "); |
| } else if (IsTouchingEjector()) { |
| EXPECT_TRUE(lifted); |
| if (ejected) { |
| const double ejector_dx = IndexMotor::kEjectorMovementVelocity / 100.0; |
| position_ = ::std::min(position_ + ejector_dx, |
| IndexMotor::kEjectorStopPosition); |
| EXPECT_FALSE(clamped); |
| } |
| printf("Ejector: "); |
| } else if (position_ == IndexMotor::kEjectorStopPosition) { |
| printf("Shot: "); |
| has_been_shot_ = true; |
| } |
| transfer_roller_position_ = transfer_roller_position; |
| index_roller_position_ = index_roller_position; |
| printf("Disc is at %f\n", position_); |
| } |
| |
| // Returns if the disc has been shot and can be removed from the robot. |
| bool has_been_shot() const { |
| return has_been_shot_; |
| } |
| |
| // Returns the position of the disc in the system. |
| double position() const { |
| return position_; |
| } |
| |
| private: |
| // Previous transfer roller position for computing deltas. |
| double transfer_roller_position_; |
| // Previous index roller position for computing deltas. |
| double index_roller_position_; |
| // Position in the robot. |
| double position_; |
| // True if the disc has been shot. |
| bool has_been_shot_; |
| }; |
| |
| |
| // 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; |
| } |
| |
| // Updates all discs, and verifies that the state of the system is sane. |
| void UpdateDiscs(bool clamped, bool lifted, bool ejected) { |
| for (Frisbee &frisbee : frisbees) { |
| frisbee.UpdatePosition(transfer_roller_position(), |
| index_roller_position(), |
| clamped, |
| lifted, |
| ejected); |
| } |
| |
| // Make sure nobody is too close to anybody else. |
| Frisbee *last_frisbee = NULL; |
| for (Frisbee &frisbee : frisbees) { |
| if (last_frisbee) { |
| const double distance = frisbee.position() - last_frisbee->position(); |
| double min_distance; |
| if (frisbee.IsTouchingTransfer() || |
| last_frisbee->IsTouchingTransfer()) { |
| min_distance = 0.3; |
| } else { |
| min_distance = |
| IndexMotor::ConvertDiscAngleToDiscPosition(M_PI * 2.0 / 3.0); |
| } |
| |
| EXPECT_LT(min_distance, ::std::abs(distance)) << "Discs too close"; |
| } |
| last_frisbee = &frisbee; |
| } |
| |
| // Remove any shot frisbees. |
| for (int i = 0; i < static_cast<int>(frisbees.size()); ++i) { |
| if (frisbees[i].has_been_shot()) { |
| shot_frisbees.push_back(frisbees[i]); |
| frisbees.erase(frisbees.begin() + i); |
| --i; |
| } |
| } |
| } |
| |
| // 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, |
| my_index_loop_.output->loader_up, |
| my_index_loop_.output->disc_ejected); |
| } |
| |
| // Plants for the index and transfer rollers. |
| ::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); |
| } |
| |
| // Frisbees being tracked in the robot. |
| ::std::vector<Frisbee> frisbees; |
| // Frisbees that have been shot. |
| ::std::vector<Frisbee> shot_frisbees; |
| |
| private: |
| // Control loop for the indexer. |
| 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_)); |
| } |
| |
| // Loads n discs into the indexer at the bottom. |
| void LoadNDiscs(int n) { |
| my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send(); |
| // Spin it up. |
| for (int i = 0; i < 100; ++i) { |
| index_motor_plant_.SendPositionMessage(); |
| index_motor_.Iterate(); |
| index_motor_plant_.Simulate(); |
| SendDSPacket(true); |
| UpdateTime(); |
| } |
| |
| EXPECT_EQ(0, index_motor_plant_.index_roller_position()); |
| my_index_loop_.status.FetchLatest(); |
| EXPECT_TRUE(my_index_loop_.status->ready_to_intake); |
| |
| // Stuff N discs in, waiting between each one for a tiny bit of time so they |
| // don't get too close. |
| int num_grabbed = 0; |
| int wait_counter = 0; |
| while (num_grabbed < n) { |
| index_motor_plant_.SendPositionMessage(); |
| index_motor_.Iterate(); |
| if (!index_motor_plant_.BottomDiscDetect()) { |
| if (wait_counter > 0) { |
| --wait_counter; |
| } else { |
| index_motor_plant_.InsertDisc(); |
| ++num_grabbed; |
| wait_counter = 3; |
| } |
| } |
| index_motor_plant_.Simulate(); |
| SendDSPacket(true); |
| UpdateTime(); |
| } |
| |
| // Settle. |
| for (int i = 0; i < 100; ++i) { |
| index_motor_plant_.SendPositionMessage(); |
| index_motor_.Iterate(); |
| index_motor_plant_.Simulate(); |
| SendDSPacket(true); |
| UpdateTime(); |
| } |
| } |
| |
| // Copy of core that works in this process only. |
| ::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_; |
| |
| // Number of loop cycles that have been executed for tracking the current |
| // time. |
| 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(); |
| } |
| if (i > 0) { |
| EXPECT_TRUE(my_index_loop_.status.FetchLatest()); |
| EXPECT_TRUE(my_index_loop_.status->ready_to_intake); |
| } |
| index_motor_plant_.Simulate(); |
| SendDSPacket(true); |
| UpdateTime(); |
| } |
| |
| 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( |
| IndexMotor::kIndexStartPosition + IndexMotor::ConvertDiscAngleToDiscPosition(M_PI), |
| index_motor_plant_.frisbees[0].position(), 0.05); |
| } |
| |
| // Tests that the index grabs 1 disc and places it at the correct position when |
| // told to hold immediately after the disc starts into the bot. |
| TEST_F(IndexTest, GrabAndHold) { |
| my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send(); |
| for (int i = 0; i < 200; ++i) { |
| index_motor_plant_.SendPositionMessage(); |
| index_motor_.Iterate(); |
| if (i == 100) { |
| EXPECT_EQ(0, index_motor_plant_.index_roller_position()); |
| index_motor_plant_.InsertDisc(); |
| } else if (i == 102) { |
| // The disc has been seen. Tell the indexer to now hold. |
| my_index_loop_.goal.MakeWithBuilder().goal_state(0).Send(); |
| } else if (i > 102) { |
| my_index_loop_.status.FetchLatest(); |
| EXPECT_FALSE(my_index_loop_.status->ready_to_intake); |
| } |
| index_motor_plant_.Simulate(); |
| SendDSPacket(true); |
| UpdateTime(); |
| } |
| |
| 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( |
| (IndexMotor::kIndexStartPosition + |
| IndexMotor::ConvertDiscAngleToDiscPosition(M_PI)), |
| index_motor_plant_.frisbees[0].position(), 0.04); |
| } |
| |
| // Tests that the index grabs two discs and places them at the correct |
| // positions. |
| TEST_F(IndexTest, GrabTwoDiscs) { |
| LoadNDiscs(2); |
| |
| EXPECT_TRUE(my_index_loop_.status.FetchLatest()); |
| EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 2); |
| EXPECT_EQ(static_cast<size_t>(2), index_motor_plant_.frisbees.size()); |
| EXPECT_NEAR( |
| (IndexMotor::kIndexStartPosition + |
| IndexMotor::ConvertDiscAngleToDiscPosition(M_PI)), |
| index_motor_plant_.frisbees[1].position(), 0.10); |
| EXPECT_NEAR( |
| IndexMotor::ConvertDiscAngleToDiscPosition(M_PI), |
| (index_motor_plant_.frisbees[0].position() - |
| index_motor_plant_.frisbees[1].position()), 0.10); |
| } |
| |
| // Tests that the index grabs 2 discs, and loads one up into the loader to get |
| // ready to shoot. It then pulls the second disc back down to be ready to |
| // intake more. |
| TEST_F(IndexTest, ReadyGrabsOneDisc) { |
| LoadNDiscs(2); |
| |
| // Lift the discs up to the top. Wait a while to let the system settle and |
| // verify that they don't collide. |
| my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send(); |
| for (int i = 0; i < 300; ++i) { |
| index_motor_plant_.SendPositionMessage(); |
| index_motor_.Iterate(); |
| index_motor_plant_.Simulate(); |
| SendDSPacket(true); |
| UpdateTime(); |
| } |
| |
| // Verify that the disc has been grabbed. |
| my_index_loop_.output.FetchLatest(); |
| EXPECT_TRUE(my_index_loop_.output->disc_clamped); |
| // And that we are preloaded. |
| my_index_loop_.status.FetchLatest(); |
| EXPECT_TRUE(my_index_loop_.status->preloaded); |
| |
| // Pull the disc back down and verify that the transfer roller doesn't turn on |
| // until we are ready. |
| my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send(); |
| for (int i = 0; i < 100; ++i) { |
| index_motor_plant_.SendPositionMessage(); |
| index_motor_.Iterate(); |
| |
| EXPECT_TRUE(my_index_loop_.status.FetchLatest()); |
| EXPECT_TRUE(my_index_loop_.output.FetchLatest()); |
| if (!my_index_loop_.status->ready_to_intake) { |
| EXPECT_EQ(my_index_loop_.output->transfer_voltage, 0) |
| << "Transfer should be off until indexer is ready"; |
| } |
| |
| index_motor_plant_.Simulate(); |
| SendDSPacket(true); |
| UpdateTime(); |
| } |
| |
| my_index_loop_.status.FetchLatest(); |
| EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 1); |
| EXPECT_EQ(my_index_loop_.status->total_disc_count, 2); |
| my_index_loop_.output.FetchLatest(); |
| EXPECT_TRUE(my_index_loop_.output->disc_clamped); |
| |
| EXPECT_EQ(static_cast<size_t>(2), index_motor_plant_.frisbees.size()); |
| EXPECT_NEAR(IndexMotor::kReadyToLiftPosition, |
| index_motor_plant_.frisbees[0].position(), 0.01); |
| EXPECT_NEAR( |
| (IndexMotor::kIndexStartPosition + |
| IndexMotor::ConvertDiscAngleToDiscPosition(M_PI)), |
| index_motor_plant_.frisbees[1].position(), 0.10); |
| } |
| |
| // Tests that the index grabs 1 disc and continues to pull it in correctly when |
| // in the READY_LOWER state. The transfer roller should be disabled then. |
| TEST_F(IndexTest, GrabAndReady) { |
| my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send(); |
| for (int i = 0; i < 200; ++i) { |
| index_motor_plant_.SendPositionMessage(); |
| index_motor_.Iterate(); |
| if (i == 100) { |
| EXPECT_EQ(0, index_motor_plant_.index_roller_position()); |
| index_motor_plant_.InsertDisc(); |
| } else if (i == 102) { |
| my_index_loop_.goal.MakeWithBuilder().goal_state(1).Send(); |
| } else if (i > 150) { |
| my_index_loop_.status.FetchLatest(); |
| EXPECT_TRUE(my_index_loop_.status->ready_to_intake); |
| my_index_loop_.output.FetchLatest(); |
| EXPECT_EQ(my_index_loop_.output->transfer_voltage, 0.0); |
| } |
| index_motor_plant_.Simulate(); |
| SendDSPacket(true); |
| UpdateTime(); |
| } |
| |
| 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( |
| (IndexMotor::kIndexStartPosition + |
| IndexMotor::ConvertDiscAngleToDiscPosition(M_PI)), |
| index_motor_plant_.frisbees[0].position(), 0.04); |
| } |
| |
| // Tests that grabbing 4 discs ends up with 4 discs in the bot and us no longer |
| // ready. |
| TEST_F(IndexTest, GrabFourDiscs) { |
| LoadNDiscs(4); |
| |
| EXPECT_TRUE(my_index_loop_.output.FetchLatest()); |
| EXPECT_EQ(my_index_loop_.output->transfer_voltage, 0.0); |
| EXPECT_TRUE(my_index_loop_.status.FetchLatest()); |
| EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 4); |
| EXPECT_FALSE(my_index_loop_.status->ready_to_intake); |
| EXPECT_EQ(static_cast<size_t>(4), index_motor_plant_.frisbees.size()); |
| EXPECT_NEAR( |
| IndexMotor::kIndexStartPosition + IndexMotor::ConvertDiscAngleToDiscPosition(M_PI), |
| index_motor_plant_.frisbees[3].position(), 0.10); |
| EXPECT_NEAR( |
| IndexMotor::ConvertDiscAngleToDiscPosition(M_PI), |
| (index_motor_plant_.frisbees[0].position() - |
| index_motor_plant_.frisbees[1].position()), 0.10); |
| EXPECT_NEAR( |
| IndexMotor::ConvertDiscAngleToDiscPosition(M_PI), |
| (index_motor_plant_.frisbees[1].position() - |
| index_motor_plant_.frisbees[2].position()), 0.10); |
| EXPECT_NEAR( |
| IndexMotor::ConvertDiscAngleToDiscPosition(M_PI), |
| (index_motor_plant_.frisbees[2].position() - |
| index_motor_plant_.frisbees[3].position()), 0.10); |
| } |
| |
| // Tests that shooting 4 discs works. |
| TEST_F(IndexTest, ShootFourDiscs) { |
| LoadNDiscs(4); |
| |
| EXPECT_EQ(static_cast<size_t>(4), index_motor_plant_.frisbees.size()); |
| |
| my_index_loop_.goal.MakeWithBuilder().goal_state(4).Send(); |
| |
| // Lifting and shooting takes a while... |
| for (int i = 0; i < 300; ++i) { |
| index_motor_plant_.SendPositionMessage(); |
| index_motor_.Iterate(); |
| index_motor_plant_.Simulate(); |
| SendDSPacket(true); |
| UpdateTime(); |
| } |
| |
| my_index_loop_.status.FetchLatest(); |
| EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 0); |
| EXPECT_EQ(my_index_loop_.status->total_disc_count, 4); |
| my_index_loop_.output.FetchLatest(); |
| EXPECT_FALSE(my_index_loop_.output->disc_clamped); |
| EXPECT_FALSE(my_index_loop_.output->loader_up); |
| EXPECT_FALSE(my_index_loop_.output->disc_ejected); |
| |
| EXPECT_EQ(static_cast<size_t>(4), index_motor_plant_.shot_frisbees.size()); |
| } |
| |
| // Tests that discs aren't pulled out of the loader half way through being |
| // grabbed when being asked to index. |
| TEST_F(IndexTest, PreloadToIndexEarlyTransition) { |
| LoadNDiscs(2); |
| |
| // Lift the discs up to the top. Wait a while to let the system settle and |
| // verify that they don't collide. |
| my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send(); |
| for (int i = 0; i < 300; ++i) { |
| index_motor_plant_.SendPositionMessage(); |
| index_motor_.Iterate(); |
| index_motor_plant_.Simulate(); |
| SendDSPacket(true); |
| UpdateTime(); |
| // Drop out of the loop as soon as it enters the loader. |
| // This will require it to finish the job before intaking more. |
| my_index_loop_.status.FetchLatest(); |
| if (index_motor_plant_.frisbees[0].position() > |
| IndexMotor::kLoaderFreeStopPosition) { |
| break; |
| } |
| } |
| |
| // Pull the disc back down and verify that the transfer roller doesn't turn on |
| // until we are ready. |
| my_index_loop_.goal.MakeWithBuilder().goal_state(1).Send(); |
| for (int i = 0; i < 100; ++i) { |
| index_motor_plant_.SendPositionMessage(); |
| index_motor_.Iterate(); |
| index_motor_plant_.Simulate(); |
| SendDSPacket(true); |
| UpdateTime(); |
| } |
| |
| my_index_loop_.status.FetchLatest(); |
| EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 1); |
| EXPECT_EQ(my_index_loop_.status->total_disc_count, 2); |
| my_index_loop_.output.FetchLatest(); |
| EXPECT_TRUE(my_index_loop_.output->disc_clamped); |
| |
| EXPECT_EQ(static_cast<size_t>(2), index_motor_plant_.frisbees.size()); |
| EXPECT_NEAR(IndexMotor::kReadyToLiftPosition, |
| index_motor_plant_.frisbees[0].position(), 0.01); |
| EXPECT_NEAR( |
| (IndexMotor::kIndexStartPosition + |
| IndexMotor::ConvertDiscAngleToDiscPosition(M_PI)), |
| index_motor_plant_.frisbees[1].position(), 0.10); |
| } |
| |
| // Tests that disabling while grabbing a disc doesn't cause problems. |
| TEST_F(IndexTest, HandleDisable) { |
| my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send(); |
| for (int i = 0; i < 200; ++i) { |
| index_motor_plant_.SendPositionMessage(); |
| index_motor_.Iterate(); |
| if (i == 100) { |
| EXPECT_EQ(0, index_motor_plant_.index_roller_position()); |
| index_motor_plant_.InsertDisc(); |
| } else if (i == 102) { |
| my_index_loop_.goal.MakeWithBuilder().goal_state(1).Send(); |
| } else if (i > 150) { |
| my_index_loop_.status.FetchLatest(); |
| EXPECT_TRUE(my_index_loop_.status->ready_to_intake); |
| my_index_loop_.output.FetchLatest(); |
| EXPECT_EQ(my_index_loop_.output->transfer_voltage, 0.0); |
| } |
| index_motor_plant_.Simulate(); |
| SendDSPacket(i < 102 || i > 110); |
| UpdateTime(); |
| } |
| |
| 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( |
| (IndexMotor::kIndexStartPosition + |
| IndexMotor::ConvertDiscAngleToDiscPosition(M_PI)), |
| index_motor_plant_.frisbees[0].position(), 0.04); |
| } |
| |
| } // namespace testing |
| } // namespace control_loops |
| } // namespace frc971 |