Indexer wouldn't pick up a second set of discs. Tested and fixed.
diff --git a/frc971/control_loops/index/index.cc b/frc971/control_loops/index/index.cc
index 14c9800..3191729 100644
--- a/frc971/control_loops/index/index.cc
+++ b/frc971/control_loops/index/index.cc
@@ -80,11 +80,7 @@
last_bottom_disc_detect_(false),
last_top_disc_detect_(false),
no_prior_position_(true),
- 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) {
+ missing_position_count_(0) {
}
/*static*/ const double IndexMotor::kTransferStartPosition = 0.0;
@@ -296,10 +292,8 @@
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;
+ upper_open_region_.Restart(wrist_loop_->Y(0, 0));
+ lower_open_region_.Restart(wrist_loop_->Y(0, 0));
}
// If the cRIO is gone for over 1/2 of a second, assume that it rebooted.
@@ -311,10 +305,8 @@
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;
+ upper_open_region_.Restart(wrist_loop_->Y(0, 0));
+ lower_open_region_.Restart(wrist_loop_->Y(0, 0));
// 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);
@@ -332,28 +324,25 @@
if (position) {
// Reset the open region if we saw a negedge.
+ if (position->bottom_disc_negedge_wait_count !=
+ last_bottom_disc_negedge_wait_count_) {
+ // Saw a negedge, must be a new region.
+ lower_open_region_.Restart(position->bottom_disc_negedge_wait_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;
+ upper_open_region_.Restart(position->top_disc_negedge_position);
+ }
+
+ // No disc. Expand the open region.
+ if (!position->bottom_disc_detect) {
+ lower_open_region_.Expand(index_position);
}
// 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;
- }
+ upper_open_region_.Expand(index_position);
}
if (!position->top_disc_detect) {
@@ -392,12 +381,11 @@
// 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 open_width = upper_open_region_.width();
const double relative_upper_open_precentage =
- (upper_open_index_position_ - index_position) / open_width;
+ (upper_open_region_.upper_bound() - index_position) / open_width;
const double relative_lower_open_precentage =
- (index_position - lower_open_index_position_) / open_width;
+ (index_position - upper_open_region_.lower_bound()) / open_width;
printf("Width %f upper %f lower %f\n",
open_width, relative_upper_open_precentage,
relative_lower_open_precentage);
@@ -707,7 +695,8 @@
// 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_ +
+ ::std::min(upper_open_region_.lower_bound(),
+ lower_open_region_.lower_bound()) +
ConvertDiscPositionToIndex(kIndexFreeLength) * 1.5;
wrist_loop_->R << hopper_clear_verification_position, 0.0;
@@ -732,7 +721,8 @@
{
const double hopper_clear_verification_position =
- lower_open_index_position_ +
+ ::std::min(upper_open_region_.lower_bound(),
+ lower_open_region_.lower_bound()) +
ConvertDiscPositionToIndex(kIndexFreeLength) * 1.5;
if (wrist_loop_->X_hat(0, 0) >
diff --git a/frc971/control_loops/index/index.h b/frc971/control_loops/index/index.h
index 54d7c3f..e3a67c5 100644
--- a/frc971/control_loops/index/index.h
+++ b/frc971/control_loops/index/index.h
@@ -17,6 +17,40 @@
class IndexTest_LostDisc_Test;
}
+// This class represents a region of space.
+class Region {
+ public:
+ Region () : upper_bound_(0.0), lower_bound_(0.0) {}
+
+ // Restarts the region tracking by starting over with a 0 width region with
+ // the bounds at [edge, edge].
+ void Restart(double edge) {
+ upper_bound_ = edge;
+ lower_bound_ = edge;
+ }
+
+ // Expands the region to include the new point.
+ void Expand(double new_point) {
+ if (new_point > upper_bound_) {
+ upper_bound_ = new_point;
+ } else if (new_point < lower_bound_) {
+ lower_bound_ = new_point;
+ }
+ }
+
+ // Returns the width of the region.
+ double width() const { return upper_bound_ - lower_bound_; }
+ // Returns the upper and lower bounds.
+ double upper_bound() const { return upper_bound_; }
+ double lower_bound() const { return lower_bound_; }
+
+ private:
+ // Upper bound of the region.
+ double upper_bound_;
+ // Lower bound of the region.
+ double lower_bound_;
+};
+
class IndexMotor
: public aos::control_loops::ControlLoop<control_loops::IndexLoop> {
public:
@@ -295,20 +329,12 @@
// 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_;
+ // The no-disc regions for both the bottom and top beam break sensors.
+ Region upper_open_region_;
+ Region lower_open_region_;
DISALLOW_COPY_AND_ASSIGN(IndexMotor);
};
-
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/index/index_lib_test.cc b/frc971/control_loops/index/index_lib_test.cc
index ab3597c..829cdce 100644
--- a/frc971/control_loops/index/index_lib_test.cc
+++ b/frc971/control_loops/index/index_lib_test.cc
@@ -1254,9 +1254,11 @@
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),
+ EXPECT_LT(IndexMotor::ConvertDiscAngleToIndex(3 * M_PI),
index_motor_plant_.index_roller_position() - index_roller_position);
EXPECT_EQ(0u, index_motor_.frisbees_.size());
+ my_index_loop_.output.FetchLatest();
+ EXPECT_EQ(0.0, my_index_loop_.output->index_voltage);
}
// Verifies that the indexer is ready to intake imediately after loading.
@@ -1275,6 +1277,21 @@
EXPECT_EQ(1, my_index_loop_.status->hopper_disc_count);
}
+// Verifies that the indexer can shoot a disc and then intake and shoot another
+// one. This verifies that the code that forgets discs works correctly.
+TEST_F(IndexTest, CanShootIntakeAndShoot) {
+ for (int i = 1; i < 4; ++i) {
+ LoadNDiscs(1);
+ my_index_loop_.goal.MakeWithBuilder().goal_state(3).Send();
+ SimulateNCycles(200);
+ my_index_loop_.goal.MakeWithBuilder().goal_state(4).Send();
+ SimulateNCycles(500);
+ my_index_loop_.status.FetchLatest();
+ EXPECT_EQ(i, my_index_loop_.status->total_disc_count);
+ EXPECT_EQ(0, my_index_loop_.status->hopper_disc_count);
+ }
+}
+
} // namespace testing
} // namespace control_loops
} // namespace frc971