Fixed a bug in my top disc detect simulation, and the indexer now uses that information when moving quickly to locate discs.
diff --git a/frc971/control_loops/index/index.cc b/frc971/control_loops/index/index.cc
index bf0acb9..95e69d7 100644
--- a/frc971/control_loops/index/index.cc
+++ b/frc971/control_loops/index/index.cc
@@ -21,8 +21,7 @@
double IndexMotor::Frisbee::ObserveNoTopDiscSensor(
double index_position, double index_velocity) {
// The absolute disc position in meters.
- double disc_position = IndexMotor::ConvertIndexToDiscPosition(
- index_position - index_start_position_) + IndexMotor::kIndexStartPosition;
+ double disc_position = absolute_position(index_position);
if (IndexMotor::kTopDiscDetectStart <= disc_position &&
disc_position <= IndexMotor::kTopDiscDetectStop) {
// Whoops, this shouldn't be happening.
@@ -47,12 +46,18 @@
// Now going up. If we didn't see it before, and we don't see it
// now but it should be in view, it must still be below. If it were
// above, it would be going further away from us.
- printf("Moving fast up, shifting disc up\n");
+ printf("Moving fast up, shifting disc down. Disc was at %f\n",
+ absolute_position(index_position));
index_start_position_ += distance_to_below;
+ printf("Moving fast up, shifting disc down. Disc now at %f\n",
+ absolute_position(index_position));
return distance_to_below;
} else {
- printf("Moving fast down, shifting disc down\n");
+ printf("Moving fast down, shifting disc up. Disc was at %f\n",
+ absolute_position(index_position));
index_start_position_ -= distance_to_above;
+ printf("Moving fast down, shifting disc up. Disc now at %f\n",
+ absolute_position(index_position));
return -distance_to_above;
}
}
@@ -83,6 +88,8 @@
IndexMotor::ConvertDiscAngleToDiscPosition((360 * 2 + 14) * M_PI / 180);
/*static*/ const double IndexMotor::kLoaderFreeStopPosition =
kIndexStartPosition + kIndexFreeLength;
+/*static*/ const double IndexMotor::kReadyToPreload =
+ kLoaderFreeStopPosition - ConvertDiscAngleToDiscPosition(M_PI / 6.0);
/*static*/ const double IndexMotor::kReadyToLiftPosition =
kLoaderFreeStopPosition + 0.2921;
/*static*/ const double IndexMotor::kGrabberLength = 0.03175;
@@ -162,35 +169,49 @@
ConvertDiscPositionToDiscAngle(position));
}
-bool IndexMotor::MinDiscPosition(double *disc_position) {
+bool IndexMotor::MinDiscPosition(double *disc_position, Frisbee **found_disc) {
bool found_start = false;
for (unsigned int i = 0; i < frisbees_.size(); ++i) {
- const Frisbee &frisbee = frisbees_[i];
+ Frisbee &frisbee = frisbees_[i];
if (!found_start) {
if (frisbee.has_position()) {
*disc_position = frisbee.position();
+ if (found_disc) {
+ *found_disc = &frisbee;
+ }
found_start = true;
}
} else {
- *disc_position = ::std::min(frisbee.position(),
- *disc_position);
+ if (frisbee.position() <= *disc_position) {
+ *disc_position = frisbee.position();
+ if (found_disc) {
+ *found_disc = &frisbee;
+ }
+ }
}
}
return found_start;
}
-bool IndexMotor::MaxDiscPosition(double *disc_position) {
+bool IndexMotor::MaxDiscPosition(double *disc_position, Frisbee **found_disc) {
bool found_start = false;
for (unsigned int i = 0; i < frisbees_.size(); ++i) {
- const Frisbee &frisbee = frisbees_[i];
+ Frisbee &frisbee = frisbees_[i];
if (!found_start) {
if (frisbee.has_position()) {
*disc_position = frisbee.position();
+ if (found_disc) {
+ *found_disc = &frisbee;
+ }
found_start = true;
}
} else {
- *disc_position = ::std::max(frisbee.position(),
- *disc_position);
+ if (frisbee.position() > *disc_position) {
+ *disc_position = frisbee.position();
+ if (found_disc) {
+ *found_disc = &frisbee;
+ }
+ }
}
}
return found_start;
@@ -263,7 +284,7 @@
last_top_disc_posedge_count_ = position->top_disc_posedge_count;
}
- // If the cRIO is gone for 1/2 of a second, assume that it rebooted.
+ // If the cRIO is gone for over 1/2 of a second, assume that it rebooted.
if (missing_position_count_ > 50) {
last_bottom_disc_posedge_count_ = position->bottom_disc_posedge_count;
last_bottom_disc_negedge_count_ = position->bottom_disc_negedge_count;
@@ -288,8 +309,8 @@
if (!position->top_disc_detect) {
// We don't see a disc. Verify that there are no discs that we should be
// seeing.
- // Assume that discs will move slow enough that we won't one as it goes
- // by. They will either pile up above or below the sensor.
+ // Assume that discs will move slow enough that we won't miss one as it
+ // goes by. They will either pile up above or below the sensor.
double cumulative_offset = 0.0;
for (auto frisbee = frisbees_.rbegin(), rend = frisbees_.rend();
@@ -300,7 +321,10 @@
cumulative_offset += amount_moved;
}
}
+
if (position->top_disc_posedge_count != last_top_disc_posedge_count_) {
+ const double index_position = wrist_loop_->X_hat(0, 0) -
+ position->index_position + position->top_disc_posedge_position;
// TODO(aschuh): Sanity check this number...
// Requires storing when the disc was last seen with the sensor off, and
// figuring out what to do if things go south.
@@ -312,13 +336,88 @@
// 3) The top most disc is coming back down and we are seeing it.
if (wrist_loop_->X_hat(1, 0) > 50.0) {
// Moving up at a reasonable clip.
- // TODO(aschuh): Do something!
+ // 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
+ // are too many.
+ if (frisbees_.size() == 0) {
+ Frisbee new_frisbee;
+ new_frisbee.has_been_indexed_ = true;
+ new_frisbee.index_start_position_ = index_position -
+ ConvertDiscPositionToIndex(kTopDiscDetectStart -
+ kIndexStartPosition);
+ frisbees_.push_front(new_frisbee);
+ LOG(WARNING, "Added a disc to the hopper at the top sensor\n");
+ }
+
+ int above_disc_count = 0;
+ double highest_position = 0;
+ Frisbee *highest_frisbee_below_sensor = NULL;
+ for (auto frisbee = frisbees_.rbegin(), rend = frisbees_.rend();
+ frisbee != rend; ++frisbee) {
+ const double disc_position = frisbee->absolute_position(
+ index_position);
+ // It is save to use the top position for the cuttoff, since the
+ // sensor being low will result in discs being pushed off of it.
+ if (disc_position >= kTopDiscDetectStop) {
+ ++above_disc_count;
+ } else if (!highest_frisbee_below_sensor ||
+ disc_position > highest_position) {
+ highest_frisbee_below_sensor = &*frisbee;
+ highest_position = disc_position;
+ }
+ }
+ if (above_disc_count > 1) {
+ LOG(ERROR, "We have 2 discs above the top sensor.\n");
+ }
+
+ // We now have the disc. Shift all the ones below the sensor up by the
+ // computed delta.
+ const double disc_delta = IndexMotor::ConvertDiscPositionToIndex(
+ highest_position - kTopDiscDetectStart);
+ for (auto frisbee = frisbees_.rbegin(), rend = frisbees_.rend();
+ frisbee != rend; ++frisbee) {
+ const double disc_position = frisbee->absolute_position(
+ index_position);
+ if (disc_position < kTopDiscDetectStop) {
+ frisbee->OffsetDisc(disc_delta);
+ }
+ }
+ printf("Currently have %d discs, saw posedge moving up. "
+ "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) {
// Moving down at a reasonable clip.
- // Find the top disc and use that.
- // TODO(aschuh): Do something!
+ // 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.
+ double min_disc_position;
+ Frisbee *min_frisbee = NULL;
+ MinDiscPosition(&min_disc_position, &min_frisbee);
+ if (!min_frisbee) {
+ // Uh, oh, we see a disc but there isn't one...
+ LOG(ERROR, "Saw a disc up top but there isn't one in the hopper\n");
+ } else {
+ const double disc_position = min_frisbee->absolute_position(
+ index_position);
+
+ const double disc_delta_meters = disc_position - kTopDiscDetectStop;
+ const double disc_delta = IndexMotor::ConvertDiscPositionToIndex(
+ disc_delta_meters);
+ printf("Posedge going down. Moving top disc down by %f\n",
+ disc_delta_meters);
+ for (auto frisbee = frisbees_.begin(), end = frisbees_.end();
+ frisbee != end; ++frisbee) {
+ frisbee->OffsetDisc(disc_delta);
+ }
+ }
} 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!
+ //
}
}
}
@@ -405,7 +504,7 @@
// Figure out where the indexer should be to move the discs down to
// the right position.
double max_disc_position;
- if (MaxDiscPosition(&max_disc_position)) {
+ if (MaxDiscPosition(&max_disc_position, NULL)) {
printf("There is a disc down here!\n");
// TODO(aschuh): Figure out what to do if grabbing the next one
// would cause things to jam into the loader.
@@ -453,10 +552,9 @@
case Goal::SHOOT:
// Check if we have any discs to shoot or load and handle them.
double min_disc_position;
- if (MinDiscPosition(&min_disc_position)) {
- const double ready_disc_position =
- min_disc_position + ConvertDiscPositionToIndex(kIndexFreeLength) -
- ConvertDiscAngleToIndex(M_PI / 6.0);
+ if (MinDiscPosition(&min_disc_position, NULL)) {
+ const double ready_disc_position = min_disc_position +
+ ConvertDiscPositionToIndex(kReadyToPreload - kIndexStartPosition);
const double grabbed_disc_position =
min_disc_position +
diff --git a/frc971/control_loops/index/index.h b/frc971/control_loops/index/index.h
index 854b29c..7a9f6ac 100644
--- a/frc971/control_loops/index/index.h
+++ b/frc971/control_loops/index/index.h
@@ -14,6 +14,7 @@
namespace control_loops {
namespace testing {
class IndexTest_InvalidStateTest_Test;
+class IndexTest_ShiftedDiscsAreRefound_Test;
}
class IndexMotor
@@ -29,6 +30,9 @@
static const double kIndexFreeLength;
// The distance to where the disc just starts to enter the loader.
static const double kLoaderFreeStopPosition;
+ // The distance to where the next disc gets positioned while the current disc
+ // is shooting.
+ static const double kReadyToPreload;
// Distance that the grabber pulls the disc in by.
static const double kGrabberLength;
@@ -126,6 +130,14 @@
return index_start_position_;
}
+ // Returns the absolute position of the disc in meters in the hopper given
+ // that the indexer is at the provided position.
+ double absolute_position(const double index_position) const {
+ return IndexMotor::ConvertIndexToDiscPosition(
+ index_position - index_start_position_) +
+ IndexMotor::kIndexStartPosition;
+ }
+
// Shifts the disc down the indexer by the provided offset. This is to
// handle when the cRIO reboots.
void OffsetDisc(double offset) {
@@ -148,6 +160,8 @@
double index_start_position_;
};
+ const ::std::deque<Frisbee> &frisbees() const { return frisbees_; }
+
protected:
virtual void RunIteration(
const control_loops::IndexLoop::Goal *goal,
@@ -157,6 +171,7 @@
private:
friend class testing::IndexTest_InvalidStateTest_Test;
+ friend class testing::IndexTest_ShiftedDiscsAreRefound_Test;
// This class implements the CapU function correctly given all the extra
// information that we know about from the wrist motor.
@@ -180,10 +195,12 @@
};
// Sets disc_position to the minimum or maximum disc position.
+ // Sets found_disc to point to the frisbee that was found, and ignores it if
+ // found_disc is NULL.
// Returns true if there were discs, and false if there weren't.
// On false, disc_position is left unmodified.
- bool MinDiscPosition(double *disc_position);
- bool MaxDiscPosition(double *disc_position);
+ bool MinDiscPosition(double *disc_position, Frisbee **found_disc);
+ bool MaxDiscPosition(double *disc_position, Frisbee **found_disc);
// The state feedback control loop to talk to for the index.
::std::unique_ptr<IndexStateFeedbackLoop> wrist_loop_;
diff --git a/frc971/control_loops/index/index_lib_test.cc b/frc971/control_loops/index/index_lib_test.cc
index 2a657df..3677774 100644
--- a/frc971/control_loops/index/index_lib_test.cc
+++ b/frc971/control_loops/index/index_lib_test.cc
@@ -195,7 +195,6 @@
index_roller_velocity * time_left);
}
-
if (position_ >= IndexMotor::kBottomDiscDetectStop) {
HandleAfterNegedge(index_roller_velocity, elapsed_time, time_left);
}
@@ -206,7 +205,8 @@
const double disc_time =
(IndexMotor::kTopDiscDetectStart - position_) / index_roller_velocity;
top_disc_posedge_position_ = index_roller_position_ +
- index_roller_velocity * (elapsed_time + disc_time);
+ IndexMotor::ConvertDiscPositionToIndex(
+ index_roller_velocity * (elapsed_time + disc_time));
has_top_disc_posedge_position_ = true;
printf("Posedge on top sensor at %f\n", top_disc_posedge_position_);
}
@@ -672,6 +672,51 @@
}
}
+ // Loads 2 discs, and then offsets them. We then send the first disc to the
+ // grabber, and the second disc back down to the bottom. Verify that both
+ // discs get found correctly. Positive numbers shift the discs up.
+ void TestDualLostDiscs(double top_disc_offset, double bottom_disc_offset) {
+ LoadNDiscs(2);
+
+ // Move them in the indexer so they need to be re-found.
+ // The top one is moved further than the bottom one so that both edges need to
+ // be inspected.
+ index_motor_plant_.frisbees[0].OffsetIndex(
+ IndexMotor::ConvertDiscPositionToIndex(top_disc_offset));
+ index_motor_plant_.frisbees[1].OffsetIndex(
+ IndexMotor::ConvertDiscPositionToIndex(bottom_disc_offset));
+
+ // 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 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.
+ my_index_loop_.goal.MakeWithBuilder().goal_state(2).Send();
+ SimulateNCycles(300);
+
+ 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.02);
+
+ // Verify that we found the disc as accurately as the FPGA allows.
+ my_index_loop_.position.FetchLatest();
+ EXPECT_NEAR(
+ index_motor_.frisbees()[0].absolute_position(
+ my_index_loop_.position->index_position),
+ index_motor_plant_.frisbees[1].position(), 0.0001);
+ }
+
// Copy of core that works in this process only.
::aos::common::testing::GlobalCoreInstance my_core;
@@ -811,10 +856,17 @@
EXPECT_EQ(static_cast<size_t>(2), index_motor_plant_.frisbees.size());
EXPECT_NEAR(IndexMotor::kReadyToLiftPosition,
index_motor_plant_.frisbees[0].position(), 0.01);
+ printf("Top disc error is %f\n",
+ IndexMotor::kReadyToLiftPosition -
+ index_motor_plant_.frisbees[0].position());
EXPECT_NEAR(
(IndexMotor::kIndexStartPosition +
IndexMotor::ConvertDiscAngleToDiscPosition(M_PI)),
- index_motor_plant_.frisbees[1].position(), 0.10);
+ index_motor_plant_.frisbees[1].position(), 0.02);
+ printf("Bottom disc error is %f\n",
+ (IndexMotor::kIndexStartPosition +
+ IndexMotor::ConvertDiscAngleToDiscPosition(M_PI))-
+ index_motor_plant_.frisbees[1].position());
}
// Tests that the index grabs 1 disc and continues to pull it in correctly when
@@ -1057,9 +1109,29 @@
EXPECT_EQ(my_index_loop_.output->index_voltage, 0.0);
}
+// Tests that preloading 2 discs relocates the discs if they shift on the
+// indexer. Test shifting all 4 ways.
+TEST_F(IndexTest, ShiftedDiscsAreRefound) {
+ TestDualLostDiscs(0.10, 0.15);
+}
+
+TEST_F(IndexTest, ShiftedDiscsAreRefoundOtherSeperation) {
+ TestDualLostDiscs(0.15, 0.10);
+}
+
+TEST_F(IndexTest, ShiftedDownDiscsAreRefound) {
+ TestDualLostDiscs(-0.15, -0.10);
+}
+
+TEST_F(IndexTest, ShiftedDownDiscsAreRefoundOtherSeperation) {
+ TestDualLostDiscs(-0.10, -0.15);
+}
+
// TODO(aschuh): Test that we find discs corectly when moving them up.
-// Grab 2 discs, offset them down, and verify that they get shot correctly.
-// Grab 2 discs, offset them up, and verify that they get shot correctly.
+
+// TODO(aschuh): Exercise the disc coming down from above code and verify it.
+// If possible.
+
} // namespace testing
} // namespace control_loops