added a lifting timeout
Sometimes, a frisbee doesn't get all of the way into the loader which
means that it never gets to the top. The way that this problem used to
get fixed was the loader would come down, release, it'd try to load the
next frisbee, and it would push it all the way in, leaving a frisbee in
the loader at the end. With the sensor, it just gets stuck trying to
lift, so I added a timeout to fix that.
diff --git a/frc971/control_loops/index/index.cc b/frc971/control_loops/index/index.cc
index 1d2c706..3932ab5 100644
--- a/frc971/control_loops/index/index.cc
+++ b/frc971/control_loops/index/index.cc
@@ -107,6 +107,7 @@
/*static*/ const int IndexMotor::kGrabbingDelay = 5;
/*static*/ const int IndexMotor::kLiftingDelay = 2;
+/*static*/ const int IndexMotor::kLiftingTimeout = 65;
/*static*/ const int IndexMotor::kShootingDelay = 10;
/*static*/ const int IndexMotor::kLoweringDelay = 20;
@@ -871,6 +872,7 @@
if (shooter.status->average_velocity > 130 && shooter.status->ready) {
loader_state_ = LoaderState::LIFTING;
loader_countdown_ = kLiftingDelay;
+ loader_timeout_ = 0;
LOG(INFO, "Told to SHOOT_AND_RESET, moving on\n");
} else {
LOG(WARNING, "Told to SHOOT_AND_RESET, shooter too slow at %f\n",
@@ -881,6 +883,7 @@
LOG(ERROR, "Told to SHOOT_AND_RESET, no shooter data, moving on.\n");
loader_state_ = LoaderState::LIFTING;
loader_countdown_ = kLiftingDelay;
+ loader_timeout_ = 0;
}
} else if (loader_goal_ == LoaderGoal::READY) {
LOG(ERROR, "Can't go to ready when we have something grabbed.\n");
@@ -903,6 +906,11 @@
} else {
// Restart the countdown if it bounces back down or whatever.
loader_countdown_ = kLiftingDelay;
+ ++loader_timeout_;
+ if (loader_timeout_ > kLiftingTimeout) {
+ LOG(ERROR, "Loader timeout while LIFTING %d\n", loader_timeout_);
+ loader_state_ = LoaderState::LIFTED;
+ }
}
break;
case LoaderState::LIFTED: