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:
diff --git a/frc971/control_loops/index/index.h b/frc971/control_loops/index/index.h
index 94db771..8abb119 100644
--- a/frc971/control_loops/index/index.h
+++ b/frc971/control_loops/index/index.h
@@ -139,6 +139,8 @@
// Time that it takes to finish lifting the loader after the sensor is
// triggered in cycles.
static const int kLiftingDelay;
+ // Time until we give up lifting and move on in cycles.
+ 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.
@@ -310,7 +312,7 @@
// Loader goal, state, and counter.
LoaderGoal loader_goal_;
LoaderState loader_state_;
- int loader_countdown_;
+ int loader_countdown_, loader_timeout_;
// Current state of the pistons.
bool loader_up_;