fixed a bug where it wouldn't rezero when the crio rebooted
diff --git a/frc971/control_loops/zeroed_joint.h b/frc971/control_loops/zeroed_joint.h
index 7d549d3..a756d35 100644
--- a/frc971/control_loops/zeroed_joint.h
+++ b/frc971/control_loops/zeroed_joint.h
@@ -133,6 +133,7 @@
ZeroedJoint(StateFeedbackLoop<3, 1, 1> loop)
: loop_(new ZeroedStateFeedbackLoop<kNumZeroSensors>(loop, this)),
+ last_good_time_(0, 0),
state_(UNINITIALIZED),
error_count_(0),
zero_offset_(0.0),
@@ -188,11 +189,15 @@
friend class testing::WristTest_NoWindupPositive_Test;
friend class testing::WristTest_NoWindupNegative_Test;
+ static const ::aos::time::Time kRezeroTime;
+
// The state feedback control loop to talk to.
::std::unique_ptr<ZeroedStateFeedbackLoop<kNumZeroSensors>> loop_;
ConfigurationData config_data_;
+ ::aos::time::Time last_good_time_;
+
// Returns the index of the first active sensor, or -1 if none are active.
int ActiveSensorIndex(
const ZeroedJoint<kNumZeroSensors>::PositionData *position) {
@@ -255,6 +260,10 @@
};
template <int kNumZeroSensors>
+const ::aos::time::Time ZeroedJoint<kNumZeroSensors>::kRezeroTime =
+ ::aos::time::Time::InSeconds(10);
+
+template <int kNumZeroSensors>
/*static*/ const double ZeroedJoint<kNumZeroSensors>::dt = 0.01;
// Updates the zeroed joint controller and state machine.
@@ -273,10 +282,14 @@
if (error_count_ >= 4) {
output_enabled = false;
LOG(WARNING, "err_count is %d so disabling\n", error_count_);
- } else if (error_count_ >= 200) {
- LOG(WARNING, "err_count is %d so forcing a re-zero\n", error_count_);
+ } else if ((::aos::time::Time::Now() - last_good_time_) > kRezeroTime) {
+ LOG(WARNING, "err_count is %d so forcing a re-zero (or 1st time)\n",
+ error_count_);
state_ = UNINITIALIZED;
}
+ if (position) {
+ last_good_time_ = ::aos::time::Time::Now();
+ }
// Compute the absolute position of the wrist.
double absolute_position;