added the bottom sensor too
diff --git a/frc971/control_loops/index/index.cc b/frc971/control_loops/index/index.cc
index 3932ab5..4ec77ba 100644
--- a/frc971/control_loops/index/index.cc
+++ b/frc971/control_loops/index/index.cc
@@ -109,7 +109,8 @@
/*static*/ const int IndexMotor::kLiftingDelay = 2;
/*static*/ const int IndexMotor::kLiftingTimeout = 65;
/*static*/ const int IndexMotor::kShootingDelay = 10;
-/*static*/ const int IndexMotor::kLoweringDelay = 20;
+/*static*/ const int IndexMotor::kLoweringDelay = 4;
+/*static*/ const int IndexMotor::kLoweringTimeout = 120;
// TODO(aschuh): Tune these.
/*static*/ const double
@@ -900,6 +901,8 @@
if (position->loader_top) {
if (loader_countdown_ > 0) {
--loader_countdown_;
+ loader_timeout_ = 0;
+ break;
} else {
loader_state_ = LoaderState::LIFTED;
}
@@ -910,9 +913,10 @@
if (loader_timeout_ > kLiftingTimeout) {
LOG(ERROR, "Loader timeout while LIFTING %d\n", loader_timeout_);
loader_state_ = LoaderState::LIFTED;
+ } else {
+ break;
}
}
- break;
case LoaderState::LIFTED:
LOG(DEBUG, "Loader LIFTED\n");
// Disc lifted. Time to eject it out.
@@ -941,6 +945,7 @@
disc_ejected_ = true;
loader_state_ = LoaderState::LOWERING;
loader_countdown_ = kLoweringDelay;
+ loader_timeout_ = 0;
--hopper_disc_count_;
++shot_disc_count_;
case LoaderState::LOWERING:
@@ -949,11 +954,24 @@
loader_up_ = false;
disc_clamped_ = false;
disc_ejected_ = true;
- if (loader_countdown_ > 0) {
- --loader_countdown_;
- break;
+ if (position->loader_bottom) {
+ if (loader_countdown_ > 0) {
+ --loader_countdown_;
+ loader_timeout_ = 0;
+ break;
+ } else {
+ loader_state_ = LoaderState::LOWERED;
+ }
} else {
- loader_state_ = LoaderState::LOWERED;
+ // Restart the countdown if it bounces back up or something.
+ loader_countdown_ = kLoweringDelay;
+ ++loader_timeout_;
+ if (loader_timeout_ > kLoweringTimeout) {
+ LOG(ERROR, "Loader timeout while LOWERING %d\n", loader_timeout_);
+ loader_state_ = LoaderState::LOWERED;
+ } else {
+ break;
+ }
}
case LoaderState::LOWERED:
LOG(DEBUG, "Loader LOWERED\n");
diff --git a/frc971/control_loops/index/index.h b/frc971/control_loops/index/index.h
index 8abb119..7c68a7f 100644
--- a/frc971/control_loops/index/index.h
+++ b/frc971/control_loops/index/index.h
@@ -143,8 +143,12 @@
static const int kLiftingTimeout;
// Time that it takes to shoot the disc in cycles.
static const int kShootingDelay;
- // Time that it takes to lower the loader in cycles.
+ // Time that it takes to finish lowering the loader after the sensor is
+ // triggered in cycles.
static const int kLoweringDelay;
+ // Time until we give up lowering and move on in cycles.
+ // It's a long time because we really don't want to ever hit this.
+ static const int kLoweringTimeout;
// Object representing a Frisbee tracked by the indexer.
class Frisbee {
diff --git a/frc971/control_loops/index/index_motor.q b/frc971/control_loops/index/index_motor.q
index 3956b18..f8fe449 100644
--- a/frc971/control_loops/index/index_motor.q
+++ b/frc971/control_loops/index/index_motor.q
@@ -45,8 +45,10 @@
int32_t top_disc_negedge_count;
double top_disc_negedge_position;
- // Whether the hall effect for the loader being at the top is triggered.
+ // Whether the hall effects for the loader are triggered (have a magnet in
+ // front of them).
bool loader_top;
+ bool loader_bottom;
};
message Output {