Preload -> shoot now works, cRIO reboot support, and invalid states rejected.
diff --git a/frc971/control_loops/index/index.cc b/frc971/control_loops/index/index.cc
index 98d51b8..f17164c 100644
--- a/frc971/control_loops/index/index.cc
+++ b/frc971/control_loops/index/index.cc
@@ -29,7 +29,9 @@
loader_up_(false),
disc_clamped_(false),
disc_ejected_(false),
- last_bottom_disc_detect_(false) {
+ last_bottom_disc_detect_(false),
+ no_prior_position_(true),
+ missing_position_count_(0) {
}
/*static*/ const double IndexMotor::kTransferStartPosition = 0.0;
@@ -146,8 +148,13 @@
const control_loops::IndexLoop::Position *position,
control_loops::IndexLoop::Output *output,
control_loops::IndexLoop::Status *status) {
- // Make goal easy to work with.
+ // Make goal easy to work with and sanity check it.
Goal goal_enum = static_cast<Goal>(goal->goal_state);
+ if (goal->goal_state < 0 || goal->goal_state > 4) {
+ LOG(ERROR, "Goal state is %d which is out of range. Going to HOLD.\n",
+ goal->goal_state);
+ goal_enum = Goal::HOLD;
+ }
// Disable the motors now so that all early returns will return with the
// motors disabled.
@@ -164,14 +171,32 @@
// Compute a safe index position that we can use.
if (position) {
wrist_loop_->Y << position->index_position;
+ // Set the goal to be the current position if this is the first time through
+ // so we don't always spin the indexer to the 0 position before starting.
+ if (no_prior_position_) {
+ wrist_loop_->R << wrist_loop_->Y(0, 0), 0.0;
+ no_prior_position_ = false;
+ }
+
+ // If the cRIO is gone for 1/2 of a second, assume that it rebooted.
+ if (missing_position_count_ > 50) {
+ // Adjust the disc positions so that they don't have to move.
+ const double disc_offset =
+ position->index_position - wrist_loop_->X_hat(0, 0);
+ for (auto frisbee = frisbees_.begin();
+ frisbee != frisbees_.end(); ++frisbee) {
+ frisbee->OffsetDisc(disc_offset);
+ }
+ }
+ missing_position_count_ = 0;
+ } else {
+ ++missing_position_count_;
}
const double index_position = wrist_loop_->X_hat(0, 0);
// TODO(aschuh): Watch for top disc detect and update the frisbee
// position.
- // TODO(aschuh): Horizontal and centering should be here as well...
-
// Bool to track if it is safe for the goal to change yet.
bool safe_to_change_state_ = true;
switch (safe_goal_) {
@@ -224,7 +249,7 @@
// Check all non-indexed discs and see if they should be indexed.
for (auto frisbee = frisbees_.begin();
- frisbee != frisbees_.end(); ++frisbee) {
+ frisbee != frisbees_.end(); ++frisbee) {
if (!frisbee->has_been_indexed_) {
intake_voltage = transfer_voltage = 12.0;
Time elapsed_negedge_time = now -
@@ -318,6 +343,12 @@
// Stage the discs back a bit.
wrist_loop_->R << ready_disc_position, 0.0;
+ // Shoot if we are grabbed and being asked to shoot.
+ if (loader_state_ == LoaderState::GRABBED &&
+ safe_goal_ == Goal::SHOOT) {
+ loader_goal_ = LoaderGoal::SHOOT_AND_RESET;
+ }
+
// Must wait until it has been grabbed to continue.
if (loader_state_ == LoaderState::GRABBING) {
safe_to_change_state_ = false;
@@ -487,6 +518,9 @@
if (output) {
output->intake_voltage = intake_voltage;
output->transfer_voltage = transfer_voltage;
+ // TODO(aschuh): Count the number of cycles with power below
+ // kFrictionVoltage and if it is too high, turn the motor off.
+ // 50 cycles, 5 volts? Need data...
output->index_voltage = wrist_loop_->U(0, 0);
output->loader_up = loader_up_;
output->disc_clamped = disc_clamped_;
diff --git a/frc971/control_loops/index/index.h b/frc971/control_loops/index/index.h
index cca30fd..d0ee666 100644
--- a/frc971/control_loops/index/index.h
+++ b/frc971/control_loops/index/index.h
@@ -12,6 +12,9 @@
namespace frc971 {
namespace control_loops {
+namespace testing {
+class IndexTest_InvalidStateTest_Test;
+}
class IndexMotor
: public aos::control_loops::ControlLoop<control_loops::IndexLoop> {
@@ -120,6 +123,12 @@
return index_start_position_;
}
+ // Shifts the disc down the indexer by the provided offset. This is to
+ // handle when the cRIO reboots.
+ void OffsetDisc(double offset) {
+ index_start_position_ += offset;
+ }
+
// Posedge and negedge disc times.
::aos::time::Time bottom_posedge_time_;
::aos::time::Time bottom_negedge_time_;
@@ -138,6 +147,7 @@
control_loops::IndexLoop::Status *status);
private:
+ friend class testing::IndexTest_InvalidStateTest_Test;
// Sets disc_position to the minimum or maximum disc position.
// Returns true if there were discs, and false if there weren't.
// On false, disc_position is left unmodified.
@@ -223,6 +233,11 @@
::std::deque<Frisbee> frisbees_;
// std::array ?
+ // True if we haven't seen a position before.
+ bool no_prior_position_;
+ // Number of position messages that we have missed in a row.
+ uint32_t missing_position_count_;
+
DISALLOW_COPY_AND_ASSIGN(IndexMotor);
};
diff --git a/frc971/control_loops/index/index_lib_test.cc b/frc971/control_loops/index/index_lib_test.cc
index cb834ec..01f34c8 100644
--- a/frc971/control_loops/index/index_lib_test.cc
+++ b/frc971/control_loops/index/index_lib_test.cc
@@ -153,6 +153,11 @@
return position_;
}
+ // Simulates the index roller moving without the disc moving.
+ void OffsetIndex(double offset) {
+ index_roller_position_ += offset;
+ }
+
private:
// Previous transfer roller position for computing deltas.
double transfer_roller_position_;
@@ -191,7 +196,7 @@
bool BottomDiscDetect() const {
bool bottom_disc_detect = false;
for (auto frisbee = frisbees.begin();
- frisbee != frisbees.end(); ++frisbee) {
+ frisbee != frisbees.end(); ++frisbee) {
bottom_disc_detect |= frisbee->bottom_disc_detect();
}
return bottom_disc_detect;
@@ -201,7 +206,7 @@
bool TopDiscDetect() const {
bool top_disc_detect = false;
for (auto frisbee = frisbees.begin();
- frisbee != frisbees.end(); ++frisbee) {
+ frisbee != frisbees.end(); ++frisbee) {
top_disc_detect |= frisbee->top_disc_detect();
}
return top_disc_detect;
@@ -210,7 +215,7 @@
// Updates all discs, and verifies that the state of the system is sane.
void UpdateDiscs(bool clamped, bool lifted, bool ejected) {
for (auto frisbee = frisbees.begin();
- frisbee != frisbees.end(); ++frisbee) {
+ frisbee != frisbees.end(); ++frisbee) {
frisbee->UpdatePosition(transfer_roller_position(),
index_roller_position(),
clamped,
@@ -221,7 +226,7 @@
// Make sure nobody is too close to anybody else.
Frisbee *last_frisbee = NULL;
for (auto frisbee = frisbees.begin();
- frisbee != frisbees.end(); ++frisbee) {
+ frisbee != frisbees.end(); ++frisbee) {
if (last_frisbee) {
const double distance = frisbee->position() - last_frisbee->position();
double min_distance;
@@ -279,6 +284,14 @@
my_index_loop_.output->disc_ejected);
}
+ // Simulates the index roller moving without the disc moving.
+ void OffsetIndices(double offset) {
+ for (auto frisbee = frisbees.begin();
+ frisbee != frisbees.end(); ++frisbee) {
+ frisbee->OffsetIndex(offset);
+ }
+ }
+
// 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_;
@@ -340,17 +353,22 @@
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) {
+ // Lets N cycles of time pass.
+ void SimulateNCycles(int cycles) {
+ for (int i = 0; i < cycles; ++i) {
index_motor_plant_.SendPositionMessage();
index_motor_.Iterate();
index_motor_plant_.Simulate();
SendDSPacket(true);
UpdateTime();
}
+ }
+
+ // 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.
+ SimulateNCycles(100);
EXPECT_EQ(0, index_motor_plant_.index_roller_position());
my_index_loop_.status.FetchLatest();
@@ -489,13 +507,7 @@
// 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();
- }
+ SimulateNCycles(300);
// Verify that the disc has been grabbed.
my_index_loop_.output.FetchLatest();
@@ -607,13 +619,7 @@
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();
- }
+ SimulateNCycles(300);
my_index_loop_.status.FetchLatest();
EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 0);
@@ -652,13 +658,7 @@
// 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();
- }
+ SimulateNCycles(100);
my_index_loop_.status.FetchLatest();
EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 1);
@@ -706,6 +706,75 @@
index_motor_plant_.frisbees[0].position(), 0.04);
}
+// Tests that we can shoot after grabbing in the loader.
+TEST_F(IndexTest, GrabbedToShoot) {
+ 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();
+ SimulateNCycles(300);
+
+ // Verify that it is preloaded.
+ my_index_loop_.status.FetchLatest();
+ EXPECT_TRUE(my_index_loop_.status->preloaded);
+
+ // Shoot them all.
+ my_index_loop_.goal.MakeWithBuilder().goal_state(4).Send();
+ SimulateNCycles(200);
+
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 0);
+ EXPECT_EQ(my_index_loop_.status->total_disc_count, 2);
+ EXPECT_FALSE(my_index_loop_.status->preloaded);
+}
+
+// Tests that the cRIO can reboot and we don't loose discs.
+TEST_F(IndexTest, cRIOReboot) {
+ LoadNDiscs(2);
+
+ SimulateNCycles(100);
+ for (int i = 0; i < 100; ++i) {
+ // No position for a while is a cRIO reboot.
+ index_motor_.Iterate();
+ index_motor_plant_.Simulate();
+ SendDSPacket(false);
+ UpdateTime();
+ }
+
+ // Shift the plant.
+ const double kPlantOffset = 5000.0;
+ index_motor_plant_.index_plant_->Y(0, 0) += kPlantOffset;
+ index_motor_plant_.index_plant_->X(0, 0) += kPlantOffset;
+
+ // Shift the discs
+ index_motor_plant_.OffsetIndices(kPlantOffset);
+ // Let time elapse to see if the loop wants to move the discs or not.
+ SimulateNCycles(1000);
+
+ // Verify that 2 discs are at the bottom of the hopper.
+ 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 invalid states are rejected.
+TEST_F(IndexTest, InvalidStateTest) {
+ my_index_loop_.goal.MakeWithBuilder().goal_state(10).Send();
+ SimulateNCycles(2);
+ // Verify that the goal is correct.
+ EXPECT_GE(4, static_cast<int>(index_motor_.safe_goal_));
+ EXPECT_LE(0, static_cast<int>(index_motor_.safe_goal_));
+}
+
} // namespace testing
} // namespace control_loops
} // namespace frc971