Indexer now has posedge support for the top disc sensor.
diff --git a/frc971/control_loops/index/index.cc b/frc971/control_loops/index/index.cc
index bfc5be9..70c179f 100644
--- a/frc971/control_loops/index/index.cc
+++ b/frc971/control_loops/index/index.cc
@@ -18,6 +18,38 @@
namespace frc971 {
namespace control_loops {
+void IndexMotor::Frisbee::ObserveNoTopDiscSensor(
+ double index_position, double index_velocity) {
+ double disc_position = IndexMotor::ConvertIndexToDiscPosition(
+ index_position - index_start_position_);
+ if (IndexMotor::kTopDiscDetectStart <= disc_position &&
+ disc_position <= IndexMotor::kTopDiscDetectStop) {
+ // Whoops, this shouldn't be happening.
+ // Move the disc off the way that makes most sense.
+ double distance_to_above = ::std::abs(
+ disc_position - IndexMotor::kTopDiscDetectStop);
+ double distance_to_below = ::std::abs(
+ disc_position - IndexMotor::kTopDiscDetectStart);
+ if (::std::abs(index_velocity) < 100) {
+ if (distance_to_above < distance_to_below) {
+ // Move it up.
+ index_start_position_ += distance_to_above;
+ } else {
+ index_start_position_ -= distance_to_below;
+ }
+ } else {
+ if (index_velocity > 0) {
+ // 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.
+ index_start_position_ -= distance_to_below;
+ } else {
+ index_start_position_ += distance_to_above;
+ }
+ }
+ }
+}
+
IndexMotor::IndexMotor(control_loops::IndexLoop *my_index)
: aos::control_loops::ControlLoop<control_loops::IndexLoop>(my_index),
wrist_loop_(new IndexStateFeedbackLoop(MakeIndexLoop())),
@@ -30,6 +62,7 @@
disc_clamped_(false),
disc_ejected_(false),
last_bottom_disc_detect_(false),
+ last_top_disc_detect_(false),
no_prior_position_(true),
missing_position_count_(0) {
}
@@ -57,8 +90,13 @@
/*static*/ const double IndexMotor::kBottomDiscIndexDelay = 0.01;
// TODO(aschuh): Figure these out.
-/*static*/ const double IndexMotor::kTopDiscDetectStart = 18.0;
-/*static*/ const double IndexMotor::kTopDiscDetectStop = 19.0;
+/*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.
+/*static*/ const double IndexMotor::kTopDiscDetectStop =
+ IndexMotor::kTopDiscDetectStart + 10 * 0.0254;
const /*static*/ double IndexMotor::kDiscRadius = 10.875 * 0.0254 / 2;
const /*static*/ double IndexMotor::kRollerRadius = 2.0 * 0.0254 / 2;
@@ -212,6 +250,7 @@
last_bottom_disc_negedge_count_ = position->bottom_disc_negedge_count;
last_bottom_disc_negedge_wait_count_ =
position->bottom_disc_negedge_wait_count;
+ 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.
@@ -220,6 +259,7 @@
last_bottom_disc_negedge_count_ = position->bottom_disc_negedge_count;
last_bottom_disc_negedge_wait_count_ =
position->bottom_disc_negedge_wait_count;
+ last_top_disc_posedge_count_ = position->top_disc_posedge_count;
// 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);
@@ -234,8 +274,40 @@
}
const double index_position = wrist_loop_->X_hat(0, 0);
- // TODO(aschuh): Watch for top disc detect and update the frisbee
- // position.
+ if (position) {
+ 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.
+ for (auto frisbee = frisbees_.begin();
+ frisbee != frisbees_.end(); ++frisbee) {
+ frisbee->ObserveNoTopDiscSensor(
+ wrist_loop_->X_hat(0, 0), wrist_loop_->X_hat(1, 0));
+ }
+ }
+ if (position->top_disc_posedge_count != last_top_disc_posedge_count_) {
+ // 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.
+
+ // 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.
+ if (wrist_loop_->X_hat(1, 0) > 50.0) {
+ // Moving up at a reasonable clip.
+ // TODO(aschuh): Do something!
+ } 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!
+ } else {
+ // TODO(aschuh): Do something!
+ }
+ }
+ }
// Bool to track if it is safe for the goal to change yet.
bool safe_to_change_state_ = true;
@@ -553,11 +625,12 @@
if (position) {
LOG(DEBUG, "pos=%f\n", position->index_position);
last_bottom_disc_detect_ = position->bottom_disc_detect;
- last_bottom_disc_detect_ = position->bottom_disc_detect;
+ last_top_disc_detect_ = position->top_disc_detect;
last_bottom_disc_posedge_count_ = position->bottom_disc_posedge_count;
last_bottom_disc_negedge_count_ = position->bottom_disc_negedge_count;
last_bottom_disc_negedge_wait_count_ =
position->bottom_disc_negedge_wait_count;
+ last_top_disc_posedge_count_ = position->top_disc_posedge_count;
}
status->hopper_disc_count = hopper_disc_count_;
diff --git a/frc971/control_loops/index/index.h b/frc971/control_loops/index/index.h
index 870590a..713b050 100644
--- a/frc971/control_loops/index/index.h
+++ b/frc971/control_loops/index/index.h
@@ -132,6 +132,11 @@
index_start_position_ += offset;
}
+ // Potentially offsets the position with the knowledge that no discs are
+ // currently blocking the top sensor. This knowledge can be used to move
+ // this disc if it is believed to be blocking the top sensor.
+ void ObserveNoTopDiscSensor(double index_position, double index_velocity);
+
// Posedge and negedge disc times.
::aos::time::Time bottom_posedge_time_;
::aos::time::Time bottom_negedge_time_;
@@ -253,9 +258,11 @@
// Bottom disc detect from the last valid packet for detecting edges.
bool last_bottom_disc_detect_;
+ bool last_top_disc_detect_;
int32_t last_bottom_disc_posedge_count_;
int32_t last_bottom_disc_negedge_count_;
int32_t last_bottom_disc_negedge_wait_count_;
+ int32_t last_top_disc_posedge_count_;
// Frisbees are in order such that the newest frisbee is on the front.
::std::deque<Frisbee> frisbees_;
diff --git a/frc971/control_loops/index/index_lib_test.cc b/frc971/control_loops/index/index_lib_test.cc
index c8e30e4..2a657df 100644
--- a/frc971/control_loops/index/index_lib_test.cc
+++ b/frc971/control_loops/index/index_lib_test.cc
@@ -32,7 +32,9 @@
has_bottom_disc_negedge_wait_position_(false),
bottom_disc_negedge_wait_position_(0.0),
after_negedge_time_left_(IndexMotor::kBottomDiscIndexDelay),
- counted_negedge_wait_(false) {
+ counted_negedge_wait_(false),
+ has_top_disc_posedge_position_(false),
+ top_disc_posedge_position_(0.0) {
}
// Returns true if the frisbee is controlled by the transfer roller.
@@ -90,16 +92,26 @@
bool bottom_disc_detect() const { return bottom_disc_detect(position_); }
// Returns true if the disc is triggering the top disc detect sensor.
- bool top_disc_detect() const {
- return (position_ >= IndexMotor::kTopDiscDetectStart &&
- position_ <= IndexMotor::kTopDiscDetectStop);
+ bool top_disc_detect(double position) const {
+ return (position >= IndexMotor::kTopDiscDetectStart &&
+ position <= IndexMotor::kTopDiscDetectStop);
+ }
+ bool top_disc_detect() const { return top_disc_detect(position_); }
+
+ // Returns true if the bottom disc sensor will negedge after the disc moves
+ // by dx.
+ bool will_negedge_bottom_disc_detect(double disc_dx) {
+ if (bottom_disc_detect()) {
+ return !bottom_disc_detect(position_ + disc_dx);
+ }
+ return false;
}
// Returns true if the bottom disc sensor will negedge after the disc moves
// by dx.
- bool will_negedge_bottom_disc_detect(double transfer_dx) {
- if (bottom_disc_detect()) {
- return bottom_disc_detect(position_ + transfer_dx);
+ bool will_posedge_top_disc_detect(double disc_dx) {
+ if (!top_disc_detect()) {
+ return top_disc_detect(position_ + disc_dx);
}
return false;
}
@@ -183,10 +195,22 @@
index_roller_velocity * time_left);
}
+
if (position_ >= IndexMotor::kBottomDiscDetectStop) {
HandleAfterNegedge(index_roller_velocity, elapsed_time, time_left);
}
+ if (will_posedge_top_disc_detect(index_dx)) {
+ // Wohoo! Find the edge.
+ // Assume constant velocity and compute the position.
+ const double disc_time =
+ (IndexMotor::kTopDiscDetectStart - position_) / index_roller_velocity;
+ top_disc_posedge_position_ = index_roller_position_ +
+ 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_);
+ }
+
if (shrunk_time) {
position_ = IndexMotor::kGrabberStartPosition;
} else {
@@ -310,6 +334,17 @@
return bottom_disc_negedge_wait_position_;
}
+ // Returns the last position where a posedge was seen.
+ double top_disc_posedge_position() { return top_disc_posedge_position_; }
+
+ // True if the top disc has seen a posedge.
+ // Reading this flag clears it.
+ bool has_top_disc_posedge_position() {
+ bool prev = has_top_disc_posedge_position_;
+ has_top_disc_posedge_position_ = false;
+ return prev;
+ }
+
// Simulates the index roller moving without the disc moving.
void OffsetIndex(double offset) {
index_roller_position_ += offset;
@@ -333,6 +368,11 @@
// Bool for the user to record if they have counted the negedge from this
// disc.
bool counted_negedge_wait_;
+ // True if the top disc sensor posedge has occured and
+ // hasn't been counted yet.
+ bool has_top_disc_posedge_position_;
+ // The position at which the posedge occured.
+ double top_disc_posedge_position_;
};
@@ -349,6 +389,8 @@
bottom_disc_negedge_count_(0),
bottom_disc_negedge_wait_count_(0),
bottom_disc_negedge_wait_position_(0),
+ top_disc_posedge_count_(0),
+ top_disc_posedge_position_(0.0),
my_index_loop_(".frc971.control_loops.index",
0x1a7b7094, ".frc971.control_loops.index.goal",
".frc971.control_loops.index.position",
@@ -415,6 +457,10 @@
frisbee->set_counted_negedge_wait(true);
}
}
+ if (frisbee->has_top_disc_posedge_position()) {
+ ++top_disc_posedge_count_;
+ top_disc_posedge_position_ = frisbee->top_disc_posedge_position();
+ }
}
// Make sure nobody is too close to anybody else.
@@ -459,13 +505,18 @@
position->bottom_disc_negedge_wait_count = bottom_disc_negedge_wait_count_;
position->bottom_disc_negedge_wait_position =
bottom_disc_negedge_wait_position_;
- printf("bdd: %x tdd: %x posedge %d negedge %d delaycount %d delaypos %f\n",
+ position->top_disc_posedge_count = top_disc_posedge_count_;
+ position->top_disc_posedge_position = top_disc_posedge_position_;
+ printf("bdd: %x tdd: %x posedge %d negedge %d "
+ "delaycount %d delaypos %f topcount %d toppos %f\n",
position->bottom_disc_detect,
position->top_disc_detect,
position->bottom_disc_posedge_count,
position->bottom_disc_negedge_count,
position->bottom_disc_negedge_wait_count,
- position->bottom_disc_negedge_wait_position);
+ position->bottom_disc_negedge_wait_position,
+ position->top_disc_posedge_count,
+ position->top_disc_posedge_position);
position.Send();
}
@@ -508,6 +559,10 @@
int32_t bottom_disc_negedge_wait_count_;
int32_t bottom_disc_negedge_wait_position_;
+ // Posedge count and position for the upper disc sensor.
+ int32_t top_disc_posedge_count_;
+ double top_disc_posedge_position_;
+
// Returns the absolute angle of the index.
double index_roller_position() const {
return index_plant_->Y(0, 0);
@@ -1002,6 +1057,10 @@
EXPECT_EQ(my_index_loop_.output->index_voltage, 0.0);
}
+// 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.
+
} // namespace testing
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/index/index_motor.q b/frc971/control_loops/index/index_motor.q
index e3dd1c2..a001470 100644
--- a/frc971/control_loops/index/index_motor.q
+++ b/frc971/control_loops/index/index_motor.q
@@ -32,7 +32,7 @@
// The most recent index position at the posedge of the top disc detect
// and a count of how many edges have been seen.
int32_t top_disc_posedge_count;
- double top_disc_posedge_position
+ double top_disc_posedge_position;
};
message Output {