Indexer now uses the top sensor correctly at slow speeds, finds lost discs, and forgets discs that were phantom.
diff --git a/frc971/control_loops/index/index.cc b/frc971/control_loops/index/index.cc
index 95e69d7..83145c9 100644
--- a/frc971/control_loops/index/index.cc
+++ b/frc971/control_loops/index/index.cc
@@ -79,7 +79,11 @@
last_bottom_disc_detect_(false),
last_top_disc_detect_(false),
no_prior_position_(true),
- missing_position_count_(0) {
+ missing_position_count_(0),
+ upper_open_index_position_(0.0),
+ upper_open_index_position_was_negedge_(false),
+ lower_open_index_position_(0.0),
+ lower_open_index_position_was_negedge_(false) {
}
/*static*/ const double IndexMotor::kTransferStartPosition = 0.0;
@@ -106,14 +110,20 @@
/*static*/ const double IndexMotor::kBottomDiscDetectStop = 0.200025;
/*static*/ const double IndexMotor::kBottomDiscIndexDelay = 0.01;
-// TODO(aschuh): Figure these out.
+// TODO(aschuh): Verify these with the sensor actually on.
/*static*/ const double IndexMotor::kTopDiscDetectStart =
(IndexMotor::kLoaderFreeStopPosition -
- IndexMotor::ConvertDiscAngleToDiscPosition(60 * M_PI / 180));
-// This is a guess for the width of the disc radially. It should be close to 11
-// inches but a bit below.
+ IndexMotor::ConvertDiscAngleToDiscPosition(49 * M_PI / 180));
/*static*/ const double IndexMotor::kTopDiscDetectStop =
- IndexMotor::kTopDiscDetectStart + 10 * 0.0254;
+ (IndexMotor::kLoaderFreeStopPosition +
+ IndexMotor::ConvertDiscAngleToDiscPosition(19 * M_PI / 180));
+
+// I measured the angle between 2 discs. That then gives me the distance
+// between 2 posedges (or negedges). Then subtract off the width of the
+// positive pulse, and that gives the width of the negative pulse.
+/*static*/ const double IndexMotor::kTopDiscDetectMinSeperation =
+ (IndexMotor::ConvertDiscAngleToDiscPosition(120 * M_PI / 180) -
+ (IndexMotor::kTopDiscDetectStop - IndexMotor::kTopDiscDetectStart));
const /*static*/ double IndexMotor::kDiscRadius = 10.875 * 0.0254 / 2;
const /*static*/ double IndexMotor::kRollerRadius = 2.0 * 0.0254 / 2;
@@ -282,6 +292,12 @@
last_bottom_disc_negedge_wait_count_ =
position->bottom_disc_negedge_wait_count;
last_top_disc_posedge_count_ = position->top_disc_posedge_count;
+ last_top_disc_negedge_count_ = position->top_disc_negedge_count;
+ // The open positions for the upper is right here and isn't a hard edge.
+ upper_open_index_position_ = wrist_loop_->Y(0, 0);
+ upper_open_index_position_was_negedge_ = false;
+ lower_open_index_position_ = wrist_loop_->Y(0, 0);
+ lower_open_index_position_was_negedge_ = false;
}
// If the cRIO is gone for over 1/2 of a second, assume that it rebooted.
@@ -291,6 +307,12 @@
last_bottom_disc_negedge_wait_count_ =
position->bottom_disc_negedge_wait_count;
last_top_disc_posedge_count_ = position->top_disc_posedge_count;
+ last_top_disc_negedge_count_ = position->top_disc_negedge_count;
+ // We can't really trust the open range any more if the crio rebooted.
+ upper_open_index_position_ = wrist_loop_->Y(0, 0);
+ upper_open_index_position_was_negedge_ = false;
+ lower_open_index_position_ = wrist_loop_->Y(0, 0);
+ lower_open_index_position_was_negedge_ = false;
// 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);
@@ -306,6 +328,31 @@
const double index_position = wrist_loop_->X_hat(0, 0);
if (position) {
+ // Reset the open region if we saw a negedge.
+ if (position->top_disc_negedge_count != last_top_disc_negedge_count_) {
+ // Saw a negedge, must be a new region.
+ upper_open_index_position_ = position->top_disc_negedge_position;
+ lower_open_index_position_ = position->top_disc_negedge_position;
+ upper_open_index_position_was_negedge_ = true;
+ lower_open_index_position_was_negedge_ = true;
+ }
+
+ // No disc. Expand the open region.
+ if (!position->top_disc_detect) {
+ // If it is higher than it was before, the end of the region is no longer
+ // determined by the negedge.
+ if (index_position > upper_open_index_position_) {
+ upper_open_index_position_ = index_position;
+ upper_open_index_position_was_negedge_ = false;
+ }
+ // If it is lower than it was before, the end of the region is no longer
+ // determined by the negedge.
+ if (index_position < lower_open_index_position_) {
+ lower_open_index_position_ = index_position;
+ lower_open_index_position_was_negedge_ = false;
+ }
+ }
+
if (!position->top_disc_detect) {
// We don't see a disc. Verify that there are no discs that we should be
// seeing.
@@ -329,12 +376,51 @@
// Requires storing when the disc was last seen with the sensor off, and
// figuring out what to do if things go south.
- // Find a disc that we should be seeing. There are 3 cases...
- // 1) The top most disc is going up by the sensor.
- // 2) There is 1 disc almost in the loader, and past the sensor.
- // This is the next disc.
- // 3) The top most disc is coming back down and we are seeing it.
+ // 1 if discs are going up, 0 if we have no clue, and -1 if they are going
+ // down.
+ int disc_direction = 0;
if (wrist_loop_->X_hat(1, 0) > 50.0) {
+ disc_direction = 1;
+ } else if (wrist_loop_->X_hat(1, 0) < -50.0) {
+ disc_direction = -1;
+ } else {
+ // Save the upper and lower positions that we last saw a disc at.
+ // If there is a big buffer above, must be a disc from below.
+ // If there is a big buffer below, must be a disc from above.
+ // This should work to replace the velocity threshold above.
+
+ const double open_width =
+ upper_open_index_position_ - lower_open_index_position_;
+ const double relative_upper_open_precentage =
+ (upper_open_index_position_ - index_position) / open_width;
+ const double relative_lower_open_precentage =
+ (index_position - lower_open_index_position_) / open_width;
+ printf("Width %f upper %f lower %f\n",
+ open_width, relative_upper_open_precentage,
+ relative_lower_open_precentage);
+
+ if (ConvertIndexToDiscPosition(open_width) <
+ kTopDiscDetectMinSeperation * 0.9) {
+ LOG(ERROR, "Discs are way too close to each other. Doing nothing\n");
+ } else if (relative_upper_open_precentage > 0.75) {
+ // Looks like it is a disc going down from above since we are near
+ // the upper edge.
+ disc_direction = -1;
+ printf("Disc edge going down\n");
+ } else if (relative_lower_open_precentage > 0.75) {
+ // Looks like it is a disc going up from below since we are near
+ // the lower edge.
+ disc_direction = 1;
+ printf("Disc edge going up\n");
+ } else {
+ LOG(ERROR,
+ "Got an edge in the middle of what should be an open region.\n");
+ LOG(ERROR, "Open width: %f upper precentage %f %%\n",
+ open_width, relative_upper_open_precentage);
+ }
+ }
+
+ if (disc_direction > 0) {
// Moving up at a reasonable clip.
// Find the highest disc that is below the top disc sensor.
// While we are at it, count the number above and log an error if there
@@ -345,6 +431,8 @@
new_frisbee.index_start_position_ = index_position -
ConvertDiscPositionToIndex(kTopDiscDetectStart -
kIndexStartPosition);
+ ++hopper_disc_count_;
+ ++total_disc_count_;
frisbees_.push_front(new_frisbee);
LOG(WARNING, "Added a disc to the hopper at the top sensor\n");
}
@@ -383,11 +471,11 @@
}
}
printf("Currently have %d discs, saw posedge moving up. "
- "Moving down by %f to %f\n", frisbees_.size(),
+ "Moving down by %f to %f\n", frisbees_.size(),
ConvertIndexToDiscPosition(disc_delta),
highest_frisbee_below_sensor->absolute_position(
wrist_loop_->X_hat(0, 0)));
- } else if (wrist_loop_->X_hat(1, 0) < -50.0) {
+ } else if (disc_direction < 0) {
// Moving down at a reasonable clip.
// There can only be 1 disc up top that would give us a posedge.
// Find it and place it at the one spot that it can be.
@@ -412,12 +500,7 @@
}
}
} else {
- // Save the upper and lower positions that we last saw a disc at.
- // If there is a big buffer above, must be a disc from below.
- // If there is a big buffer below, must be a disc from above.
- // This should work to replace the velocity threshold above.
- // TODO(aschuh): Do something!
- //
+ LOG(ERROR, "Not sure how to handle the upper posedge, doing nothing\n");
}
}
}
@@ -537,11 +620,14 @@
// No discs! We are always ready for more if we aren't being
// asked to change state.
status->ready_to_intake = (safe_goal_ == goal_enum);
+ printf("Ready to intake, zero discs. %d %d %d\n",
+ status->ready_to_intake, hopper_disc_count_, safe_goal_);
}
// Turn on the transfer roller if we are ready.
if (status->ready_to_intake && hopper_disc_count_ < 4 &&
safe_goal_ == Goal::INTAKE) {
+ printf("Go\n");
intake_voltage = transfer_voltage = 12.0;
}
}
@@ -570,6 +656,7 @@
// We already have a disc in the loader.
// Stage the discs back a bit.
wrist_loop_->R << ready_disc_position, 0.0;
+ printf("Loader not ready but asked to shoot\n");
// Shoot if we are grabbed and being asked to shoot.
if (loader_state_ == LoaderState::GRABBED &&
@@ -603,10 +690,64 @@
}
// This frisbee is now gone. Take it out of the queue.
frisbees_.pop_back();
- --hopper_disc_count_;
}
}
}
+ } else {
+ if (loader_state_ != LoaderState::READY) {
+ // 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;
+ }
+ } else {
+ // Ok, no discs in sight. Spin the hopper up by 150% of it's full
+ // range and verify that we don't see anything.
+ printf("Moving the indexer to verify that it is clear\n");
+ const double hopper_clear_verification_position =
+ lower_open_index_position_ +
+ ConvertDiscPositionToIndex(kIndexFreeLength) * 1.5;
+
+ wrist_loop_->R << hopper_clear_verification_position, 0.0;
+ if (::std::abs(wrist_loop_->X_hat(0, 0) -
+ hopper_clear_verification_position) <
+ ConvertDiscPositionToIndex(0.05)) {
+ printf("Should be empty\n");
+ // We are at the end of the range. There are no more discs here.
+ while (frisbees_.size() > 0) {
+ LOG(ERROR, "Dropping an extra disc since it can't exist\n");
+ frisbees_.pop_back();
+ --hopper_disc_count_;
+ --total_disc_count_;
+ }
+ if (hopper_disc_count_ != 0) {
+ LOG(ERROR,
+ "Emptied the hopper out but there are still discs there\n");
+ }
+ }
+ }
+ }
+
+ {
+ const double hopper_clear_verification_position =
+ lower_open_index_position_ +
+ ConvertDiscPositionToIndex(kIndexFreeLength) * 1.5;
+
+ if (wrist_loop_->X_hat(0, 0) >
+ hopper_clear_verification_position +
+ ConvertDiscPositionToIndex(0.05)) {
+ // We are at the end of the range. There are no more discs here.
+ while (frisbees_.size() > 0) {
+ LOG(ERROR, "Dropping an extra disc since it can't exist\n");
+ frisbees_.pop_back();
+ --hopper_disc_count_;
+ --total_disc_count_;
+ }
+ if (hopper_disc_count_ != 0) {
+ LOG(ERROR,
+ "Emptied the hopper out but there are still discs there\n");
+ }
+ }
}
printf("READY_SHOOTER or SHOOT\n");
@@ -706,6 +847,7 @@
disc_ejected_ = true;
loader_state_ = LoaderState::LOWERING;
loader_countdown_ = kLoweringDelay;
+ --hopper_disc_count_;
case LoaderState::LOWERING:
printf("Loader LOWERING %d\n", loader_countdown_);
// Lowering the loader back down.
@@ -743,6 +885,7 @@
last_bottom_disc_negedge_wait_count_ =
position->bottom_disc_negedge_wait_count;
last_top_disc_posedge_count_ = position->top_disc_posedge_count;
+ last_top_disc_negedge_count_ = position->top_disc_negedge_count;
}
status->hopper_disc_count = hopper_disc_count_;
@@ -761,6 +904,9 @@
if (safe_to_change_state_) {
safe_goal_ = goal_enum;
}
+ if (hopper_disc_count_ < 0) {
+ LOG(ERROR, "NEGATIVE DISCS. VERY VERY BAD\n");
+ }
}
} // namespace control_loops
diff --git a/frc971/control_loops/index/index.h b/frc971/control_loops/index/index.h
index 7a9f6ac..64d515a 100644
--- a/frc971/control_loops/index/index.h
+++ b/frc971/control_loops/index/index.h
@@ -14,7 +14,7 @@
namespace control_loops {
namespace testing {
class IndexTest_InvalidStateTest_Test;
-class IndexTest_ShiftedDiscsAreRefound_Test;
+class IndexTest_LostDisc_Test;
}
class IndexMotor
@@ -64,6 +64,9 @@
static const double kTopDiscDetectStart;
static const double kTopDiscDetectStop;
+ // Minimum distance between 2 frisbees as seen by the top disc detect sensor.
+ static const double kTopDiscDetectMinSeperation;
+
// Converts the angle of the indexer to the angle of the disc.
static double ConvertIndexToDiscAngle(const double angle);
// Converts the angle of the indexer to the position that the center of the
@@ -160,6 +163,7 @@
double index_start_position_;
};
+ // Returns where the indexer thinks the frisbees are.
const ::std::deque<Frisbee> &frisbees() const { return frisbees_; }
protected:
@@ -171,7 +175,7 @@
private:
friend class testing::IndexTest_InvalidStateTest_Test;
- friend class testing::IndexTest_ShiftedDiscsAreRefound_Test;
+ friend class testing::IndexTest_LostDisc_Test;
// This class implements the CapU function correctly given all the extra
// information that we know about from the wrist motor.
@@ -206,8 +210,8 @@
::std::unique_ptr<IndexStateFeedbackLoop> wrist_loop_;
// Count of the number of discs that we have collected.
- uint32_t hopper_disc_count_;
- uint32_t total_disc_count_;
+ int32_t hopper_disc_count_;
+ int32_t total_disc_count_;
enum class Goal {
// Hold position, in a low power state.
@@ -281,16 +285,27 @@
int32_t last_bottom_disc_negedge_count_;
int32_t last_bottom_disc_negedge_wait_count_;
int32_t last_top_disc_posedge_count_;
+ int32_t last_top_disc_negedge_count_;
// Frisbees are in order such that the newest frisbee is on the front.
::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_;
+ // Upper position that is known to be open on the indexer because we saw it
+ // open.
+ double upper_open_index_position_;
+ // True if the upper position was set by a negedge and can be truly trusted.
+ bool upper_open_index_position_was_negedge_;
+ // Lower position that is known to be open on the indexer because we saw it
+ // open.
+ double lower_open_index_position_;
+ // True if the lower position was set by a negedge and can be truly trusted.
+ bool lower_open_index_position_was_negedge_;
+
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 6d488fd..6963a64 100644
--- a/frc971/control_loops/index/index_lib_test.cc
+++ b/frc971/control_loops/index/index_lib_test.cc
@@ -443,10 +443,14 @@
".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()));
+ // Starts a disc offset from the start of the index.
+ void InsertDisc(double offset = IndexMotor::kBottomDiscDetectStart - 0.001) {
+ Frisbee new_frisbee(transfer_roller_position(),
+ index_roller_position(),
+ offset);
+ ASSERT_FALSE(new_frisbee.bottom_disc_detect());
+ ASSERT_FALSE(new_frisbee.top_disc_detect());
+ frisbees.push_back(new_frisbee);
}
// Returns true if the bottom disc sensor is triggered.
@@ -906,7 +910,7 @@
}
my_index_loop_.status.FetchLatest();
- EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 1);
+ EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 2);
EXPECT_EQ(my_index_loop_.status->total_disc_count, 2);
my_index_loop_.output.FetchLatest();
EXPECT_TRUE(my_index_loop_.output->disc_clamped);
@@ -1038,7 +1042,7 @@
SimulateNCycles(100);
my_index_loop_.status.FetchLatest();
- EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 1);
+ EXPECT_EQ(my_index_loop_.status->hopper_disc_count, 2);
EXPECT_EQ(my_index_loop_.status->total_disc_count, 2);
my_index_loop_.output.FetchLatest();
EXPECT_TRUE(my_index_loop_.output->disc_clamped);
@@ -1185,11 +1189,71 @@
TestDualLostDiscs(-0.10, -0.15);
}
-// TODO(aschuh): Test that we find discs corectly when moving them up.
+// Verifies that the indexer is ready to intake imediately after loading.
+TEST_F(IndexTest, IntakingAfterLoading) {
+ LoadNDiscs(1);
+ my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
+ SimulateNCycles(200);
+ my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send();
+ SimulateNCycles(10);
+ my_index_loop_.output.FetchLatest();
+ EXPECT_EQ(12.0, my_index_loop_.output->transfer_voltage);
+ my_index_loop_.status.FetchLatest();
+ EXPECT_TRUE(my_index_loop_.status->ready_to_intake);
+}
-// TODO(aschuh): Exercise the disc coming down from above code and verify it.
-// If possible.
+// Verifies that the indexer is ready to intake imediately after loading.
+TEST_F(IndexTest, CanShootOneDiscAfterReady) {
+ LoadNDiscs(1);
+ my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
+ SimulateNCycles(200);
+ my_index_loop_.goal.MakeWithBuilder().goal_state(4).Send();
+ SimulateNCycles(100);
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(1, my_index_loop_.status->total_disc_count);
+ EXPECT_EQ(0, my_index_loop_.status->hopper_disc_count);
+}
+// Verifies that the indexer is ready to intake imediately after loading.
+TEST_F(IndexTest, GotExtraDisc) {
+ LoadNDiscs(1);
+ my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
+ SimulateNCycles(200);
+
+ double index_roller_position = index_motor_plant_.index_roller_position();
+ index_motor_plant_.InsertDisc(IndexMotor::kTopDiscDetectStart - 0.1);
+ index_motor_plant_.InsertDisc(IndexMotor::kTopDiscDetectStart - 0.6);
+ SimulateNCycles(100);
+ my_index_loop_.goal.MakeWithBuilder().goal_state(4).Send();
+ SimulateNCycles(300);
+
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(3, my_index_loop_.status->total_disc_count);
+ EXPECT_EQ(0, my_index_loop_.status->hopper_disc_count);
+ EXPECT_LT(IndexMotor::ConvertDiscAngleToIndex(4 * M_PI),
+ index_motor_plant_.index_roller_position() - index_roller_position);
+}
+
+// Verifies that the indexer is ready to intake imediately after loading.
+TEST_F(IndexTest, LostDisc) {
+ LoadNDiscs(3);
+ my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
+ SimulateNCycles(200);
+
+ index_motor_plant_.frisbees.erase(
+ index_motor_plant_.frisbees.begin() + 1);
+
+ double index_roller_position = index_motor_plant_.index_roller_position();
+ my_index_loop_.goal.MakeWithBuilder().goal_state(4).Send();
+ SimulateNCycles(300);
+
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(2, my_index_loop_.status->total_disc_count);
+ EXPECT_EQ(0, my_index_loop_.status->hopper_disc_count);
+ EXPECT_LT(IndexMotor::ConvertDiscAngleToIndex(4 * M_PI),
+ index_motor_plant_.index_roller_position() - index_roller_position);
+ EXPECT_EQ(0u, index_motor_.frisbees_.size());
+}
} // namespace testing
} // namespace control_loops