fixed some small(ish) index bugs
diff --git a/frc971/control_loops/index/index.cc b/frc971/control_loops/index/index.cc
index 8a8d540..a8ef5bf 100644
--- a/frc971/control_loops/index/index.cc
+++ b/frc971/control_loops/index/index.cc
@@ -266,6 +266,7 @@
// 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_) {
+ LOG(INFO, "no prior position; resetting\n");
wrist_loop_->R << wrist_loop_->Y(0, 0), 0.0;
wrist_loop_->X_hat(0, 0) = wrist_loop_->Y(0, 0);
no_prior_position_ = false;
@@ -282,6 +283,7 @@
// If the cRIO is gone for over 1/2 of a second, assume that it rebooted.
if (missing_position_count_ > 50) {
+ LOG(INFO, "assuming cRIO rebooted\n");
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_ =
@@ -442,11 +444,17 @@
frisbee->OffsetDisc(disc_delta);
}
}
- LOG(INFO, "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)));
+ if (highest_frisbee_below_sensor) {
+ LOG(INFO, "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 {
+ LOG(INFO, "Currently have %d discs, saw posedge moving up. "
+ "Moving down by %f\n", frisbees_.size(),
+ ConvertIndexToDiscPosition(disc_delta));
+ }
} 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.
diff --git a/frc971/control_loops/index/index.h b/frc971/control_loops/index/index.h
index f488978..62a582c 100644
--- a/frc971/control_loops/index/index.h
+++ b/frc971/control_loops/index/index.h
@@ -1,5 +1,5 @@
-#ifndef FRC971_CONTROL_LOOPS_WRIST_H_
-#define FRC971_CONTROL_LOOPS_WRIST_H_
+#ifndef FRC971_CONTROL_LOOPS_INDEX_INDEX_H_
+#define FRC971_CONTROL_LOOPS_INDEX_INDEX_H_
#include <deque>
#include "aos/common/libstdc++/memory"
@@ -343,4 +343,4 @@
} // namespace control_loops
} // namespace frc971
-#endif // FRC971_CONTROL_LOOPS_WRIST_H_
+#endif // FRC971_CONTROL_LOOPS_INDEX_INDEX_H_